Received messages from drone publish to rostopics

NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt

@@ -3,6 +3,8 @@ project(navigator_drone_comm)
 
 find_package(catkin REQUIRED COMPONENTS
   rospy
+  geometry_msgs
+  std_msgs
 )
 
 catkin_python_setup()

NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/driver.py

@@ -3,7 +3,10 @@
 
 import rospy
 from electrical_protocol import ROSSerialDevice
+from geometry_msgs.msg import Point
+from navigator_msgs.msg import DroneTarget
 from navigator_msgs.srv import DroneMission, DroneMissionRequest
+from std_msgs.msg import Header
 from std_srvs.srv import Empty, EmptyRequest
 
 from .packets import (
@@ -25,6 +28,14 @@ class DroneCommDevice(
 ):
     def __init__(self, port: str):
         super().__init__(port, 57600)
+        self.gps_pub = rospy.Publisher("~gps", Point, queue_size=5)
+        self.target_pub = rospy.Publisher("~target", DroneTarget, queue_size=5)
+        self.drone_heartbeat_pub = rospy.Publisher(
+            "~drone_heartbeat",
+            Header,
+            queue_size=10,
+        )
+        self.received_seq_num = 0
         self.estop_service = rospy.Service("~estop", Empty, self.estop)
         self.start_service = rospy.Service("~start", DroneMission, self.start)
         self.stop_service = rospy.Service("~stop", Empty, self.stop)
@@ -51,6 +62,7 @@ def stop(self, _: EmptyRequest):
     def heartbeat_send(self, _):
         # rospy.loginfo("sending heartbeat")
         self.send_packet(HeartbeatSetPacket())
+        pass
 
     def heartbeat_check(self, _):
         passed_check = self.drone_heartbeat_event.wait(1)
@@ -59,6 +71,7 @@ def heartbeat_check(self, _):
         else:
             # self.stop_send() # Uncomment to stop drone if no heartbeat is received
             rospy.logerr("No heartbeat received from drone")
+            pass
 
     def estop_send(self):
         # rospy.loginfo("sending EStop")
@@ -78,10 +91,15 @@ def on_packet_received(
     ):
         if isinstance(packet, HeartbeatReceivePacket):
             self.drone_heartbeat_event.set()
+            hbt_msg = Header(self.received_seq_num, rospy.Time.now(), "drone_heartbeat")
+            self.drone_heartbeat_pub.publish(hbt_msg)
+            self.received_seq_num += 1
         elif isinstance(packet, GPSDronePacket):
-            rospy.loginfo("Received GPS packet: %s", packet)
+            point_msg = Point(packet.lat, packet.lon, packet.alt)
+            self.gps_pub.publish(point_msg)
         elif isinstance(packet, TargetPacket):
-            rospy.loginfo("Received Target packet: %s", packet)
+            target_msg = DroneTarget(packet.lat, packet.lon, packet.color.name)
+            self.target_pub.publish(target_msg)
         else:
-            rospy.logerr("Received unexpected packet type")
+            rospy.logerr("Received unexpected packet type from drone")
         return

NaviGator/hardware_drivers/navigator_drone_comm/package.xml

@@ -9,7 +9,9 @@
   <license>TODO</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
   <build_depend>rospy</build_depend>
   <build_export_depend>rospy</build_export_depend>
+  <exec_depend>message_runtime</exec_depend>
   <exec_depend>rospy</exec_depend>
 </package>

NaviGator/hardware_drivers/navigator_drone_comm/test/test_simulated_drone.py

@@ -9,10 +9,17 @@
 import rostest
 from navigator_drone_comm.driver import DroneCommDevice
 from navigator_drone_comm.packets import (
+    Color,
+    EStopPacket,
     GPSDronePacket,
     HeartbeatReceivePacket,
+    HeartbeatSetPacket,
+    StartPacket,
+    StopPacket,
     TargetPacket,
 )
+from navigator_msgs.srv import DroneMission, DroneMissionRequest
+from std_srvs.srv import Empty, EmptyRequest
 
 
 class SimulatedBasicTest(unittest.TestCase):
@@ -24,32 +31,90 @@ def setUpClass(cls):
         cls.master, cls.slave = pty.openpty()
         serial_name = os.ttyname(cls.slave)
         cls.device = DroneCommDevice(serial_name)
+        cls.estop_proxy = rospy.ServiceProxy(
+            "/test_simulated_drone/estop",
+            Empty,
+        )
+        cls.estop_proxy.wait_for_service()
+        cls.stop_proxy = rospy.ServiceProxy(
+            "/test_simulated_drone/stop",
+            Empty,
+        )
+        cls.stop_proxy.wait_for_service()
+        cls.start_proxy = rospy.ServiceProxy(
+            "/test_simulated_drone/start",
+            DroneMission,
+        )
+        cls.start_proxy.wait_for_service()
 
     def test_device_initialization(self):
         self.assertIsNotNone(self.device)
 
+    def test_estop(self):
+        self.estop_proxy(EmptyRequest())
+        # give up to 3 tries to hear the estop packet and not just heartbeat
+        for i in range(3):
+            try:
+                packet = EStopPacket.from_bytes(os.read(self.master, 8))
+                break
+            except RuntimeError:
+                # ignore heartbeat packets here
+                pass
+        self.assertIsInstance(packet, EStopPacket)
+
+    def test_stop(self):
+        self.stop_proxy(EmptyRequest())
+        # give up to 3 tries to hear the stop packet and not just heartbeat
+        for i in range(3):
+            try:
+                packet = StopPacket.from_bytes(os.read(self.master, 8))
+                break
+            except RuntimeError:
+                # ignore heartbeat packets here
+                pass
+        self.assertIsInstance(packet, StopPacket)
+
+    def test_start_mission(self):
+        self.start_proxy(DroneMissionRequest("mymission"))
+        # give up to 3 tries to hear the start packet and not just heartbeat
+        for i in range(3):
+            try:
+                packet = StartPacket.from_bytes(os.read(self.master, 28))
+                break
+            except RuntimeError:
+                # ignore heartbeat packets here
+                rospy.logerr()
+                pass
+        self.assertIsInstance(packet, StartPacket)
+
+    # TODO add asserts
     def test_gps_drone_receive(self):
         gps_packet = GPSDronePacket(lat=37.7749, lon=-122.4194, alt=30.0)
-        self.device.on_packet_received(gps_packet)
+        os.write(self.master, bytes(gps_packet))
+        rospy.sleep(0.5)
 
     def test_target_receive(self):
-        target_packet = TargetPacket(lat=-67.7745, lon=12.654, color="r")
-        self.device.on_packet_received(target_packet)
+        target_packet = TargetPacket(lat=-67.7745, lon=12.654, color=Color.BLUE)
+        os.write(self.master, bytes(target_packet))
+        rospy.sleep(0.5)
 
+    # tests that a heartbeat is sent from the boat every second
+    def test_z_sending_heartbeats(self):
+        start_time = time.time()
+        for i in range(1, 4):
+            packet = HeartbeatSetPacket.from_bytes(os.read(self.master, 8))
+            self.assertIsInstance(packet, HeartbeatSetPacket)
+            self.assertLess(time.time() - start_time, 1 * i + 0.1)
+
+    # TODO
+    # test that if a heartbeat is received every second, no error sounds
     def test_z_heartbeat_receive(self):
-        rospy.loginfo("Testing receiving heartbeats for 5 secs...")
-        for i in range(5):
+        rospy.loginfo("Testing receiving 2 heartbeats...")
+        for i in range(3):
             heartbeat_packet = HeartbeatReceivePacket()
             self.device.on_packet_received(heartbeat_packet)
             time.sleep(1)
 
-    def test_z_longrun(self):
-        rospy.loginfo("Starting long test (ctl+c to end)...")
-        start_time = time.time()
-        duration = 200
-        while time.time() - start_time < duration:
-            rospy.rostime.wallsleep(0.1)
-
     @classmethod
     def tearDownClass(cls):
         os.close(cls.master)

NaviGator/utils/navigator_msgs/CMakeLists.txt

@@ -28,6 +28,7 @@ add_message_files(
   Hosts.msg
   ColoramaDebug.msg
   ScanTheCode.msg
+  DroneTarget.msg
 )
 
 add_service_files(

NaviGator/utils/navigator_msgs/msg/DroneTarget.msg

@@ -0,0 +1,3 @@
+float64 lat
+float64 lon
+string color

mil_common/drivers/electrical_protocol/electrical_protocol/packet.py

@@ -111,14 +111,20 @@ def __init_subclass__(
         _packet_registry.setdefault(class_id, {})[subclass_id] = cls
 
     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):
                 if issubclass(field_type, Enum):
-                    setattr(self, name, field_type(self.__dict__[name]))
+                    value = (
+                        self.__dict__[name]
+                        if not isinstance(self.__dict__[name], bytes)
+                        else self.__dict__[name].decode()
+                    )
+                    setattr(self, name, field_type(value))
                 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(