From 42601193721efc482d0ae8c81a8b069e7e771f0b Mon Sep 17 00:00:00 2001 From: Alex Johnson Date: Sun, 3 Nov 2024 22:23:39 +0000 Subject: [PATCH] Received messages from drone publish to rostopics --- .../navigator_drone_comm/CMakeLists.txt | 2 + .../navigator_drone_comm/driver.py | 24 ++++- .../navigator_drone_comm/package.xml | 2 + .../test/test_simulated_drone.py | 89 ++++++++++++++++--- NaviGator/utils/navigator_msgs/CMakeLists.txt | 1 + .../utils/navigator_msgs/msg/DroneTarget.msg | 3 + .../electrical_protocol/packet.py | 8 +- 7 files changed, 113 insertions(+), 16 deletions(-) create mode 100644 NaviGator/utils/navigator_msgs/msg/DroneTarget.msg diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt b/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt index 1a127acb1..5bd0c7b10 100644 --- a/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt +++ b/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() diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/driver.py b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/driver.py index f2bb6d2e7..16bbbe52d 100644 --- a/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/driver.py +++ b/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 diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/package.xml b/NaviGator/hardware_drivers/navigator_drone_comm/package.xml index 379e124db..0427fae46 100644 --- a/NaviGator/hardware_drivers/navigator_drone_comm/package.xml +++ b/NaviGator/hardware_drivers/navigator_drone_comm/package.xml @@ -9,7 +9,9 @@ TODO catkin + message_generation rospy rospy + message_runtime rospy diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/test/test_simulated_drone.py b/NaviGator/hardware_drivers/navigator_drone_comm/test/test_simulated_drone.py index 05887fe69..be70b6054 100755 --- a/NaviGator/hardware_drivers/navigator_drone_comm/test/test_simulated_drone.py +++ b/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) diff --git a/NaviGator/utils/navigator_msgs/CMakeLists.txt b/NaviGator/utils/navigator_msgs/CMakeLists.txt index bcecf0405..69567174e 100644 --- a/NaviGator/utils/navigator_msgs/CMakeLists.txt +++ b/NaviGator/utils/navigator_msgs/CMakeLists.txt @@ -28,6 +28,7 @@ add_message_files( Hosts.msg ColoramaDebug.msg ScanTheCode.msg + DroneTarget.msg ) add_service_files( diff --git a/NaviGator/utils/navigator_msgs/msg/DroneTarget.msg b/NaviGator/utils/navigator_msgs/msg/DroneTarget.msg new file mode 100644 index 000000000..675aac0e1 --- /dev/null +++ b/NaviGator/utils/navigator_msgs/msg/DroneTarget.msg @@ -0,0 +1,3 @@ +float64 lat +float64 lon +string color diff --git a/mil_common/drivers/electrical_protocol/electrical_protocol/packet.py b/mil_common/drivers/electrical_protocol/electrical_protocol/packet.py index dde372b87..5d7e6516a 100644 --- a/mil_common/drivers/electrical_protocol/electrical_protocol/packet.py +++ b/mil_common/drivers/electrical_protocol/electrical_protocol/packet.py @@ -111,6 +111,7 @@ 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", @@ -118,7 +119,12 @@ def __post_init__(self): "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(