diff --git a/NaviGator/utils/navigator_msgs/CMakeLists.txt b/NaviGator/utils/navigator_msgs/CMakeLists.txt index a40f8917c..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 @@ -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..bfd3fcabc 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 +int8 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/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 index 60ad9f6d4..5ce11f5ad 100644 --- a/NaviGator/utils/navigator_msgs/srv/MessageWildlifeEncounter.srv +++ b/NaviGator/utils/navigator_msgs/srv/MessageWildlifeEncounter.srv @@ -1,3 +1,5 @@ -string[] buoy_array # list of buoys (R,G,B), up to three buoys +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/navigator_robotx_comms.py b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py index 67384e6bd..31e66e670 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 ( @@ -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. @@ -73,10 +76,10 @@ def to_string( delim: str, team_id: str, edt_date_time: Any, - gps_array: Optional[Any], - odom: Optional[Odometry], - uav_status: Optional[int], - system_mode: Optional[int], + gps_array: Any, + odom: Odometry | None, + uav_status: int | None, + system_mode: int | None, use_test_data: bool, ) -> str: """ @@ -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. @@ -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. @@ -331,7 +334,7 @@ class RobotXWildlifeEncounterMessage: 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. @@ -411,7 +414,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. @@ -484,7 +487,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. @@ -561,7 +564,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. @@ -638,7 +641,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. @@ -715,7 +718,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,6 +735,14 @@ 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, @@ -758,7 +769,10 @@ def to_string( str: The encoded message. """ - data = f"{self.message_id}{delim}{edt_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: