diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/__init__.py b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/__init__.py index ee619ad46..9ad2cbe04 100644 --- a/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/__init__.py +++ b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/__init__.py @@ -1,5 +1,9 @@ """ The :mod:`navigator_drone_comm` module is used to communicate with the drone from Navigator. +Packets received from the drone are published to /navigator_drone_comm/target, /navigator_drone_comm/gps, +and /navigator_drone_comm/target. +Can send packets to the drone by calling the services /navigator_drone_comm/estop, /navigator_drone_comm/stop, +/navigator_drone_comm/start "example mission name". """ from .driver import DroneCommDevice 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 16bbbe52d..2ba1ac5b6 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 @@ -74,15 +74,12 @@ def heartbeat_check(self, _): pass def estop_send(self): - # rospy.loginfo("sending EStop") self.send_packet(EStopPacket()) def start_send(self, mission_name: str): - # rospy.loginfo(f"sending Start for mission: %s", mission_name) self.send_packet(StartPacket(name=mission_name)) def stop_send(self): - # rospy.loginfo("sending Stop") self.send_packet(StopPacket()) def on_packet_received( @@ -94,6 +91,7 @@ def on_packet_received( hbt_msg = Header(self.received_seq_num, rospy.Time.now(), "drone_heartbeat") self.drone_heartbeat_pub.publish(hbt_msg) self.received_seq_num += 1 + self.drone_heartbeat_pub.publish(hbt_msg) elif isinstance(packet, GPSDronePacket): point_msg = Point(packet.lat, packet.lon, packet.alt) self.gps_pub.publish(point_msg) 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 be70b6054..d4fcbe74e 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 @@ -7,6 +7,7 @@ import rospy import rostest +from geometry_msgs.msg import Point from navigator_drone_comm.driver import DroneCommDevice from navigator_drone_comm.packets import ( Color, @@ -18,7 +19,9 @@ StopPacket, TargetPacket, ) +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 @@ -26,6 +29,18 @@ class SimulatedBasicTest(unittest.TestCase): def __init__(self, *args): super().__init__(*args) + @classmethod + def gps_callback(cls, data): + cls.drone_gps = data + + @classmethod + def target_callback(cls, data): + cls.target = data + + @classmethod + def heartbeat_drone_callback(cls, data): + cls.last_drone_heartbeat = data + @classmethod def setUpClass(cls): cls.master, cls.slave = pty.openpty() @@ -46,6 +61,12 @@ def setUpClass(cls): DroneMission, ) cls.start_proxy.wait_for_service() + cls.drone_gps = None + cls.target = None + cls.last_drone_heartbeat = None + rospy.Subscriber("~gps", Point, cls.gps_callback) + rospy.Subscriber("~target", DroneTarget, cls.target_callback) + rospy.Subscriber("~drone_heartbeat", Header, cls.heartbeat_drone_callback) def test_device_initialization(self): self.assertIsNotNone(self.device) @@ -83,20 +104,21 @@ def test_start_mission(self): 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.assertEqual(self.drone_gps, None) + gps_packet = GPSDronePacket(lat=37.77, lon=-122.4194, alt=30.0) os.write(self.master, bytes(gps_packet)) rospy.sleep(0.5) + self.assertAlmostEqual(round(self.drone_gps.x, 3), 37.77) def test_target_receive(self): target_packet = TargetPacket(lat=-67.7745, lon=12.654, color=Color.BLUE) os.write(self.master, bytes(target_packet)) rospy.sleep(0.5) + self.assertEqual(self.target.color, "BLUE") # tests that a heartbeat is sent from the boat every second def test_z_sending_heartbeats(self): @@ -106,14 +128,14 @@ def test_z_sending_heartbeats(self): 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 + # test that the boat can receive drone heartbeats def test_z_heartbeat_receive(self): - rospy.loginfo("Testing receiving 2 heartbeats...") for i in range(3): heartbeat_packet = HeartbeatReceivePacket() self.device.on_packet_received(heartbeat_packet) time.sleep(1) + self.assertEqual(self.last_drone_heartbeat.frame_id, "drone_heartbeat") + self.assertEqual(self.last_drone_heartbeat.seq, i) @classmethod def tearDownClass(cls):