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
+string 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.
 
@@ -374,10 +377,7 @@ def to_string(
             str: The encoded message.
         """
 
-        data_ = f"{self.message_id}{delim}{edt_date_time}{delim}{team_id}{delim}{len(data.buoy_array)!s}"
-
-        for animal in data.buoy_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:
@@ -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.
 
@@ -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.
 
@@ -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.
 
@@ -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.
 
@@ -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,6 +732,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 +766,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:

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 (
@@ -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)
@@ -478,7 +485,9 @@ def test_wildlife_encounter_message(self):
         times_ran = 0
         self.server.connect()
         # data to test message with
-        buoy_array = ["R", "B", "G"]
+        circling_wildlife = "R"
+        clockwise = False
+        number_of_circles = 1
 
         rospy.wait_for_service("wildlife_encounter_message")
         send_robot_x_wildlife_encounter_message = rospy.ServiceProxy(
@@ -491,7 +500,11 @@ def test_wildlife_encounter_message(self):
         try:
             while not rospy.is_shutdown() and times_ran < self.number_of_iterations:
                 rx_data = None
-                send_robot_x_wildlife_encounter_message(buoy_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)
@@ -512,7 +525,7 @@ def test_wildlife_encounter_message(self):
                         final_checksum_string = hex_checksum + "\r\n"
                         self.assertEqual(
                             len(data_list),
-                            8,
+                            7,
                             "wildlife encounter message formatting incorrect",
                         )
                         if self.use_test_data is True:
@@ -563,26 +576,6 @@ def test_wildlife_encounter_message(self):
                                 final_checksum_string,
                                 "wildlife encounter message checksum incorrect",
                             )
-                            self.assertEqual(
-                                int(data_list[4]),
-                                len(buoy_array),
-                                "buoy array length incorrect",
-                            )
-
-                            for i, buoy in enumerate(buoy_array):
-                                if i != len(buoy_array) - 1:
-                                    self.assertEqual(
-                                        data_list[5 + i],
-                                        buoy,
-                                        "buoy incorrect",
-                                    )
-                                else:
-                                    buoy_ = data_list[5 + i].split("*")[0]
-                                    self.assertEqual(
-                                        buoy,
-                                        buoy_,
-                                        "buoy 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: