diff --git a/.github/workflows/autopush.yaml b/.github/workflows/autopush.yaml index b5f1e51f1..3e4a368eb 100644 --- a/.github/workflows/autopush.yaml +++ b/.github/workflows/autopush.yaml @@ -25,7 +25,8 @@ env: jobs: pre-commit: name: Run pre-commit - runs-on: ubuntu-latest + runs-on: + group: mala-lab-pre-commit if: github.event.sender.login == 'cbrxyz' steps: - name: Check out code from GitHub diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 193015444..ae0eec32e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -37,7 +37,8 @@ jobs: name: Run tests and build docs needs: avoid-duplicate-ci if: needs.avoid-duplicate-ci.outputs.should_skip != 'true' - runs-on: self-hosted + runs-on: + group: mala-lab-main steps: - name: Configure catkin workspace folder structure run: | @@ -111,7 +112,8 @@ jobs: deploy-docs: name: Deploy docs from master - runs-on: self-hosted + runs-on: + group: mala-lab-main needs: super-ci # https://github.com/actions/runner/issues/491#issuecomment-850884422 if: | diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 000000000..b708b620f --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,61 @@ +--- +name: Run pre-commit + +# yamllint disable-line rule:truthy +on: [push, workflow_dispatch] + +# Cancels this run if a new one referring to the same object and same workflow +# is requested +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +env: + # The version of caching we are using. This can be upgraded if we + # significantly change CI to the point where old caches become irrelevant. + CACHE_VERSION: 0 + # Default Python version. Noetic defaults to 3.8. + DEFAULT_PYTHON: 3.8 + # Location of the pre-commit cache. This is set by pre-commit, not us! + PRE_COMMIT_CACHE: ~/.cache/pre-commit + +jobs: + pre-commit: + name: Run pre-commit + runs-on: + group: mala-lab-pre-commit + steps: + - name: Check out code from GitHub + uses: actions/checkout@v4 + with: + submodules: recursive + - name: Set up Python ${{ env.DEFAULT_PYTHON }} + uses: actions/setup-python@v5 + with: + python-version: ${{ env.DEFAULT_PYTHON }} + cache: "pip" + - name: Install pre-commit hooks + run: | + python --version + pip install "$(cat requirements.txt | grep pre-commit)" + - name: Generate pre-commit cache key + id: pre-commit_cache_key + run: > + echo "::set-output + name=key::${{ env.CACHE_VERSION }}-${{ env.DEFAULT_PYTHON }}-${{ + hashFiles('.pre-commit-config.yaml') }}" + - name: Restore base pre-commit environment + id: cache-pre-commmit + uses: actions/cache@v4 + with: + path: ${{ env.PRE_COMMIT_CACHE }} + key: > + ${{ runner.os + }}-pre-commit-${{ steps.pre-commit_cache_key.outputs.key }} + - name: Install pre-commit dependencies if no cache + if: steps.cache-precommit.outputs.cache-hit != 'true' + run: | + pre-commit install-hooks + - name: Run pre-commit + run: | + pre-commit run --all-files --show-diff-on-failure diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a17237696..566493875 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -3,6 +3,7 @@ exclude: ^deprecated/ default_language_version: python: python3.8 + node: lts ci: autofix_prs: false @@ -19,7 +20,7 @@ repos: hooks: - id: black - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.1 hooks: - id: clang-format - repo: https://github.com/PyCQA/autoflake @@ -40,7 +41,7 @@ repos: exclude: ^docker|deprecated|NaviGator/simulation/VRX - repo: https://github.com/astral-sh/ruff-pre-commit # Ruff version. - rev: 'v0.6.3' + rev: 'v0.6.9' hooks: - id: ruff args: [--fix, --exit-non-zero-on-fix] @@ -50,7 +51,7 @@ repos: hooks: - id: codespell args: - - --ignore-words-list=fpr,ser,wan,te,wan,ba,ned,ans,hist,nd,wronly,Voight,assertIn + - --ignore-words-list=fpr,ser,wan,te,wan,ba,ned,ans,hist,nd,wronly,Voight,assertIn,flor - --skip="./.*,*.csv,*.json" - --quiet-level=2 exclude_types: [csv, json] @@ -74,7 +75,7 @@ repos: - id: prettier-package-xml - id: sort-package-xml - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-case-conflict diff --git a/NaviGator/hardware_drivers/navigator_ball_launcher/navigator_ball_launcher/driver.py b/NaviGator/hardware_drivers/navigator_ball_launcher/navigator_ball_launcher/driver.py index f78f22784..a729fd271 100644 --- a/NaviGator/hardware_drivers/navigator_ball_launcher/navigator_ball_launcher/driver.py +++ b/NaviGator/hardware_drivers/navigator_ball_launcher/navigator_ball_launcher/driver.py @@ -33,7 +33,7 @@ def __init__(self, port: str): self.heard_nack = False self.drops = 0 self.temp_timer = None - print("done init") + rospy.loginfo("Ball launcher device initialized") def get_drop_count(self) -> int: return self.drops @@ -51,10 +51,10 @@ def _check_for_valid_response(self, event: str): elif not self.heard_ack: rospy.logerr(f"Failed to {event} (no response from board)") - def _check_for_dropped_ball(self): + def _check_for_dropped_ball(self, _): self._check_for_valid_response("drop ball") - def _check_for_spun(self): + def _check_for_spun(self, _): self._check_for_valid_response("set spin") def drop_ball(self, _: EmptyRequest): @@ -84,7 +84,6 @@ def spin(self, request: SetBoolRequest): return {} def on_packet_received(self, packet: AckPacket | NackPacket) -> None: - print("inc packet", packet) if isinstance(packet, AckPacket): self.heard_ack = True elif isinstance(packet, NackPacket): diff --git a/NaviGator/hardware_drivers/navigator_ball_launcher/nodes/driver.py b/NaviGator/hardware_drivers/navigator_ball_launcher/nodes/driver.py index aed93d345..2c95f899e 100755 --- a/NaviGator/hardware_drivers/navigator_ball_launcher/nodes/driver.py +++ b/NaviGator/hardware_drivers/navigator_ball_launcher/nodes/driver.py @@ -4,6 +4,6 @@ if __name__ == "__main__": rospy.init_node("ball_launcher") - port = "/dev/serial/by-id/usb-Raspberry_Pi_Pico_E6635C08CB65BC36-if00" + port = str(rospy.get_param("~port")) device = BallLauncherDevice(port) rospy.spin() diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt b/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt new file mode 100644 index 000000000..7672c7baf --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.0.2) +project(navigator_drone_comm) + +find_package(catkin REQUIRED COMPONENTS + rospy +) + +catkin_python_setup() +catkin_package() + +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/__init__.py b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/__init__.py new file mode 100644 index 000000000..9c356af2d --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/__init__.py @@ -0,0 +1,10 @@ +from .packets import ( + Color, + EStopPacket, + GPSDronePacket, + HeartbeatReceivePacket, + HeartbeatSetPacket, + StartPacket, + StopPacket, + TargetPacket, +) diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/packets.py b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/packets.py new file mode 100644 index 000000000..a60003dd3 --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/packets.py @@ -0,0 +1,91 @@ +from dataclasses import dataclass +from enum import Enum + +from electrical_protocol import Packet + + +@dataclass +class HeartbeatReceivePacket( + Packet, + class_id=0x20, + subclass_id=0x00, + payload_format="", +): + """ + Heartbeat packet sent from the drone. + """ + + +@dataclass +class HeartbeatSetPacket(Packet, class_id=0x20, subclass_id=0x01, payload_format=""): + """ + Heartbeat packet sent from the boat. + """ + + +@dataclass +class GPSDronePacket(Packet, class_id=0x20, subclass_id=0x02, payload_format=" + + navigator_drone_comm + 0.0.0 + The navigator_drone_comm package + + Alex Johnson + + TODO + + catkin + rospy + rospy + rospy + diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/setup.py b/NaviGator/hardware_drivers/navigator_drone_comm/setup.py new file mode 100644 index 000000000..ec0c3a7a2 --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/setup.py @@ -0,0 +1,11 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup + +# Fetch values from package.xml +setup_args = generate_distutils_setup( + packages=["navigator_drone_comm"], +) + +setup(**setup_args) diff --git a/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_a.yaml b/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_a.yaml new file mode 100644 index 000000000..4d6db140e --- /dev/null +++ b/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_a.yaml @@ -0,0 +1,42 @@ +--- +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + frame: ecef + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + frame: ecef + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + state: true + type: '' + x1: 744450.0 + x2: 744336.0 + x3: 744342.0 + x4: 744456.0 + y1: -5618775.0 + y2: -5618790.0 + y3: -5618835.0 + y4: -5618820.0 + z1: 2915236.0 + z2: 2915236.0 + z3: 2915150.0 + z4: 2915150.0 + state: [] + x1: 744450.0 + x2: 744336.0 + x3: 744342.0 + x4: 744456.0 + y1: -5618775.0 + y2: -5618790.0 + y3: -5618835.0 + y4: -5618820.0 + z1: 2915236.0 + z2: 2915236.0 + z3: 2915150.0 + z4: 2915150.0 +state: [] diff --git a/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_b.yaml b/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_b.yaml new file mode 100644 index 000000000..990542f7d --- /dev/null +++ b/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_b.yaml @@ -0,0 +1,42 @@ +--- +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + frame: ecef + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + frame: ecef + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + state: true + type: '' + x1: 744456.0 + x2: 744342.0 + x3: 744348.0 + x4: 744463.0 + y1: -5618820.0 + y2: -5618835.0 + y3: -5618885.0 + y4: -5618870.0 + z1: 2915149.0 + z2: 2915149.0 + z3: 2915052.0 + z4: 2915052.0 + state: [] + x1: 744456.0 + x2: 744342.0 + x3: 744348.0 + x4: 744463.0 + y1: -5618820.0 + y2: -5618835.0 + y3: -5618885.0 + y4: -5618870.0 + z1: 2915149.0 + z2: 2915149.0 + z3: 2915052.0 + z4: 2915052.0 +state: [] diff --git a/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_c.yaml b/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_c.yaml new file mode 100644 index 000000000..defd2e816 --- /dev/null +++ b/NaviGator/mission_control/navigator_launch/config/bounds_sarasota_c.yaml @@ -0,0 +1,42 @@ +--- +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + frame: ecef + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + frame: ecef + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + state: true + type: '' + x1: 744463.0 + x2: 744349.0 + x3: 744354.0 + x4: 744468.0 + y1: -5618871.0 + y2: -5618886.0 + y3: -5618926.0 + y4: -5618911.0 + z1: 2915050.0 + z2: 2915050.0 + z3: 2914972.0 + z4: 2914972.0 + state: [] + x1: 744463.0 + x2: 744349.0 + x3: 744354.0 + x4: 744468.0 + y1: -5618871.0 + y2: -5618886.0 + y3: -5618926.0 + y4: -5618911.0 + z1: 2915050.0 + z2: 2915050.0 + z3: 2914972.0 + z4: 2914972.0 +state: [] diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar.yaml index 33e89d4c0..2eaae4de5 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar.yaml @@ -21,5 +21,9 @@ ogrid_width_meters: 500 ogrid_resolution_meters_per_cell: 0.3 ogrid_inflation_meters: 0.8 +# Intensity filter +intensity_filter_min_intensity: 10 +intensity_filter_max_intensity: 100 + # yamllint disable-line rule:line-length -classifications: ["white_cylinder", "black_cylinder", "red_cylinder", "green_cylinder","blue_manatee_buoy", "green_iguana_buoy", "red_python_buoy", "UNKNOWN"] +classifications: ["white_cylinder", "black_cylinder", "red_cylinder", "green_cylinder", "blue_manatee_buoy", "green_iguana_buoy", "red_python_buoy", "UNKNOWN"] diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml index 26a7bbc0f..582682cc9 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml @@ -21,6 +21,10 @@ ogrid_width_meters: 500 ogrid_resolution_meters_per_cell: 0.3 ogrid_inflation_meters: 0.8 +# Intensity filter +intensity_filter_min_intensity: 0 +intensity_filter_max_intensity: 1 + # yamllint disable-line rule:line-length # classifications: ["buoy", "dock", "stc_platform", "red_totem", "green_totem", "blue_totem", "yellow_totem", "black_totem", "surmark46104", "surmark950400", "surmark950410", "UNKNOWN"] # yamllint disable-line rule:line-length diff --git a/NaviGator/mission_control/navigator_launch/launch/robotx_comms.launch b/NaviGator/mission_control/navigator_launch/launch/robotx_comms.launch index 48779236c..6495fd37c 100644 --- a/NaviGator/mission_control/navigator_launch/launch/robotx_comms.launch +++ b/NaviGator/mission_control/navigator_launch/launch/robotx_comms.launch @@ -6,7 +6,7 @@ - + diff --git a/NaviGator/mission_control/navigator_launch/launch/shore.launch b/NaviGator/mission_control/navigator_launch/launch/shore.launch index 5454bfa18..a9605b17f 100644 --- a/NaviGator/mission_control/navigator_launch/launch/shore.launch +++ b/NaviGator/mission_control/navigator_launch/launch/shore.launch @@ -1,9 +1,14 @@ - - + + - + + + + - - + + + + diff --git a/NaviGator/mission_control/navigator_launch/launch/shore/xbox_controller.launch b/NaviGator/mission_control/navigator_launch/launch/shore/xbox_controller.launch index 66239600b..4f613d1fe 100644 --- a/NaviGator/mission_control/navigator_launch/launch/shore/xbox_controller.launch +++ b/NaviGator/mission_control/navigator_launch/launch/shore/xbox_controller.launch @@ -1,9 +1,10 @@ + + - - + diff --git a/NaviGator/mission_control/navigator_launch/launch/simulation.launch b/NaviGator/mission_control/navigator_launch/launch/simulation.launch index 6f0543cf0..d830beea8 100644 --- a/NaviGator/mission_control/navigator_launch/launch/simulation.launch +++ b/NaviGator/mission_control/navigator_launch/launch/simulation.launch @@ -7,6 +7,9 @@ + + @@ -27,7 +30,7 @@ - + diff --git a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch index d81cc0026..64d2e88bf 100644 --- a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch +++ b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch @@ -2,8 +2,8 @@ - kp: [1500.0, 1500.0, 5600.0] - kd: [300.0, 200.0, 3000.0] + kp: [1000.0, 700, 5600.0] + kd: [200.0, 400, 3000.0] ki: [0.2, 0.2, 0.2] kg: [5.0, 5.0, 5.0, 5.0, 5.0] mass: 251.19 diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py b/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py index fe91727ce..10cc3264f 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py @@ -1,9 +1,7 @@ import mil_missions_core -# RobotX 2024 Missions -from .wildlife_2024 import Wildlife2024 - from . import pose_editor +from .autonomous_2024 import Autonomous2024 from .back_and_forth import BackAndForth from .circle import Circle from .circle_tower import CircleTower @@ -27,6 +25,7 @@ from .killed import Killed from .move import Move from .navigation import Navigation +from .navigation_gatefinder import NavigationGatefinder from .navigator import NaviGatorMission from .obstacle_avoid import ObstacleAvoid from .pinger import PingerMission @@ -43,6 +42,9 @@ from .teleop import Teleop from .wildlife import Wildlife +# RobotX 2024 Missions +from .wildlife_2024 import Wildlife2024 + # Currently breaks mission server, TODO: fix or delete # from .track_target import TrackTarget diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py new file mode 100644 index 000000000..d1ff9007d --- /dev/null +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py @@ -0,0 +1,44 @@ +from __future__ import annotations + +import asyncio + +import rospy + +from .docking import Docking +from .entrance_gate2 import EntranceGate2 +from .navigation import Navigation +from .navigator import NaviGatorMission +from .scan_the_code import ScanTheCodeMission +from .wildlife import Wildlife + + +class Autonomous2024(NaviGatorMission): + + # timeout (in secs) + TIMEOUT = 180 + + async def run_mission(self, mission_cls: type[NaviGatorMission], name: str): + rospy.loginfo(f"[autonomous] beginning {name}...") + try: + await asyncio.wait_for(mission_cls().run(""), self.TIMEOUT) + except asyncio.TimeoutError: + rospy.logwarn(f"[autonomous] ran out of time on {name}!") + + async def run(self, args: str): + # Step 1: Entrance and exit gates + await self.run_mission(EntranceGate2, "entrance gate") + + # Step 2: Scan the Code + await self.run_mission(ScanTheCodeMission, "scan the code") + + # Step 3: Wildlife Mission + await self.run_mission(Wildlife, "wildlife") + + # Step 4: Navigation Mission + await self.run_mission(Navigation, "navigation") + + # Step 5: Dock Mission + await self.run_mission(Docking, "docking") + + # Step 6: UAV Mission + pass diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation_gatefinder.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation_gatefinder.py new file mode 100644 index 000000000..aea2603c2 --- /dev/null +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation_gatefinder.py @@ -0,0 +1,394 @@ +from __future__ import annotations + +import math +from dataclasses import dataclass +from enum import Enum, auto +from typing import ClassVar + +import axros +import numpy as np +import rospy +from geometry_msgs.msg import Pose +from mil_msgs.msg import PerceptionObject +from std_srvs.srv import SetBoolRequest +from tf.transformations import quaternion_from_euler + +from .navigator import NaviGatorMission + +FAILURE_THRESHOLD = 2000 + + +class BuoyColor(Enum): + RED = auto() + GREEN = auto() + WHITE = auto() + UNKNOWN = auto() + + @classmethod + def from_label(cls, label: str): + if "red" in label.lower(): + return cls.RED + if "green" in label.lower(): + return cls.GREEN + if "white" in label.lower(): + return cls.WHITE + return cls.UNKNOWN + + +@dataclass +class Buoy: + color: BuoyColor + pose: Pose + id: int + + @classmethod + def from_perception_object(cls, perception_object: PerceptionObject): + return cls( + color=BuoyColor.from_label(perception_object.labeled_classification), + pose=perception_object.pose, + id=perception_object.id, + ) + + def xyz(self): + return np.array( + [self.pose.position.x, self.pose.position.y, self.pose.position.z], + ) + + def distance(self, pose: Pose) -> float: + return math.sqrt( + ((pose.position.x - self.pose.position.x) ** 2) + + ((pose.position.y - self.pose.position.y) ** 2), + ) + + +@dataclass +class Gate: + left_buoy: Buoy + right_buoy: Buoy + traversed: bool = False + required_left_color: ClassVar[BuoyColor] + required_right_color: ClassVar[BuoyColor] + + @property + def mid_pose(self): + # Calculate the midpoint of the found buoys + mid_x = (self.left_buoy.pose.position.x + self.right_buoy.pose.position.x) / 2 + mid_y = (self.left_buoy.pose.position.y + self.right_buoy.pose.position.y) / 2 + slope = (self.left_buoy.pose.position.x - self.right_buoy.pose.position.x) / ( + self.left_buoy.pose.position.y - self.right_buoy.pose.position.y + ) + angle = math.atan(slope) + math.pi / 2 + pose = Pose() + pose.position.x = mid_x + pose.position.y = mid_y + quat = quaternion_from_euler(0, 0, angle) + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + return (mid_x, mid_y, 0), quat + + @classmethod + def other_color(cls, color_one: BuoyColor) -> BuoyColor: + if color_one == cls.required_left_color: + return cls.required_right_color + if color_one == cls.required_right_color: + return cls.required_left_color + raise RuntimeError(f"Color {color_one} cannot form this gate.") + + +@dataclass +class MiddleGate(Gate): + required_left_color = BuoyColor.RED + required_right_color = BuoyColor.GREEN + + +@dataclass +class StartGate(Gate): + required_left_color = BuoyColor.RED + required_right_color = BuoyColor.GREEN + + +@dataclass +class EndGate(Gate): + required_left_color = BuoyColor.RED + required_right_color = BuoyColor.GREEN + + +class GateClassifier: + def __init__(self, mission: NavigationGatefinder): + self.mission = mission + self.trajectory = [] + self.ids_invesitigated = set() + self.failures_to_move = 0 + self.midpoint_lengths = [] + + def _nearby_buoys(self, buoy: Buoy) -> list[Buoy]: + """ + Returns a list of nearby buoys, sorted by distance. + """ + nearby = sorted(self.ordered_buoys, key=lambda b: b.distance(buoy.pose)) + return [n for n in nearby if n.distance(buoy.pose) < 20] + + def mark_completed(self, gate: Gate) -> None: + pass + + async def get_next_gate(self, boat_pose) -> Gate | int: + # Get all database objects + objects: list[PerceptionObject] = ( + await self.mission.database_query(name="all") + ).objects + + # get nearest red and nearest green buoys + nearest_red: None | Buoy = None + nearest_green: None | Buoy = None + for db_object in objects: + # Could we do this more efficiently with a different database request? + DB_BUOY = Buoy.from_perception_object(db_object) + if ( + db_object.labeled_classification == "red_cylinder" + and DB_BUOY.id not in self.ids_invesitigated + ): + if nearest_red: + distance = np.linalg.norm(DB_BUOY.xyz() - boat_pose) + nearest_red_distance = np.linalg.norm(nearest_red.xyz() - boat_pose) + nearest_red = ( + DB_BUOY if distance < nearest_red_distance else nearest_red + ) + else: + nearest_red = DB_BUOY + + if ( + db_object.labeled_classification == "green_cylinder" + and DB_BUOY.id not in self.ids_invesitigated + ): + if nearest_green: + distance = np.linalg.norm(DB_BUOY.xyz() - boat_pose) + nearest_green_distance = np.linalg.norm( + nearest_green.xyz() - boat_pose, + ) + nearest_green = ( + DB_BUOY if distance < nearest_green_distance else nearest_green + ) + else: + nearest_green = DB_BUOY + + # calculate midpoint between both points + # if ONLY green then go to a point 5 meters from the right of the green buoy + # if ONLY red then go to a point 5 meters from the left of the red buoy + + # mark those buoys as investigated + mid_point = None + if nearest_green and nearest_red: + self.ids_invesitigated.add(nearest_red.id) + self.ids_invesitigated.add(nearest_green.id) + mid_x = (nearest_green.pose.position.x + nearest_red.pose.position.x) / 2 + mid_y = (nearest_green.pose.position.y + nearest_red.pose.position.y) / 2 + mid_point = (mid_x, mid_y, 0) + self.trajectory.append(mid_point) + + # missing red + elif nearest_green: + rospy.logerr( + f"Could not find red for green({nearest_green.id}), here is the boats pos:\n{boat_pose}", + ) + # Get nearest red to the green + objects: list[PerceptionObject] = ( + await self.mission.database_query(name="all") + ).objects + nearest_red = None + for db_object in objects: + # Could we do this more efficiently with a different database request? + rospy.logerr(db_object.labeled_classification) + DB_BUOY = Buoy.from_perception_object(db_object) + if ( + db_object.labeled_classification == "red_cylinder" + and DB_BUOY.id not in self.ids_invesitigated + ): + if nearest_red: + distance = np.linalg.norm(DB_BUOY.xyz() - nearest_green.xyz()) + nearest_red_distance = np.linalg.norm( + nearest_red.xyz() - nearest_green.xyz(), + ) + nearest_red = ( + DB_BUOY if distance < nearest_red_distance else nearest_red + ) + else: + nearest_red = DB_BUOY + # Set midpoint if nearest red was found + if nearest_red: + mid_x = ( + nearest_green.pose.position.x + nearest_red.pose.position.x + ) / 2 + mid_y = ( + nearest_green.pose.position.y + nearest_red.pose.position.y + ) / 2 + mid_point = (mid_x, mid_y, 0) + self.trajectory.append(mid_point) + + # missing green + elif nearest_red: + rospy.logerr( + f"Could not find green for red({nearest_red.id}), here is the boats pos:\n{boat_pose}", + ) + # Get nearest green to the red + objects: list[PerceptionObject] = ( + await self.mission.database_query(name="all") + ).objects + nearest_green = None + for db_object in objects: + DB_BUOY = Buoy.from_perception_object(db_object) + if ( + db_object.labeled_classification == "green_cylinder" + and DB_BUOY.id not in self.ids_invesitigated + ): + if nearest_green: + distance = np.linalg.norm(DB_BUOY.xyz() - nearest_red.xyz()) + nearest_green_distance = np.linalg.norm( + nearest_green.xyz() - nearest_red.xyz(), + ) + nearest_green = ( + DB_BUOY + if distance < nearest_green_distance + else nearest_green + ) + else: + nearest_green = DB_BUOY + # Set midpoint if nearest red was found + if nearest_green: + mid_x = ( + nearest_green.pose.position.x + nearest_red.pose.position.x + ) / 2 + mid_y = ( + nearest_green.pose.position.y + nearest_red.pose.position.y + ) / 2 + mid_point = (mid_x, mid_y, 0) + self.trajectory.append(mid_point) + + rospy.logerr(mid_point) + + # return next move point + if mid_point and nearest_green and nearest_red: + self.failures_to_move = 0 + rospy.logerr(f"nearest red: {nearest_red.id}") + rospy.logerr(f"nearest green: {nearest_green.id}") + self.trajectory.append(mid_point) + self.midpoint_lengths.append( + math.sqrt( + (nearest_green.pose.position.x - mid_point[0]) ** 2 + + (nearest_green.pose.position.y - mid_point[1]) ** 2, + ), + ) + return Gate(nearest_red, nearest_green) + + if self.failures_to_move > FAILURE_THRESHOLD: + rospy.logerr("FAILURE THRESHOLD REACHED") + return -1 + # from sklearn.preprocessing import PolynomialFeatures + # from sklearn.linear_model import LinearRegression + # import matplotlib.pyplot as plt + + # points=np.array(self.trajectory) + # x = points[:, 0].reshape(-1, 1) # Reshape for sklearn + # y = points[:, 1] + # # Follow the trajectory until you reach the farthest cylinder that is within the + # poly = PolynomialFeatures(degree=2) # You can change the degree as needed + # x_poly = poly.fit_transform(x) + + # # Fit the polynomial regression model + # model = LinearRegression() + # model.fit(x_poly, y) + # x_pred = np.linspace(min(x), max(x), 100) + + # # Predict using the transformed features + # y_pred = model.predict(poly.transform(x_pred)) + + # # Plot the results + # plt.scatter(x, y, color='blue', label='Original Points') + # plt.plot(x_pred, y_pred, color='red', label='Predicted Trajectory') + # plt.xlabel('X Position') + # plt.ylabel('Y Position') + # plt.title('Robot Trajectory Prediction (Polynomial)') + # plt.legend() + # plt.show() + + self.failures_to_move += 1 + return 0 + + async def available_gates(self) -> list[Gate]: + """ + Returns a list of the gates that still need to be traversed. + """ + gates: list[Gate] = [] + objects: list[PerceptionObject] = ( + await self.mission.database_query(name="all") + ).objects + found_buoys: set[Buoy] = set() + for db_object in objects: + # Could we do this more efficiently with a different database request? + if "buoy" not in db_object.labeled_classification: + continue + # Ignore circular buoys + if "circular" in db_object.labeled_classification: + continue + found_buoys.add(Buoy.from_perception_object(db_object)) + + # Order the buoys by distance (to form gates easier) + pose = self.mission.pose + assert pose is not None + self.ordered_buoys: list[Buoy] = sorted( + found_buoys, + key=lambda b: b.distance(pose), + ) + + # Turn the buoys into gates + handled_buoys: set[Buoy] = set() + print(f"found {len(found_buoys)}...") + for buoy in found_buoys: + # Ignore if already a part of another gate + if buoy in handled_buoys: + continue + # Find all nearby buoys that could potentially form a gates + nearby_buoys = self._nearby_buoys(buoy) + # Which gate should we be making? + intended_gate = MiddleGate + if len(gates) == 0: + intended_gate = StartGate + other_color = intended_gate.other_color(buoy.color) + nearby_colored_buoys = [b for b in nearby_buoys if b.color == other_color] + if len(nearby_colored_buoys) == 0: + raise RuntimeError( + "Can't find any nearby buoys matching the color needed for this gate!", + ) + closest_nearest_buoy = nearby_colored_buoys[0] + gates.append(intended_gate(buoy, closest_nearest_buoy)) + handled_buoys.add(buoy) + handled_buoys.add(closest_nearest_buoy) + return gates + + +class NavigationGatefinder(NaviGatorMission): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + async def run(self, parameters): + await self.change_wrench("autonomous") + await axros.util.wrap_time_notice( + self.set_classifier_enabled.wait_for_service(), + 4, + "classifier enable service", + ) + await self.set_classifier_enabled(SetBoolRequest(True)) + # start_pose = await self.tx_pose() + classifier = GateClassifier(self) + + while True: + rospy.logerr(classifier.ids_invesitigated) + next_gate = await classifier.get_next_gate(self.pose[0]) + if next_gate is Gate: + await self.move.set_position(next_gate.mid_pose[0]).set_orientation( + next_gate.mid_pose[1], + ).go() + elif next_gate == -1: + rospy.loginfo("All Done!") + break diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py index 8aaa7b40e..b1318ab79 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py @@ -143,7 +143,11 @@ async def explore_closest_until(self, is_done, filter_and_sort) -> dict: # Exit if done ret = is_done(objects, positions) - labels = [obj[0].labeled_classification for obj in ret] if ret is not None else [] + labels = ( + [obj[0].labeled_classification for obj in ret] + if ret is not None + else [] + ) self.send_feedback(f"Analyzing objects: {labels}") if ret is not None: if move_id_tuple is not None: diff --git a/NaviGator/perception/navigator_vision/train_test_split.py b/NaviGator/perception/navigator_vision/train_test_split.py new file mode 100644 index 000000000..e45c97cb6 --- /dev/null +++ b/NaviGator/perception/navigator_vision/train_test_split.py @@ -0,0 +1,106 @@ +import os +import random +import shutil + +home = os.path.expanduser("~") +# Source folder containing all files + +source_folder = os.path.join(home, "Robosub 2024 Data", "buoy_v1") + +# Destination folders for train and test sets +train_folder = "train" +test_folder = "test" +val_folder = "val" + +# Create train and test folders if they don't exist +# os.makedirs(os.path.join(image_folder, train_folder)) +# os.makedirs(os.path.join(image_folder, test_folder)) +# os.makedirs(os.path.join(label_folder, train_folder)) +# os.makedirs(os.path.join(label_folder, test_folder)) + +# List all files in the source folder +for i in range(1): + folder = f"s{i+1}" + print(folder) + image_folder = os.path.join(source_folder, folder, "images") + label_folder = os.path.join(source_folder, folder, "labels") + + files = os.listdir(image_folder) + files = [ + entry for entry in files if os.path.isfile(os.path.join(image_folder, entry)) + ] + random.shuffle(files) # Shuffle the list of files + + # Calculate split indices + split_index = int(0.8 * len(files)) # 80% for train, 20% for test and val + offset = int((len(files) - split_index) / 2) + test_val_split_index = split_index + offset + + # Split files into train and test sets + train_files = files[:split_index] + test_files = files[split_index:test_val_split_index] + val_files = files[test_val_split_index:] + + # Move train files + print("\nOrganizing Training Data\n") + for file in train_files: + base = os.path.splitext(os.path.basename(file))[0] + + # Move images + image_file = base + ".png" + image_train_path = os.path.join(source_folder, train_folder, "images") + print("Moving image:\n" + image_file + " => " + image_train_path) + src = os.path.join(image_folder, image_file) + dst = os.path.join(image_train_path, image_file) + shutil.move(src, dst) + + # Move labels + label_file = base + ".txt" + label_train_path = os.path.join(source_folder, train_folder, "labels") + print("Moving label:\n" + label_file + " => " + label_train_path) + src = os.path.join(label_folder, label_file) + dst = os.path.join(label_train_path, label_file) + shutil.move(src, dst) + + print("\nOrganizing Testing Data\n") + # Move test files + for file in test_files: + base = os.path.splitext(os.path.basename(file))[0] + # Move images + image_file = base + ".png" + image_test_path = os.path.join(source_folder, test_folder, "images") + print("Moving image:\n" + image_file + " => " + image_test_path) + src = os.path.join(image_folder, image_file) + dst = os.path.join(image_test_path, image_file) + shutil.move(src, dst) + + # Move labels + label_file = base + ".txt" + label_test_path = os.path.join(source_folder, test_folder, "labels") + print("Moving label:\n" + label_file + " => " + label_test_path) + src = os.path.join(label_folder, label_file) + dst = os.path.join(label_test_path, label_file) + shutil.move(src, dst) + + print("\nOrganizing Validation Data\n") + # Move test files + for file in val_files: + base = os.path.splitext(os.path.basename(file))[0] + # Move images + image_file = base + ".png" + image_val_path = os.path.join(source_folder, val_folder, "images") + print("Moving image:\n" + image_file + " => " + image_val_path) + src = os.path.join(image_folder, image_file) + dst = os.path.join(image_val_path, image_file) + shutil.move(src, dst) + + # Move labels + label_file = base + ".txt" + label_val_path = os.path.join(source_folder, val_folder, "labels") + print("Moving label:\n" + label_file + " => " + label_val_path) + src = os.path.join(label_folder, label_file) + dst = os.path.join(label_val_path, label_file) + shutil.move(src, dst) + + +print("Splitting and moving files completed successfully.") diff --git a/NaviGator/scripts/led_panel_xbee.py b/NaviGator/scripts/led_panel_xbee.py new file mode 100755 index 000000000..f8b5f428d --- /dev/null +++ b/NaviGator/scripts/led_panel_xbee.py @@ -0,0 +1,43 @@ +#! /usr/bin/env python3 +""" +Script to send a 3-series color code to the physical LED panel used at testing, +over XBee/Zigbee. +""" +import random +import time + +import serial.tools.list_ports + +# pip3 install digi-xbee +from digi.xbee.devices import XBeeDevice + + +def generate_code() -> str: + choices = list("RGB") + first_code = random.choice(choices) + choices.remove(first_code) + second_code = random.choice(choices) + choices.append(first_code) + choices.remove(second_code) + third_code = random.choice(choices) + return f"{first_code}{second_code}{third_code}" + + +if __name__ == "__main__": + print("Available devices:") + for port, _, _ in serial.tools.list_ports.comports(): + print(port) + print() + device_name = input("Which serial device do you want to connect to? > ") + device = XBeeDevice(device_name, 9600) + device.open() + print("Device is open! Resetting... ") + device.send_data_broadcast("off") + time.sleep(2) + code_to_send = input( + 'What code would you like to send (ie, "RGB")? You can also type "random". > ', + ) + code = code_to_send if code_to_send != "random" else generate_code() + device.send_data_broadcast(code) + print(f'Sent "{code}"! Turning off device...') + device.close() diff --git a/NaviGator/scripts/tmux_start.sh b/NaviGator/scripts/tmux_start.sh new file mode 100755 index 000000000..86bbd27c4 --- /dev/null +++ b/NaviGator/scripts/tmux_start.sh @@ -0,0 +1,25 @@ +#!/bin/bash +if tmux has-session -t auto; then + tmux a -t auto +else + # First window (panes) + tmux new-session -d -s auto + tmux send-keys -t auto:0.0 'roslaunch navigator_launch master.launch --screen' Enter + tmux split-window -h -t auto + tmux split-window -v -t auto + + # Second window (alarms, other panes) + sleep 1.5 + tmux new-window -t auto + tmux split-window -h -t auto:1 + tmux split-window -v -t auto:1 + tmux split-window -v -t auto:1.0 + tmux send-keys 'amonitor kill' Enter + tmux split-window -h + tmux send-keys 'amonitor hw-kill' Enter + tmux select-pane -t auto:1.0 + + # Return to the first window + tmux select-window -t auto:0 + tmux a -t auto +fi diff --git a/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_clear.world b/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_clear.world index e6adad950..f48b90002 100644 --- a/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_clear.world +++ b/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_clear.world @@ -85,7 +85,6 @@ -33.722718 150.674031 0.0 10 10 - 300 wamv_external_pivot_joint @@ -208,16 +207,6 @@ - - buoy_start_r - model://mb_marker_buoy_white - -520.338 168.871 0 0 0 -1.44 - - - buoy_start_l - model://mb_marker_buoy_white - -531.2 176.364 0 0 0 -1.44 - buoy_green1 model://mb_marker_buoy_green @@ -278,15 +267,5 @@ model://mb_marker_buoy_red -500.408539 209.651611 0 0 0 -1.44 - - buoy_end_r - model://mb_marker_buoy_white - -490.633240 200.413437 0 0 0 -1.44 - - - buoy_end_l - model://mb_marker_buoy_white - -492.262939 209.520996 0 0 0 -1.44 - diff --git a/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_obs.world b/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_obs.world index e339607e3..3975b4e93 100644 --- a/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_obs.world +++ b/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path1_obs.world @@ -208,16 +208,6 @@ - - buoy_start_r - model://mb_marker_buoy_white - -520.338 168.871 0 0 0 -1.44 - - - buoy_start_l - model://mb_marker_buoy_white - -531.2 176.364 0 0 0 -1.44 - buoy_green1 model://mb_marker_buoy_green @@ -278,16 +268,6 @@ model://mb_marker_buoy_red -500.408539 209.651611 0 0 0 -1.44 - - buoy_end_r - model://mb_marker_buoy_white - -490.633240 200.413437 0 0 0 -1.44 - - - buoy_end_l - model://mb_marker_buoy_white - -492.262939 209.520996 0 0 0 -1.44 - buoy_black1 model://mb_round_buoy_black diff --git a/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path2_clear.world b/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path2_clear.world index 9aebd1fdb..37cfe0bb8 100644 --- a/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path2_clear.world +++ b/NaviGator/simulation/navigator_gazebo/worlds/pathfinding/path2_clear.world @@ -208,16 +208,6 @@ - - buoy_start_r - model://mb_marker_buoy_white - -520.338 168.871 0 0 0 -1.44 - - - buoy_start_l - model://mb_marker_buoy_white - -531.2 176.364 0 0 0 -1.44 - green1 model://mb_marker_buoy_green @@ -278,15 +268,5 @@ model://mb_marker_buoy_red -509.021057 211.978912 0 0 0 -1.44 - - buoy_end_r - model://mb_marker_buoy_white - -501.999146 201.727905 0 0 0 -1.44 - - - buoy_end_l - model://mb_marker_buoy_white - -499.310181 210.086487 0 0 0 -1.44 - diff --git a/NaviGator/utils/navigator_msgs/CMakeLists.txt b/NaviGator/utils/navigator_msgs/CMakeLists.txt index 54c1c2ebf..dffa1e0f2 100644 --- a/NaviGator/utils/navigator_msgs/CMakeLists.txt +++ b/NaviGator/utils/navigator_msgs/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs message_generation geometry_msgs + geographic_msgs genmsg actionlib_msgs actionlib @@ -57,7 +58,7 @@ add_service_files( MessageEntranceExitGate.srv MessageFindFling.srv MessageFollowPath.srv - MessageReactReport.srv + MessageWildlifeEncounter.srv MessageUAVReplenishment.srv MessageUAVSearchReport.srv TwoClosestCones.srv @@ -75,6 +76,7 @@ add_action_files( generate_messages( DEPENDENCIES std_msgs + geographic_msgs geometry_msgs actionlib_msgs sensor_msgs @@ -83,6 +85,7 @@ generate_messages( catkin_package( CATKIN_DEPENDS std_msgs + geographic_msgs geometry_msgs sensor_msgs ) diff --git a/NaviGator/utils/navigator_msgs/package.xml b/NaviGator/utils/navigator_msgs/package.xml index 3d2adcefe..bb8d8761f 100644 --- a/NaviGator/utils/navigator_msgs/package.xml +++ b/NaviGator/utils/navigator_msgs/package.xml @@ -10,6 +10,7 @@ actionlib actionlib_msgs actionlib_msgs + geographic_msgs geometry_msgs message_generation message_runtime @@ -19,6 +20,7 @@ actionlib actionlib_msgs std_msgs + geographic_msgs geometry_msgs message_runtime actionlib_msgs diff --git a/NaviGator/utils/navigator_msgs/srv/MessageDetectDock.srv b/NaviGator/utils/navigator_msgs/srv/MessageDetectDock.srv index 4dc1c5c20..cc31c4a0c 100644 --- a/NaviGator/utils/navigator_msgs/srv/MessageDetectDock.srv +++ b/NaviGator/utils/navigator_msgs/srv/MessageDetectDock.srv @@ -1,4 +1,5 @@ -string color # color of docking bay R, G, B +string color # color of docking bay to park in ('R' for example) int8 ams_status # 1=docking, 2=complete +string status_of_delivery # S = 'scanning', D = 'delivering' --- string message diff --git a/NaviGator/utils/navigator_msgs/srv/MessageFollowPath.srv b/NaviGator/utils/navigator_msgs/srv/MessageFollowPath.srv index 7202046d8..960640f2e 100644 --- a/NaviGator/utils/navigator_msgs/srv/MessageFollowPath.srv +++ b/NaviGator/utils/navigator_msgs/srv/MessageFollowPath.srv @@ -1,3 +1,4 @@ +string entry_color # color of the entry buoy ('R' for example) int8 finished # 1=in progress 2=completed --- string message diff --git a/NaviGator/utils/navigator_msgs/srv/MessageReactReport.srv b/NaviGator/utils/navigator_msgs/srv/MessageReactReport.srv deleted file mode 100644 index 398a4694f..000000000 --- a/NaviGator/utils/navigator_msgs/srv/MessageReactReport.srv +++ /dev/null @@ -1,3 +0,0 @@ -string[] animal_array # list of animals (P,C,T), up to three animals (Platypus, Turtle, Croc) ---- -string message diff --git a/NaviGator/utils/navigator_msgs/srv/MessageUAVReplenishment.srv b/NaviGator/utils/navigator_msgs/srv/MessageUAVReplenishment.srv index 2d39c3479..6648fa920 100644 --- a/NaviGator/utils/navigator_msgs/srv/MessageUAVReplenishment.srv +++ b/NaviGator/utils/navigator_msgs/srv/MessageUAVReplenishment.srv @@ -1,4 +1,5 @@ int8 uav_status # 1=stowed, 2=deployed, 3=faulted int8 item_status # 0=not picked up, 1=picked up, 2=delivered +string item_color # color of the item ('R' for example) --- string message diff --git a/NaviGator/utils/navigator_msgs/srv/MessageUAVSearchReport.srv b/NaviGator/utils/navigator_msgs/srv/MessageUAVSearchReport.srv index 60830244d..1c68f5247 100644 --- a/NaviGator/utils/navigator_msgs/srv/MessageUAVSearchReport.srv +++ b/NaviGator/utils/navigator_msgs/srv/MessageUAVSearchReport.srv @@ -1,13 +1,7 @@ -string object1 -float64 object1_latitude -string object1_n_s -float64 object1_longitude -string object1_e_w +string object1 # 'R' for the R pad, 'N' for the N pad +geographic_msgs/GeoPoint object1_location string object2 -float64 object2_latitude -string object2_n_s -float64 object2_longitude -string object2_e_w +geographic_msgs/GeoPoint object2_location int8 uav_status # 1=manual, 2=autonomous, 3=faulted --- string message diff --git a/NaviGator/utils/navigator_msgs/srv/MessageWildlifeEncounter.srv b/NaviGator/utils/navigator_msgs/srv/MessageWildlifeEncounter.srv new file mode 100644 index 000000000..5ce11f5ad --- /dev/null +++ b/NaviGator/utils/navigator_msgs/srv/MessageWildlifeEncounter.srv @@ -0,0 +1,5 @@ +string circling_wildlife # which animal to circle ('P' for example (python)) +bool clockwise # whether to circle clockwise or counter-clockwise +int8 number_of_circles # how many circles to do +--- +string message diff --git a/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/__init__.py b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/__init__.py index b276a6c29..241d39065 100644 --- a/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/__init__.py +++ b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/__init__.py @@ -4,8 +4,8 @@ RobotXFindFlingMessage, RobotXFollowPathMessage, RobotXHeartbeatMessage, - RobotXReactReportMessage, RobotXScanCodeMessage, RobotXUAVReplenishmentMessage, RobotXUAVSearchReportMessage, + RobotXWildlifeEncounterMessage, ) diff --git a/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py index 6a20ad08a..e2c3a01eb 100644 --- a/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py +++ b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py @@ -4,10 +4,13 @@ of messages for the RobotX Communication Protocol """ +from __future__ import annotations + import math -from typing import Any, List, Optional, Tuple +from typing import Any import tf.transformations as trans +from geographic_msgs.msg import GeoPoint from mil_tools import rosmsg_to_numpy from nav_msgs.msg import Odometry from navigator_msgs.srv import ( @@ -15,8 +18,8 @@ MessageEntranceExitGateRequest, MessageFindFlingRequest, MessageFollowPathRequest, - MessageReactReportRequest, MessageUAVReplenishmentRequest, + MessageWildlifeEncounterRequest, ) @@ -36,8 +39,8 @@ class RobotXHeartbeatMessage: .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarasota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -50,7 +53,7 @@ def __init__(self): self.message_id = "RXHRB" self.timestamp_last = None - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ From a message representing a message as a string, return the data and checksum lists encoded in the string. @@ -72,11 +75,11 @@ def to_string( self, delim: str, team_id: str, - aedt_date_time: Any, - gps_array: Optional[Any], - odom: Optional[Odometry], - uav_status: Optional[int], - system_mode: Optional[int], + edt_date_time: Any, + gps_array: Any, + odom: Odometry | None, + uav_status: int | None, + system_mode: int | None, use_test_data: bool, ) -> str: """ @@ -87,7 +90,7 @@ def to_string( Args: delim (str): The delimiter to use when separating the data. team_id (str): The team ID used by MIL when sending messages. - aedt_date_time (Any): Presumably (??) a datetime object representing the + edt_date_time (Any): Presumably (??) a datetime object representing the current time in AEDT. gps_array (Optional[Any]): A specific message type containing at least a point. (??) odom (Optional[Odometry]): An optional odometry message to encode in the message. @@ -138,7 +141,7 @@ def to_string( if system_mode is None: system_mode = 0 - first_half_data = f"{self.message_id}{delim}{aedt_date_time}{delim}{latitude}{delim}{north_south}" + first_half_data = f"{self.message_id}{delim}{edt_date_time}{delim}{latitude}{delim}{north_south}" second_half_data = f"{longitude}{delim}{east_west}{delim}{team_id}{delim}{system_mode}{delim}{uav_status!s}" @@ -163,8 +166,8 @@ class RobotXEntranceExitGateMessage: .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarasota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -176,7 +179,7 @@ class RobotXEntranceExitGateMessage: def __init__(self): self.message_id = "RXGAT" - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ Constructs a list of data values and a checksum list from a provided message. @@ -197,7 +200,7 @@ def to_string( self, delim: str, team_id: Any, - aedt_date_time: Any, + edt_date_time: Any, data: MessageEntranceExitGateRequest, use_test_data: bool, ) -> str: @@ -209,7 +212,7 @@ def to_string( delim (str): The delimiter to use in between data values. team_id (Any): A value (??) that can be converted to a string to represent the MIL team ID. - aedt_date_time (Any): A value (??) used to represent the current date + time + edt_date_time (Any): A value (??) used to represent the current date + time in AEDT. data (MessageEntranceExitGateRequest): The data about the entrance/exit gate mission. @@ -220,7 +223,7 @@ def to_string( str: The encoded message. """ - data = f"{self.message_id}{delim}{aedt_date_time}{delim}{team_id}{delim}{data.entrance_gate!s}{delim}{data.exit_gate!s}" + data = f"{self.message_id}{delim}{edt_date_time}{delim}{team_id}{delim}{data.entrance_gate!s}{delim}{data.exit_gate!s}" # test data if use_test_data: @@ -241,8 +244,8 @@ class RobotXFollowPathMessage: .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarasota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -254,7 +257,7 @@ class RobotXFollowPathMessage: def __init__(self): self.message_id = "RXPTH" - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ Constructs a list of data values and a checksum list from a provided message. @@ -275,7 +278,7 @@ def to_string( self, delim: str, team_id: Any, - aedt_date_time: Any, + edt_date_time: Any, data: MessageFollowPathRequest, use_test_data: bool, ) -> str: @@ -287,7 +290,7 @@ def to_string( delim (str): The delimiter to use in between data values. team_id (Any): A value (??) that can be converted to a string to represent the MIL team ID. - aedt_date_time (Any): A value (??) used to represent the current date + time + edt_date_time (Any): A value (??) used to represent the current date + time in AEDT. data (MessageFollowPathRequest): The data about the follow path mission. use_test_data (bool): Whether to use test data in the message. If ``True``, @@ -297,7 +300,7 @@ def to_string( str: The encoded message. """ - data = f"{self.message_id}{delim}{aedt_date_time}{delim}{team_id}{delim}{data.finished!s}" + data = f"{self.message_id}{delim}{edt_date_time}{delim}{team_id}{delim}{data.finished!s}" # test data if use_test_data: @@ -312,14 +315,14 @@ def to_string( return msg_return -class RobotXReactReportMessage: +class RobotXWildlifeEncounterMessage: """ - Handles formation of react report message. + Handles formation of the wildlife encounter message. .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarsota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -331,7 +334,7 @@ class RobotXReactReportMessage: def __init__(self): self.message_id = "RXENC" - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ Constructs a list of data values and a checksum list from a provided message. @@ -352,8 +355,8 @@ def to_string( self, delim: str, team_id: Any, - aedt_date_time: Any, - data: MessageReactReportRequest, + edt_date_time: Any, + data: MessageWildlifeEncounterRequest, use_test_data: bool, ) -> str: """ @@ -364,9 +367,9 @@ def to_string( delim (str): The delimiter to use in between data values. team_id (Any): A value (??) that can be converted to a string to represent the MIL team ID. - aedt_date_time (Any): A value (??) used to represent the current date + time + edt_date_time (Any): A value (??) used to represent the current date + time in AEDT. - data (MessageReactReportRequest): The data about the react report mission. + data (MessageWildlifeEncounterRequest): The data about the wildlife encounter mission. use_test_data (bool): Whether to use test data in the message. If ``True``, then most of the other parameters are ignored. @@ -374,10 +377,7 @@ def to_string( str: The encoded message. """ - data_ = f"{self.message_id}{delim}{aedt_date_time}{delim}{team_id}{delim}{len(data.animal_array)!s}" - - for animal in data.animal_array: - data_ += delim + animal + data_ = f"{self.message_id}{delim}{edt_date_time}{delim}{team_id}{delim}{data.circling_wildlife}{delim}{'CW' if data.clockwise else 'CCW'}{delim}{data.number_of_circles}" # test data if use_test_data: @@ -398,8 +398,8 @@ class RobotXScanCodeMessage: .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarasota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -411,7 +411,7 @@ class RobotXScanCodeMessage: def __init__(self): self.message_id = "RXCOD" - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ Returns the information encoded in a message. @@ -432,7 +432,7 @@ def to_string( self, delim: str, team_id: Any, - aedt_date_time: Any, + edt_date_time: Any, color_pattern: str, use_test_data: bool, ) -> str: @@ -443,14 +443,14 @@ def to_string( delim (str): The string delimiter used to separate distinct data points in the message. team_id (Any): The team ID used by MIL in the competition. - aedt_date_time (Any): The datetime to send in AEDT. + edt_date_time (Any): The datetime to send in AEDT. color_pattern (str): The color pattern to send in the message. use_test_data (bool): Whether to use test data when sending the message. Returns: str: The constructed message. """ - data = f"{self.message_id}{delim}{aedt_date_time}{delim}{team_id}{delim}{color_pattern}" + data = f"{self.message_id}{delim}{edt_date_time}{delim}{team_id}{delim}{color_pattern}" # test data if use_test_data: @@ -471,8 +471,8 @@ class RobotXDetectDockMessage: .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarasota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -484,7 +484,7 @@ class RobotXDetectDockMessage: def __init__(self): self.message_id = "RXDOK" - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ Constructs a list of data values and a checksum list from a provided message. @@ -505,7 +505,7 @@ def to_string( self, delim: str, team_id: Any, - aedt_date_time: Any, + edt_date_time: Any, data: MessageDetectDockRequest, use_test_data: bool, ) -> str: @@ -517,7 +517,7 @@ def to_string( delim (str): The delimiter to use in between data values. team_id (Any): A value (??) that can be converted to a string to represent the MIL team ID. - aedt_date_time (Any): A value (??) used to represent the current date + time + edt_date_time (Any): A value (??) used to represent the current date + time in AEDT. data (MessageDetectDockRequest): The data about the detect dock mission. use_test_data (bool): Whether to use test data in the message. If ``True``, @@ -527,7 +527,7 @@ def to_string( str: The encoded message. """ - data = f"{self.message_id}{delim}{aedt_date_time}{delim}{team_id}{delim}{data.color!s}{delim}{data.ams_status!s}" + data = f"{self.message_id}{delim}{edt_date_time}{delim}{team_id}{delim}{data.color!s}{delim}{data.ams_status!s}" # test data if use_test_data: @@ -548,8 +548,8 @@ class RobotXFindFlingMessage: .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarasota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -561,7 +561,7 @@ class RobotXFindFlingMessage: def __init__(self): self.message_id = "RXFLG" - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ Constructs a list of data values and a checksum list from a provided message. @@ -582,7 +582,7 @@ def to_string( self, delim: str, team_id: Any, - aedt_date_time: Any, + edt_date_time: Any, data: MessageFindFlingRequest, use_test_data: bool, ) -> str: @@ -594,7 +594,7 @@ def to_string( delim (str): The delimiter to use in between data values. team_id (Any): A value (??) that can be converted to a string to represent the MIL team ID. - aedt_date_time (Any): A value (??) used to represent the current date + time + edt_date_time (Any): A value (??) used to represent the current date + time in AEDT. data (MessageFindFlingRequest): The data about the find fling mission. use_test_data (bool): Whether to use test data in the message. If ``True``, @@ -604,7 +604,7 @@ def to_string( str: The encoded message. """ - data = f"{self.message_id}{delim}{aedt_date_time}{delim}{team_id}{delim}{data.color!s}{delim}{data.ams_status!s}" + data = f"{self.message_id}{delim}{edt_date_time}{delim}{team_id}{delim}{data.color!s}{delim}{data.ams_status!s}" # test data if use_test_data: @@ -625,8 +625,8 @@ class RobotXUAVReplenishmentMessage: .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarasota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -638,7 +638,7 @@ class RobotXUAVReplenishmentMessage: def __init__(self): self.message_id = "RXUAV" - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ Constructs a list of data values and a checksum list from a provided message. @@ -659,7 +659,7 @@ def to_string( self, delim: str, team_id: Any, - aedt_date_time: Any, + edt_date_time: Any, data: MessageUAVReplenishmentRequest, use_test_data: bool, ) -> str: @@ -671,7 +671,7 @@ def to_string( delim (str): The delimiter to use in between data values. team_id (Any): A value (??) that can be converted to a string to represent the MIL team ID. - aedt_date_time (Any): A value (??) used to represent the current date + time + edt_date_time (Any): A value (??) used to represent the current date + time in AEDT. data (MessageUAVReplenishmentRequest): The data about the WAV replenishment mission. use_test_data (bool): Whether to use test data in the message. If ``True``, @@ -681,7 +681,7 @@ def to_string( str: The encoded message. """ - data = f"{self.message_id}{delim}{aedt_date_time}{delim}{team_id}{delim}{data.uav_status!s}{delim}{data.item_status!s}" + data = f"{self.message_id}{delim}{edt_date_time}{delim}{team_id}{delim}{data.uav_status!s}{delim}{data.item_status!s}" # test data if use_test_data: @@ -702,8 +702,8 @@ class RobotXUAVSearchReportMessage: .. warning:: - The following code pertains to the **2022 edition** of the AUVSI RobotX - competition, held in Australia. Updates to the specifications may have changed + The following code pertains to the **2024 edition** of the AUVSI RobotX + competition, held in Sarasota. Updates to the specifications may have changed since this competition, and therefore, the code may not accurately represent the specifications MIL must produce for the competition. @@ -715,7 +715,7 @@ class RobotXUAVSearchReportMessage: def __init__(self): self.message_id = "RXSAR" - def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: + def from_string(self, delim: bytes, string: str) -> tuple[list[str], list[str]]: """ Constructs a list of data values and a checksum list from a provided message. @@ -732,11 +732,19 @@ def from_string(self, delim: bytes, string: str) -> Tuple[List[str], List[str]]: checksum_list = string.split(b"*") return data_list, checksum_list + def lat_lon_ns(self, point: GeoPoint) -> tuple[float, str, float, str]: + return ( + abs(point.latitude), + "N" if point.latitude >= 0 else "S", + abs(point.longitude), + "E" if point.longitude >= 0 else "W", + ) + def to_string( self, delim: str, team_id: Any, - aedt_date_time: Any, + edt_date_time: Any, data: MessageUAVReplenishmentRequest, use_test_data: bool, ) -> str: @@ -748,7 +756,7 @@ def to_string( delim (str): The delimiter to use in between data values. team_id (Any): A value (??) that can be converted to a string to represent the MIL team ID. - aedt_date_time (Any): A value (??) used to represent the current date + time + edt_date_time (Any): A value (??) used to represent the current date + time in AEDT. data (MessageUAVReplenishmentRequest): The data about the WAV replenishment mission. use_test_data (bool): Whether to use test data in the message. If ``True``, @@ -758,7 +766,10 @@ def to_string( str: The encoded message. """ - data = f"{self.message_id}{delim}{aedt_date_time}{delim}{data.object1!s}{delim}{data.object1_latitude!s}{delim}{data.object1_n_s!s}{delim}{data.object1_longitude!s}{delim}{data.object1_e_w!s}{delim}{data.object2!s}{delim}{data.object2_latitude!s}{delim}{data.object2_n_s!s}{delim}{data.object2_longitude!s}{delim}{data.object2_e_w!s}{delim}{team_id}{delim}{data.uav_status!s}" + o1_lat, o1_ns, o1_lon, o1_ew = self.lat_lon_ns(data.object1_location) + o2_lat, o2_ns, o2_lon, o2_ew = self.lat_lon_ns(data.object2_location) + + data = f"{self.message_id}{delim}{edt_date_time}{delim}{data.object1!s}{delim}{o1_lat!s}{delim}{o1_ns!s}{delim}{o1_lon!s}{delim}{o1_ew!s}{delim}{data.object2!s}{delim}{o2_lat!s}{delim}{o2_ns!s}{delim}{o2_lon!s}{delim}{o2_ew!s}{delim}{team_id}{delim}{data.uav_status!s}" # test data if use_test_data: diff --git a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py index f64f9a4dc..15abcca72 100755 --- a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py +++ b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py @@ -28,15 +28,15 @@ MessageFollowPath, MessageFollowPathRequest, MessageFollowPathResponse, - MessageReactReport, - MessageReactReportRequest, - MessageReactReportResponse, MessageUAVReplenishment, MessageUAVReplenishmentRequest, MessageUAVReplenishmentResponse, MessageUAVSearchReport, MessageUAVSearchReportRequest, MessageUAVSearchReportResponse, + MessageWildlifeEncounter, + MessageWildlifeEncounterRequest, + MessageWildlifeEncounterResponse, ) from navigator_robotx_comms.navigator_robotx_comms import ( RobotXDetectDockMessage, @@ -44,10 +44,10 @@ RobotXFindFlingMessage, RobotXFollowPathMessage, RobotXHeartbeatMessage, - RobotXReactReportMessage, RobotXScanCodeMessage, RobotXUAVReplenishmentMessage, RobotXUAVSearchReportMessage, + RobotXWildlifeEncounterMessage, ) from ros_alarms import AlarmListener from ros_alarms_msgs.msg import Alarm @@ -100,7 +100,7 @@ def __init__(self): # time last called self.time_last_entrance_exit = None self.time_last_follow_path = None - self.time_last_react_report = None + self.time_last_wildlife_encounter = None self.time_last_find_fling = None self.time_last_uav_replenishment = None self.time_last_uav_search_report = None @@ -116,7 +116,7 @@ def __init__(self): self.robotx_scan_code_message = RobotXScanCodeMessage() self.robotx_detect_dock_message = RobotXDetectDockMessage() self.robotx_follow_path_message = RobotXFollowPathMessage() - self.robotx_react_report_message = RobotXReactReportMessage() + self.robotx_wildlife_encounter_message = RobotXWildlifeEncounterMessage() self.robotx_find_fling_message = RobotXFindFlingMessage() self.robotx_uav_replenishment_message = RobotXUAVReplenishmentMessage() self.robotx_uav_search_report_message = RobotXUAVSearchReportMessage() @@ -144,10 +144,10 @@ def __init__(self): MessageFollowPath, self.handle_follow_path_message, ) - self.service_react_report_message = rospy.Service( - "react_report_message", - MessageReactReport, - self.handle_react_report_message, + self.service_wildlife_encounter_message = rospy.Service( + "wildlife_encounter_message", + MessageWildlifeEncounter, + self.handle_wildlife_encounter_message, ) self.service_detect_dock_message = rospy.Service( "detect_dock_message", @@ -241,12 +241,12 @@ def handle_heartbeat_message(self, _) -> None: message to the AUVSI Technical Director station. """ self.update_system_mode() - aedt_date_time = self.get_aedt_date_time() + edt_date_time = self.get_edt_date_time() message = self.robotx_heartbeat_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, self.gps_array, self.odom, self.uav_status, @@ -275,11 +275,11 @@ def handle_entrance_exit_gate_message( if seconds_elapsed < 1: rospy.sleep(1 - seconds_elapsed) self.time_last_entrance_exit = rospy.get_time() - aedt_date_time = self.get_aedt_date_time() + edt_date_time = self.get_edt_date_time() message = self.robotx_entrance_exit_gate_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, data, self.use_test_data, ) @@ -306,11 +306,11 @@ def handle_follow_path_message( if seconds_elapsed < 1: rospy.sleep(1 - seconds_elapsed) self.time_last_follow_path = rospy.get_time() - aedt_date_time = self.get_aedt_date_time() + edt_date_time = self.get_edt_date_time() message = self.robotx_follow_path_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, data, self.use_test_data, ) @@ -318,36 +318,36 @@ def handle_follow_path_message( self.robotx_client.send_message(message) return MessageFollowPathResponse(message) - def handle_react_report_message( + def handle_wildlife_encounter_message( self, - data: MessageReactReportRequest, - ) -> MessageReactReportResponse: + data: MessageWildlifeEncounterRequest, + ) -> MessageWildlifeEncounterResponse: """ - Handles requests to make messages to use in the React Report Mission + Handles requests to make messages to use in the Wildlife Encounter Mission Args: - data (MessageReactReportRequest): The request to the service. + data (MessageWildlifeEncounterRequest): The request to the service. Returns: - MessageReactReportResponse: The response from the service. The response + MessageWildlifeEncounterResponse: The response from the service. The response contains the message needed to send to AUVSI. """ - if self.time_last_react_report is not None: - seconds_elapsed = rospy.get_time() - self.time_last_react_report + if self.time_last_wildlife_encounter is not None: + seconds_elapsed = rospy.get_time() - self.time_last_wildlife_encounter if seconds_elapsed < 1: rospy.sleep(1 - seconds_elapsed) - self.time_last_react_report = rospy.get_time() - aedt_date_time = self.get_aedt_date_time() - message = self.robotx_react_report_message.to_string( + self.time_last_wildlife_encounter = rospy.get_time() + edt_date_time = self.get_edt_date_time() + message = self.robotx_wildlife_encounter_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, data, self.use_test_data, ) self.robotx_client.send_message(message) - return MessageReactReportResponse(message) + return MessageWildlifeEncounterResponse(message) def handle_scan_code_message(self, color_pattern: str) -> None: """ @@ -366,11 +366,11 @@ def handle_scan_code_message(self, color_pattern: str) -> None: if seconds_elapsed < 1: rospy.sleep(1 - seconds_elapsed) self.time_last_scan_code = rospy.get_time() - aedt_date_time = self.get_aedt_date_time() + edt_date_time = self.get_edt_date_time() message = self.robotx_scan_code_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, color_pattern, self.use_test_data, ) @@ -395,11 +395,11 @@ def handle_detect_dock_message( if seconds_elapsed < 1: rospy.sleep(1 - seconds_elapsed) self.time_last_detect_dock = rospy.get_time() - aedt_date_time = self.get_aedt_date_time() + edt_date_time = self.get_edt_date_time() message = self.robotx_detect_dock_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, data, self.use_test_data, ) @@ -426,11 +426,11 @@ def handle_find_fling_message( if seconds_elapsed < 1: rospy.sleep(1 - seconds_elapsed) self.time_last_find_fling = rospy.get_time() - aedt_date_time = self.get_aedt_date_time() + edt_date_time = self.get_edt_date_time() message = self.robotx_find_fling_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, data, self.use_test_data, ) @@ -457,11 +457,11 @@ def handle_uav_replenishment_message( if seconds_elapsed < 1: rospy.sleep(1 - seconds_elapsed) self.time_last_uav_replenishment = rospy.get_time() - aedt_date_time = self.get_aedt_date_time() + edt_date_time = self.get_edt_date_time() message = self.robotx_uav_replenishment_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, data, self.use_test_data, ) @@ -488,11 +488,11 @@ def handle_uav_search_report_message( if seconds_elapsed < 1: rospy.sleep(1 - seconds_elapsed) self.time_last_uav_search_report = rospy.get_time() - aedt_date_time = self.get_aedt_date_time() + edt_date_time = self.get_edt_date_time() message = self.robotx_uav_search_report_message.to_string( self.delim, self.team_id, - aedt_date_time, + edt_date_time, data, self.use_test_data, ) @@ -500,7 +500,7 @@ def handle_uav_search_report_message( self.robotx_client.send_message(message) return MessageUAVSearchReportResponse(message) - def get_aedt_date_time(self) -> str: + def get_edt_date_time(self) -> str: """ Gets the current time in AEDT in the format of ``%d%m%y{self.delim}%H%M%S``. This is the format specified by AUVSI to use in messages. @@ -508,10 +508,10 @@ def get_aedt_date_time(self) -> str: Returns: str: The constructed string representing the date and time. """ - # AEDT is 11 hours ahead of UTC - aedt_time = datetime.datetime.utcnow() + datetime.timedelta(hours=11, minutes=0) - date_string = aedt_time.strftime("%d%m%y") - time_string = aedt_time.strftime("%H%M%S") + # EDT is 11 hours ahead of UTC + edt_time = datetime.datetime.utcnow() - datetime.timedelta(hours=5) + date_string = edt_time.strftime("%d%m%y") + time_string = edt_time.strftime("%H%M%S") return date_string + self.delim + time_string diff --git a/NaviGator/utils/navigator_robotx_comms/test/robotx_comms_server.py b/NaviGator/utils/navigator_robotx_comms/test/robotx_comms_server.py index 22642e27a..48f557836 100755 --- a/NaviGator/utils/navigator_robotx_comms/test/robotx_comms_server.py +++ b/NaviGator/utils/navigator_robotx_comms/test/robotx_comms_server.py @@ -11,6 +11,7 @@ import rospy import rostest +from geographic_msgs.msg import GeoPoint from mil_tools import thread_lock from navigator_msgs.msg import ScanTheCode from navigator_msgs.srv import ( @@ -18,9 +19,9 @@ MessageEntranceExitGate, MessageFindFling, MessageFollowPath, - MessageReactReport, MessageUAVReplenishment, MessageUAVSearchReport, + MessageWildlifeEncounter, ) from navigator_robotx_comms.navigator_robotx_comms import ( BitwiseXORChecksum, @@ -29,10 +30,10 @@ RobotXFindFlingMessage, RobotXFollowPathMessage, RobotXHeartbeatMessage, - RobotXReactReportMessage, RobotXScanCodeMessage, RobotXUAVReplenishmentMessage, RobotXUAVSearchReportMessage, + RobotXWildlifeEncounterMessage, ) lock = threading.Lock() @@ -301,6 +302,7 @@ def test_detect_dock_message(self): # data to test message with dock_color = "R" ams_status = 1 + status_of_delivery = "S" rospy.wait_for_service("detect_dock_message") send_robot_x_detect_dock_message = rospy.ServiceProxy( @@ -313,7 +315,11 @@ def test_detect_dock_message(self): try: while not rospy.is_shutdown() and times_ran < self.number_of_iterations: rx_data = None - send_robot_x_detect_dock_message(dock_color, ams_status) + send_robot_x_detect_dock_message( + dock_color, + ams_status, + status_of_delivery, + ) while rx_data is None: rx_data = self.server.receive_message() split_rx_data = rx_data.splitlines(True) @@ -394,6 +400,7 @@ def test_follow_path_message(self): times_ran = 0 self.server.connect() # data to test message with + finished_color = "R" finished = 1 rospy.wait_for_service("follow_path_message") @@ -407,7 +414,7 @@ def test_follow_path_message(self): try: while not rospy.is_shutdown() and times_ran < self.number_of_iterations: rx_data = None - send_robot_x_follow_path_message(finished) + send_robot_x_follow_path_message(finished_color, finished) while rx_data is None: rx_data = self.server.receive_message() split_rx_data = rx_data.splitlines(True) @@ -474,29 +481,35 @@ def test_follow_path_message(self): finally: self.server.disconnect() - def test_react_report_message(self): + def test_wildlife_encounter_message(self): times_ran = 0 self.server.connect() # data to test message with - animal_array = ["P", "C", "T"] - - rospy.wait_for_service("react_report_message") - send_robot_x_react_report_message = rospy.ServiceProxy( - "react_report_message", - MessageReactReport, + circling_wildlife = "R" + clockwise = False + number_of_circles = 1 + + rospy.wait_for_service("wildlife_encounter_message") + send_robot_x_wildlife_encounter_message = rospy.ServiceProxy( + "wildlife_encounter_message", + MessageWildlifeEncounter, ) - robot_x_react_report_message = RobotXReactReportMessage() + robot_x_wildlife_encounter_message = RobotXWildlifeEncounterMessage() try: while not rospy.is_shutdown() and times_ran < self.number_of_iterations: rx_data = None - send_robot_x_react_report_message(animal_array) + send_robot_x_wildlife_encounter_message( + circling_wildlife, + clockwise, + number_of_circles, + ) while rx_data is None: rx_data = self.server.receive_message() split_rx_data = rx_data.splitlines(True) for message in split_rx_data: - deserialized_msg = robot_x_react_report_message.from_string( + deserialized_msg = robot_x_wildlife_encounter_message.from_string( self.delim, message, ) @@ -512,8 +525,8 @@ def test_react_report_message(self): final_checksum_string = hex_checksum + "\r\n" self.assertEqual( len(data_list), - 8, - "react report message formatting incorrect", + 7, + "wildlife encounter message formatting incorrect", ) if self.use_test_data is True: test_data = "$RXENC,111221,161229,ROBOT,3,P,C,T*51\r\n" @@ -527,13 +540,13 @@ def test_react_report_message(self): self.assertEqual( checksum_list[1], checksum_list_test_data[1], - "react report message checksum incorrect", + "wildlife encounter message checksum incorrect", ) self.assertEqual( data_list[4], list_test_data[4], - "animal array length incorrect", + "buoy array length incorrect", ) for i in range(int(data_list[4])): @@ -541,15 +554,15 @@ def test_react_report_message(self): self.assertEqual( data_list[5 + i], list_test_data[5 + i], - "animal incorrect", + "buoy incorrect", ) else: - msg_animal = data_list[5 + i].split("*")[0] - animal_ = list_test_data[5 + i].split("*")[0] + msg_buoy = data_list[5 + i].split("*")[0] + buoy_ = list_test_data[5 + i].split("*")[0] self.assertEqual( - msg_animal, - animal_, - "animal incorrect", + msg_buoy, + buoy_, + "buoy incorrect", ) else: @@ -561,28 +574,8 @@ def test_react_report_message(self): self.assertEqual( checksum_list[1], final_checksum_string, - "react report message checksum incorrect", - ) - self.assertEqual( - int(data_list[4]), - len(animal_array), - "animal array length incorrect", + "wildlife encounter message checksum incorrect", ) - - for i, animal in enumerate(animal_array): - if i != len(animal_array) - 1: - self.assertEqual( - data_list[5 + i], - animal, - "animal incorrect", - ) - else: - animal_ = data_list[5 + i].split("*")[0] - self.assertEqual( - animal, - animal_, - "animal incorrect", - ) times_ran += 1 finally: @@ -685,6 +678,7 @@ def test_uav_replenishment_message(self): # data to test message with uav_status = 1 item_status = 0 + item_color = "R" rospy.wait_for_service("uav_replenishment_message") send_robot_x_uav_replenishment_message = rospy.ServiceProxy( @@ -697,7 +691,11 @@ def test_uav_replenishment_message(self): try: while not rospy.is_shutdown() and times_ran < self.number_of_iterations: rx_data = None - send_robot_x_uav_replenishment_message(uav_status, item_status) + send_robot_x_uav_replenishment_message( + uav_status, + item_status, + item_color, + ) while rx_data is None: rx_data = self.server.receive_message() split_rx_data = rx_data.splitlines(True) @@ -779,7 +777,13 @@ def test_uav_search_report_message(self): self.server.connect() # data to test message with object1 = "R" + object1_location = GeoPoint() + object1_location.latitude = 11 + object1_location.longitude = 12 object2 = "S" + object2_location = GeoPoint() + object2_location.latitude = 11 + object2_location.longitude = 12 uav_status = 2 rospy.wait_for_service("uav_search_report_message") @@ -795,15 +799,9 @@ def test_uav_search_report_message(self): rx_data = None send_robot_x_uav_search_report_message( object1, - 0, - "", - 0, - "", + object1_location, object2, - 0, - "", - 0, - "", + object2_location, uav_status, ) while rx_data is None: diff --git a/SubjuGator/drivers/waterlinked_a50_ros_driver b/SubjuGator/drivers/waterlinked_a50_ros_driver new file mode 160000 index 000000000..fa1895247 --- /dev/null +++ b/SubjuGator/drivers/waterlinked_a50_ros_driver @@ -0,0 +1 @@ +Subproject commit fa1895247409cdfcf1b51059835fd88783220584 diff --git a/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp b/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp index 35f85d221..6424caa79 100644 --- a/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp +++ b/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp @@ -1691,7 +1691,7 @@ class SourceFile for (line_idx = 1; line_idx < line_start; ++line_idx) { getline(*_file, line); - if (not *_file) + if (not*_file) { return lines; } @@ -1712,7 +1712,7 @@ class SourceFile for (; line_idx < line_start + line_count; ++line_idx) { getline(*_file, line); - if (not *_file) + if (not*_file) { return lines; } diff --git a/docs/navigator/reference.rst b/docs/navigator/reference.rst index 84b49e4e4..7a21dbec5 100644 --- a/docs/navigator/reference.rst +++ b/docs/navigator/reference.rst @@ -127,25 +127,25 @@ MessageFollowPath :type: str -MessageReactReport -^^^^^^^^^^^^^^^^^^ -.. attributetable:: navigator_msgs.srv.MessageReactReportRequest +MessageWildlifeEncounter +^^^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: navigator_msgs.srv.MessageWildlifeEncounterRequest -.. class:: navigator_msgs.srv.MessageReactReportRequest +.. class:: navigator_msgs.srv.MessageWildlifeEncounterRequest - The request class for the ``navigator_msgs/MessageReactReport`` service. + The request class for the ``navigator_msgs/MessageWildlifeEncounter`` service. - .. attribute:: animal_array + .. attribute:: buoy_array - List of animals (P,C,T), up to three animals (Platypus, Turtle, Croc) + List of buoys (R, G, B) representing the order of the wildlife traversal :type: string[] -.. attributetable:: navigator_msgs.srv.MessageReactReportResponse +.. attributetable:: navigator_msgs.srv.MessageWildlifeEncounterResponse -.. class:: navigator_msgs.srv.MessageReactReportResponse +.. class:: navigator_msgs.srv.MessageWildlifeEncounterResponse - The response class for the ``navigator_msgs/MessageReactReport`` service. + The response class for the ``navigator_msgs/MessageWildlifeEncounter`` service. .. attribute:: message @@ -350,11 +350,11 @@ RobotXHeartbeatMessage .. autoclass:: navigator_robotx_comms.RobotXHeartbeatMessage :members: -RobotXReactReportMessage -^^^^^^^^^^^^^^^^^^^^^^^^ -.. attributetable:: navigator_robotx_comms.RobotXReactReportMessage +RobotXWildlifeEncounterMessage +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: navigator_robotx_comms.RobotXWildlifeEncounterMessage -.. autoclass:: navigator_robotx_comms.RobotXReactReportMessage +.. autoclass:: navigator_robotx_comms.RobotXWildlifeEncounterMessage :members: RobotXScanCodeMessage @@ -411,3 +411,65 @@ SetSpinPacket .. autoclass:: navigator_ball_launcher.SetSpinPacket :members: + +:mod:`navigator_drone_comm` - Boat-drone communication standard +--------------------------------------------------------------- + +.. automodule:: navigator_drone_comm + :members: + +HeartbeatReceivePacket +^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: navigator_drone_comm.HeartbeatReceivePacket + +.. autoclass:: navigator_drone_comm.HeartbeatReceivePacket + :members: + +HeartbeatSetPacket +^^^^^^^^^^^^^^^^^^ +.. attributetable:: navigator_drone_comm.HeartbeatSetPacket + +.. autoclass:: navigator_drone_comm.HeartbeatSetPacket + :members: + +GPSDronePacket +^^^^^^^^^^^^^^ +.. attributetable:: navigator_drone_comm.GPSDronePacket + +.. autoclass:: navigator_drone_comm.GPSDronePacket + :members: + +EStopPacket +^^^^^^^^^^^ +.. attributetable:: navigator_drone_comm.EStopPacket + +.. autoclass:: navigator_drone_comm.EStopPacket + :members: + +StopPacket +^^^^^^^^^^ +.. attributetable:: navigator_drone_comm.StopPacket + +.. autoclass:: navigator_drone_comm.StopPacket + :members: + +StartPacket +^^^^^^^^^^^ +.. attributetable:: navigator_drone_comm.StartPacket + +.. autoclass:: navigator_drone_comm.StartPacket + :members: + +Color +^^^^^ +.. attributetable:: navigator_drone_comm.Color + +.. autoclass:: navigator_drone_comm.Color + :members: + +TargetPacket +^^^^^^^^^^^^ +.. attributetable:: navigator_drone_comm.TargetPacket + +.. autoclass:: navigator_drone_comm.TargetPacket + :members: diff --git a/docs/navigator/testing_checklist.rst b/docs/navigator/testing_checklist.rst index 817e59344..41a650804 100644 --- a/docs/navigator/testing_checklist.rst +++ b/docs/navigator/testing_checklist.rst @@ -30,9 +30,14 @@ Boat and Trailers |uc| Boat prep - |uc| Attach lidar - |uc| Attach camera covers - - |uc| Stow wifi antenna, light post, rf antenna + - |uc| Stow wifi antenna, light post - |uc| Strap computer box closed - |uc| Stow batteries in main trailer + - |uc| Detach the large RF kill antenna + - |uc| Detach ball launcher tube + - |uc| Make sure hydrophone mount it horizontal + - |uc| Landing pad is detached + - |uc| check for no loose fasteners |uc| Boat trailer prep - |uc| Strap pontoons down on both sides - |uc| Check boat trailer lights @@ -46,6 +51,15 @@ Boat and Trailers Mechanical ^^^^^^^^^^ |uc| Challenge elements +|uc| Tape Measure +|uc| 7/16 wrench +|uc| 5/32 Allen Key +|uc| Duct tape and scissor +|uc| Pliers +|uc| Flat-head screwdriver +|uc| O'ring grease (Molykote 55) +|uc| Cable grease (Molykote 44) +|uc| Large and small zip ties Electrical ^^^^^^^^^^ diff --git a/docs/reference/electrical_protocol.rst b/docs/reference/electrical_protocol.rst index 58abe2922..621af67e3 100644 --- a/docs/reference/electrical_protocol.rst +++ b/docs/reference/electrical_protocol.rst @@ -114,6 +114,20 @@ byte length. | Launcher | | | | | Board) | | | | +------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x20 | 0x00 | Empty | :class:`navigator_drone_comm.HeartbeatReceivePacket` | +| (Drone) +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x01 | Empty | :class:`navigator_drone_comm.HeartbeatSetPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x02 | ``fff`` | :class:`navigator_drone_comm.GPSDronePacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x03 | Empty | :class:`navigator_drone_comm.EStopPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x04 | ``20s`` | :class:`navigator_drone_comm.StartPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x05 | Empty | :class:`navigator_drone_comm.StopPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x06 | ``ffc`` | :class:`navigator_drone_comm.TargetPacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ Exceptions ~~~~~~~~~~ diff --git a/hw/navigator/etc/NetworkManager/system-connections/ufdevice.nmconnection b/hw/navigator/etc/NetworkManager/system-connections/ufdevice.nmconnection new file mode 100644 index 000000000..5212de937 --- /dev/null +++ b/hw/navigator/etc/NetworkManager/system-connections/ufdevice.nmconnection @@ -0,0 +1,30 @@ +# This connection requires the MAC address being uploaded to: +# https://deviceregistration.dhnet.ufl.edu + +[connection] +id=ufdevice +uuid=418cfbde-76c1-4208-9fac-6f0dbbd0f565 +type=wifi +interface-name=wlo1 +permissions= + +[wifi] +mac-address-blacklist= +mode=infrastructure +ssid=ufdevice + +[wifi-security] +auth-alg=open +key-mgmt=wpa-psk +psk=gogators + +[ipv4] +dns-search= +method=auto + +[ipv6] +addr-gen-mode=stable-privacy +dns-search= +method=auto + +[proxy] diff --git a/mil_common/drivers/electrical_protocol/CMakeLists.txt b/mil_common/drivers/electrical_protocol/CMakeLists.txt index 91051ac36..48085d3f1 100644 --- a/mil_common/drivers/electrical_protocol/CMakeLists.txt +++ b/mil_common/drivers/electrical_protocol/CMakeLists.txt @@ -10,4 +10,5 @@ catkin_package() if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(test/simulated.test) + add_rostest(test/noros.test) endif() diff --git a/mil_common/drivers/electrical_protocol/electrical_protocol/driver_noros.py b/mil_common/drivers/electrical_protocol/electrical_protocol/driver_noros.py new file mode 100644 index 000000000..44d7e7d7c --- /dev/null +++ b/mil_common/drivers/electrical_protocol/electrical_protocol/driver_noros.py @@ -0,0 +1,266 @@ +#! /usr/bin/env python3 +##################################3 +# electrical_protocol driver without ROS +# +# To run: +# - Leave running in another process/terminal session +# - Inherit from this class to write your own packet sender/receiver +##################################3 +from __future__ import annotations + +import abc +import contextlib +import logging +import threading +from typing import Any, Generic, TypeVar, Union, cast, get_args, get_origin + +import serial + +from .packet import SYNC_CHAR_1, Packet + +SendPackets = TypeVar("SendPackets", bound=Packet) +RecvPackets = TypeVar("RecvPackets", bound=Packet) + + +logger = logging.getLogger(__name__) + + +class BufferThread(threading.Thread): + def __init__(self, event, callable): + super().__init__() + self.stopped = event + self.hz = 20.0 + self.callable = callable + + def run(self): + while not self.stopped.wait(1 / self.hz): + self.callable() + + def set_hz(self, hz: float): + self.hz = hz + + +class ROSSerialDevice(Generic[SendPackets, RecvPackets]): + """ + Represents a generic serial device, which is expected to be the main component + of an individual ROS node. + + Attributes: + port (Optional[str]): The port used for the serial connection, if provided. + baudrate (Optional[int]): The baudrate to use with the device, if provided. + device (Optional[serial.Serial]): The serial class used to communicate with + the device. + rate (float): The reading rate of the device, in Hertz. Set to `20` by default. + """ + + device: serial.Serial | None + _recv_T: Any + _send_T: Any + + def is_connected(self) -> bool: + return self.device is not None + + def is_open(self) -> bool: + return bool(self.device) and self.device.is_open + + def __init__( + self, + port: str | None, + baudrate: int | None, + buffer_process_hz: float = 20.0, + ) -> None: + """ + Arguments: + port (Optional[str]): The serial port to connect to. If ``None``, connection + will not be established on initialization; rather, the user can use + :meth:`~.connect` to connect later. + baudrate (Optional[int]): The baudrate to connect with. If ``None`` and + a port is specified, then 115200 is assumed. + """ + self.port = port + self.baudrate = baudrate + if port: + self.device = serial.Serial(port, baudrate or 115200, timeout=0.1) + if not self.device.is_open: + self.device.open() + self.device.reset_input_buffer() + self.device.reset_output_buffer() + else: + self.device = None + self.lock = threading.Lock() + self.rate = buffer_process_hz + self.buff_event = threading.Event() + self.buff_thread = BufferThread(self.buff_event, self._process_buffer) + self.buff_thread.daemon = True + self.buff_thread.start() + + def __init_subclass__(cls) -> None: + # this is a super hack lol :P + # cred: https://stackoverflow.com/a/71720366 + cls._send_T = get_args(cls.__orig_bases__[0])[0] # type: ignore + cls._recv_T = get_args(cls.__orig_bases__[0])[1] # type: ignore + + def connect(self, port: str, baudrate: int) -> None: + """ + Connects to the port with the given baudrate. If the device is already + connected, the input and output buffers will be flushed. + + Arguments: + port (str): The serial port to connect to. + baudrate (int): The baudrate to connect with. + """ + self.port = port + self.baudrate = baudrate + self.device = serial.Serial(port, baudrate, timeout=0.1) + if not self.device.is_open: + self.device.open() + self.device.reset_input_buffer() + self.device.reset_output_buffer() + + def close(self) -> None: + """ + Closes the serial device. + """ + logger.info("Shutting down thread...") + self.buff_event.set() + logger.info("Closing serial device...") + if not self.device: + raise RuntimeError("Device is not connected.") + else: + # TODO: Find a better way to deal with these os errors + with contextlib.suppress(OSError): + if self.device.in_waiting: + logger.warn( + "Shutting down device, but packets were left in buffer...", + ) + self.device.close() + + def write(self, data: bytes) -> None: + """ + Writes a series of raw bytes to the device. This method should rarely be + used; using :meth:`~.send_packet` is preferred because of the guarantees + it provides through the packet class. + + Arguments: + data (bytes): The data to send. + """ + if not self.device: + raise RuntimeError("Device is not connected.") + self.device.write(data) + + def send_packet(self, packet: SendPackets) -> None: + """ + Sends a given packet to the device. + + Arguments: + packet (:class:`~.Packet`): The packet to send. + """ + with self.lock: + self.write(bytes(packet)) + + def _read_from_stream(self) -> bytes: + # Read until SOF is encourntered in case buffer contains the end of a previous packet + if not self.device: + raise RuntimeError("Device is not connected.") + sof = None + for _ in range(10): + sof = self.device.read(1) + if not len(sof): + continue + sof_int = int.from_bytes(sof, byteorder="big") + if sof_int == SYNC_CHAR_1: + break + if not isinstance(sof, bytes): + raise TimeoutError("No SOF received in one second.") + sof_int = int.from_bytes(sof, byteorder="big") + if sof_int != SYNC_CHAR_1: + logger.error("Where da start char at?") + data = sof + # Read sync char 2, msg ID, subclass ID + data += self.device.read(3) + length = self.device.read(2) # read payload length + data += length + data += self.device.read( + int.from_bytes(length, byteorder="little") + 2, + ) # read data and checksum + return data + + def _correct_type(self, provided: Any) -> bool: + # either: + # 1. RecvPackets is a Packet --> check isinstance on the type var + # 2. RecvPackets is a Union of Packets --> check isinstance on all + if get_origin(self._recv_T) is Union: + return isinstance(provided, get_args(self._recv_T)) + else: + return isinstance(provided, self._recv_T) + + def adjust_read_rate(self, rate: float) -> None: + """ + Sets the reading rate to a specified amount. + + Arguments: + rate (float): The reading speed to use, in hz. + """ + self.rate = min(rate, 1_000) + self.buff_thread.set_hz(rate) + + def scale_read_rate(self, scale: float) -> None: + """ + Scales the reading rate of the device handle by some factor. + + Arguments: + scale (float): The amount to scale the reading rate by. + """ + self.adjust_read_rate(self.rate * scale) + + def _read_packet(self) -> bool: + if not self.device: + raise RuntimeError("Device is not connected.") + try: + with self.lock: + if not self.is_open() or self.device.in_waiting == 0: + return False + if self.device.in_waiting > 200: + logger.warn( + "Packets are coming in much quicker than expected, upping rate...", + ) + self.scale_read_rate(1 + self.device.in_waiting / 1000) + packed_packet = self._read_from_stream() + assert isinstance(packed_packet, bytes) + packet = Packet.from_bytes(packed_packet) + except serial.SerialException as e: + logger.error(f"Error reading packet: {e}") + return False + except OSError: + logger.error("Cannot read from serial device.") + return False + if not self._correct_type(packet): + logger.error( + f"Received unexpected packet: {packet} (expected: {self._recv_T})", + ) + return False + packet = cast(RecvPackets, packet) + self.on_packet_received(packet) + return True + + def _process_buffer(self) -> None: + if not self.is_open(): + return + try: + self._read_packet() + except Exception as e: + logger.error(f"Error reading recent packet: {e}") + import traceback + + traceback.print_exc() + + @abc.abstractmethod + def on_packet_received(self, packet: RecvPackets) -> None: + """ + Abstract method to be implemented by subclasses for handling packets + sent by the physical electrical board. + + Arguments: + packet (:class:`~.Packet`): The packet that is received. + """ + pass diff --git a/mil_common/drivers/electrical_protocol/electrical_protocol/packet.py b/mil_common/drivers/electrical_protocol/electrical_protocol/packet.py index d69ed073b..dde372b87 100644 --- a/mil_common/drivers/electrical_protocol/electrical_protocol/packet.py +++ b/mil_common/drivers/electrical_protocol/electrical_protocol/packet.py @@ -112,17 +112,15 @@ def __init_subclass__( def __post_init__(self): for name, field_type in get_cache_hints(self.__class__).items(): - if ( - name - not in [ - "class_id", - "subclass_id", - "payload_format", - ] - and not isinstance(self.__dict__[name], field_type) - and issubclass(field_type, Enum) - ): - setattr(self, name, field_type(self.__dict__[name])) + if name not in [ + "class_id", + "subclass_id", + "payload_format", + ] and not isinstance(self.__dict__[name], field_type): + if issubclass(field_type, Enum): + setattr(self, name, field_type(self.__dict__[name])) + elif issubclass(field_type, str): + setattr(self, name, self.__dict__[name].rstrip(b"\x00").decode()) if self.payload_format and not self.payload_format.startswith( ("<", ">", "=", "!"), ): @@ -163,7 +161,21 @@ def _calculate_checksum(cls, data: bytes) -> tuple[int, int]: return sum1, sum2 def __bytes__(self): - payload = struct.pack(self.payload_format, *self.__dict__.values()) + ready_values = [] + for value in self.__dict__.values(): + if isinstance(value, Enum): + ready_values.append( + ( + value.value + if not isinstance(value.value, str) + else value.value.encode() + ), + ) + elif isinstance(value, str): + ready_values.append(value.encode()) + else: + ready_values.append(value) + payload = struct.pack(self.payload_format, *ready_values) data = struct.pack( f" None: - self.answer_topic.publish(Float32(data=packet.result)) - self.next_packet.set() + if isinstance(packet, AnswerPacket): + self.answer_one_topic.publish(Float32(data=packet.result)) + self.next_packet.set() + elif isinstance(packet, EnumPacket): + self.answer_two_topic.publish(Float32(data=packet.number.value)) if __name__ == "__main__": diff --git a/mil_common/drivers/electrical_protocol/test/calculator_device_noros.py b/mil_common/drivers/electrical_protocol/test/calculator_device_noros.py new file mode 100755 index 000000000..ffdae7314 --- /dev/null +++ b/mil_common/drivers/electrical_protocol/test/calculator_device_noros.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +from dataclasses import dataclass +from threading import Event +from typing import Union + +import rospy +from electrical_protocol import Packet +from electrical_protocol.driver_noros import ROSSerialDevice +from std_msgs.msg import Float32, String +from std_srvs.srv import Empty, EmptyRequest, EmptyResponse + + +@dataclass +class RequestAddPacket(Packet, class_id=0x37, subclass_id=0x00, payload_format=" None: + self.answer_topic.publish(Float32(data=packet.result)) + self.next_packet.set() + + +if __name__ == "__main__": + rospy.init_node("calculator_device") + device = CalculatorDevice() + rospy.on_shutdown(device.close) + rospy.spin() diff --git a/mil_common/drivers/electrical_protocol/test/noros.test b/mil_common/drivers/electrical_protocol/test/noros.test new file mode 100644 index 000000000..0f85ba523 --- /dev/null +++ b/mil_common/drivers/electrical_protocol/test/noros.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/mil_common/drivers/electrical_protocol/test/test_simulated_basic.py b/mil_common/drivers/electrical_protocol/test/test_simulated_basic.py index c722cd420..0151bde82 100755 --- a/mil_common/drivers/electrical_protocol/test/test_simulated_basic.py +++ b/mil_common/drivers/electrical_protocol/test/test_simulated_basic.py @@ -4,6 +4,7 @@ import pty import unittest from dataclasses import dataclass +from enum import Enum, IntEnum import rospy import rostest @@ -12,6 +13,18 @@ from std_srvs.srv import Empty +class CalculatorMode(IntEnum): + ADD = 0 + SUB = 1 + MUL = 2 + + +class Sign(Enum): + PLUS = "+" + MINUS = "-" + MULTIPLY = "*" + + @dataclass class RequestAddPacket(Packet, class_id=0x37, subclass_id=0x00, payload_format=" 2: + rospy.logerr(f"JSONDecodeError: {e}") + except KeyError as e: + rospy.logerr(f"Key Error: {e}") + + +if __name__ == "__main__": + with contextlib.suppress(rospy.ROSInterruptException): + rospy.init_node("pingpublisher", anonymous=True) + main() diff --git a/mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh b/mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh new file mode 100755 index 000000000..cff851f10 --- /dev/null +++ b/mil_common/drivers/mil_passive_sonar/src/fakeping_pipeline.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +sdgps generate-fake-sonar-solution --rate 1 --position '[-30, 10, -2]' --circle '{"radius":3,"axis":[0,0,1],"period":30}' ! sonar-simulate --role rover --transmit-pattern robosub-2019 --base-hydrophone-arrangement robosub-2019 --rover-hydrophone-arrangement uf-mil-sub7 --cn0 80 --multipath extreme ! filter-sonar-raw-samples --highpass 15e3 --lowpass 45e3 --notch 40e3 ! extract-robosub-pings ! robosub-ping-solver ! listen-robosub-ping-solution-tcp 2007 diff --git a/mil_common/drivers/mil_passive_sonar/src/pipeline.sh b/mil_common/drivers/mil_passive_sonar/src/pipeline.sh new file mode 100755 index 000000000..5cff2b143 --- /dev/null +++ b/mil_common/drivers/mil_passive_sonar/src/pipeline.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +sdgps connect-sonar-raw-tcp --host 192.168.37.61 --port 2006 ! filter-sonar-raw-samples --highpass 15e3 --lowpass 45e3 --notch 40e3 ! extract-robosub-pings ! robosub-ping-solver ! listen-robosub-ping-solution-tcp 2007 diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg b/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg index c0c5463ac..658a0bf15 100755 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg @@ -26,4 +26,8 @@ gen.add("ogrid_width_meters", double_t, 16, "", 1000, 1, 10000) gen.add("ogrid_resolution_meters_per_cell", double_t, 16, "", 0.3, 0., 10.) gen.add("ogrid_inflation_meters", double_t, 16, "", 2, 0., 10.) +# Intensity filter +gen.add("intensity_filter_min_intensity", double_t, 32, "", 10.0, 0., 1000.) +gen.add("intensity_filter_max_intensity", double_t, 32, "", 100.0, 0., 1000.) + exit(gen.generate("point_cloud_object_detection_and_recognition", "pcodar", "PCODAR")) diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp index 0e3c01f8a..8e64ac5d2 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp @@ -82,6 +82,10 @@ class NodeBase // Visualization MarkerManager marker_manager_; OgridManager ogrid_manager_; + + // Intensity filter + double intensity_filter_min_intensity; + double intensity_filter_max_intensity; }; class Node : public NodeBase @@ -99,6 +103,7 @@ class Node : public NodeBase private: bool bounds_update_cb(const mil_bounds::BoundsConfig& config) override; void ConfigCallback(Config const& config, uint32_t level) override; + void update_config(Config const& config); /// Reset PCODAR bool Reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) override; diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp index daed67565..310a94b3f 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp @@ -15,6 +15,8 @@ NodeBase::NodeBase(ros::NodeHandle _nh) , tf_listener(tf_buffer_, nh_) , global_frame_("enu") , config_server_(_nh) + , intensity_filter_min_intensity(10) + , intensity_filter_max_intensity(100) , objects_(std::make_shared()) { config_server_.setCallback(std::bind(&NodeBase::ConfigCallback, this, std::placeholders::_1, std::placeholders::_2)); @@ -99,12 +101,6 @@ bool NodeBase::transform_point_cloud(const sensor_msgs::PointCloud2& pc_msg, poi if (!transform_to_global(pc_msg.header.frame_id, pc_msg.header.stamp, transform)) return false; - // Transform from PCL2 - // pcl::PCLPointCloud2 pcl_pc2; - // pcl_conversions::toPCL(pc_msg, pcl_pc2); - // point_cloud_i pcloud; - // pcl::fromPCLPointCloud2(pcl_pc2, pcloud); - // Change pc_msg to a new point cloud point_cloud_i pcloud; pcl::fromROSMsg(pc_msg, pcloud); @@ -131,6 +127,12 @@ Node::Node(ros::NodeHandle _nh) : NodeBase(_nh) input_cloud_filter_.set_robot_footprint(min, max); } +void Node::update_config(Config const& config) +{ + this->intensity_filter_min_intensity = config.intensity_filter_min_intensity; + this->intensity_filter_max_intensity = config.intensity_filter_max_intensity; +} + void Node::ConfigCallback(Config const& config, uint32_t level) { NodeBase::ConfigCallback(config, level); @@ -142,6 +144,8 @@ void Node::ConfigCallback(Config const& config, uint32_t level) detector_.update_config(config); if (!level || level & 8) ass.update_config(config); + if (!level || level & 32) + this->update_config(config); } void Node::initialize() @@ -180,14 +184,14 @@ void Node::velodyne_cb(const sensor_msgs::PointCloud2ConstPtr& pcloud) if (!transform_point_cloud(*pcloud, *pc)) return; - // our new filter vvv - pcl::PassThrough _temp_intensity_filter; - _temp_intensity_filter.setInputCloud(pc); - _temp_intensity_filter.setFilterFieldName("intensity"); - _temp_intensity_filter.setFilterLimits(10, 100); + // Intensity filter + pcl::PassThrough _intensity_filter; + _intensity_filter.setInputCloud(pc); + _intensity_filter.setFilterFieldName("intensity"); + _intensity_filter.setFilterLimits(this->intensity_filter_min_intensity, this->intensity_filter_max_intensity); point_cloud_ptr pc_without_i = boost::make_shared(); point_cloud_i_ptr pc_i_filtered = boost::make_shared(); - _temp_intensity_filter.filter(*pc_i_filtered); + _intensity_filter.filter(*pc_i_filtered); pc_without_i->points.resize(pc_i_filtered->size()); for (size_t i = 0; i < pc_i_filtered->points.size(); i++) diff --git a/mil_common/ros_alarms/include/ros_alarms/listener.hpp b/mil_common/ros_alarms/include/ros_alarms/listener.hpp index f5b68e6ae..db7e19213 100644 --- a/mil_common/ros_alarms/include/ros_alarms/listener.hpp +++ b/mil_common/ros_alarms/include/ros_alarms/listener.hpp @@ -320,8 +320,11 @@ class AlarmListener template AlarmListener::AlarmListener(ros::NodeHandle &nh, std::string alarm_name) -try : __nh(nh), __alarm_name(alarm_name), __get_alarm(__nh.serviceClient("/alarm/get")), - __async_spinner(1, &__cb_queue) +try + : __nh(nh) + , __alarm_name(alarm_name) + , __get_alarm(__nh.serviceClient("/alarm/get")) + , __async_spinner(1, &__cb_queue) { std::stringstream obj_name; // For better error msgs diff --git a/scripts/install.sh b/scripts/install.sh index 75e50065d..fbef4dbbe 100755 --- a/scripts/install.sh +++ b/scripts/install.sh @@ -70,6 +70,9 @@ Fetching latest apt packages... $(hash_header) EOF +# Install neovim (to prevent cameron from going mad) +sudo apt-add-repository ppa:neovim-ppa/stable -y + # Update apt sudo apt update @@ -104,6 +107,7 @@ mil_system_install --no-install-recommends \ ruby \ wget \ vim \ + neovim \ expect \ doxygen \ doxygen-doc \ diff --git a/scripts/setup.bash b/scripts/setup.bash index fa3d442af..c56efc21f 100755 --- a/scripts/setup.bash +++ b/scripts/setup.bash @@ -91,15 +91,16 @@ alias gazebogui="rosrun gazebo_ros gzclient __name:=gzclient" # Preflight aliases alias preflight='python3 $MIL_REPO/mil_common/utils/mil_tools/scripts/mil-preflight/main.py' +alias ntmux='$MIL_REPO/NaviGator/scripts/tmux_start.sh' # Process killing aliases alias killgazebo="killall -9 gzserver && killall -9 gzclient" alias killros='$MIL_REPO/scripts/kill_ros.sh' alias killprocess='$MIL_REPO/scripts/kill_process.sh' -startxbox() { +xbox() { rosservice call /wrench/select "topic: '/wrench/rc'" - roslaunch navigator_launch shore.launch + roslaunch navigator_launch shore.launch device_input:="$1" } # catkin_make for one specific package only @@ -193,7 +194,18 @@ rossubconnect() { rosconnect "192.168.37.60" } -alias xbox=startxbox +prettycp() { + rsync --recursive --times --modify-window=2 --progress --verbose --itemize-changes --stats --human-readable "$1" "$2" +} + +mount_ssd() { + sudo mkdir -p /mnt/ssd + sudo mount -t exfat /dev/sda1 /mnt/ssd +} + +unmount_ssd() { + sudo umount /mnt/ssd +} # PYTHONPATH modifications export PYTHONPATH="${HOME}/catkin_ws/src/mil/mil_common/perception/vision_stack/src:${HOME}/catkin_ws/src/mil/mil_common/axros/axros/src:${PYTHONPATH}"