diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9a2bda6ad..566493875 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -51,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] 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 cdde70798..f4ec88287 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", "UNKNOWN"] diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml index 8c50bb395..39c0d39fa 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml @@ -12,8 +12,8 @@ cluster_min_points: 5 cluster_max_points: 1000 # Associator -associator_max_distance: 2.0 -associator_forget_unseen: false +associator_max_distance: 20.0 +associator_forget_unseen: true # Ogrid ogrid_height_meters: 500 @@ -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 5bcbc0521..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,6 +1,6 @@ - + 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_missions/navigator_missions/__init__.py b/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py index 7b7ecff0a..429f1706d 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py @@ -1,6 +1,7 @@ import mil_missions_core from . import pose_editor +from .autonomous_2024 import Autonomous2024 from .back_and_forth import BackAndForth from .circle import Circle from .circle_tower import CircleTower 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/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/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/docs/navigator/reference.rst b/docs/navigator/reference.rst index d85e375ab..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 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/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="()) { config_server_.setCallback(std::bind(&NodeBase::ConfigCallback, this, std::placeholders::_1, std::placeholders::_2)); @@ -125,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); @@ -136,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() @@ -171,14 +181,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/scripts/setup.bash b/scripts/setup.bash index 876478fb8..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 @@ -206,8 +207,6 @@ unmount_ssd() { sudo umount /mnt/ssd } -alias xbox=startxbox - # 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}"