navigator_robotx_comms: Add UAV replenishment message for 2024, other…

… message fixups

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
 )

NaviGator/utils/navigator_msgs/package.xml

@@ -10,6 +10,7 @@
   <build_depend>actionlib</build_depend>
   <build_depend>actionlib_msgs</build_depend>
   <build_depend>actionlib_msgs</build_depend>
+  <build_depend>geographic_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>message_runtime</build_depend>
@@ -19,6 +20,7 @@
   <run_depend>actionlib</run_depend>
   <run_depend>actionlib_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>geographic_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>actionlib_msgs</run_depend>

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

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

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

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

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

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: