From 7f4e3331c8b214ce6ddbe350e89f2f4e94870017 Mon Sep 17 00:00:00 2001 From: Alex Johnson Date: Wed, 30 Oct 2024 23:37:36 +0000 Subject: [PATCH] wrote navigator_drone_comm driver and simulated tests --- .../navigator_drone_comm/CMakeLists.txt | 5 ++ .../navigator_drone_comm/__init__.py | 1 + .../navigator_drone_comm/driver.py | 87 +++++++++++++++++++ .../navigator_drone_comm/nodes/driver.py | 9 ++ .../navigator_drone_comm/test/simulated.test | 5 ++ .../test/test_simulated_drone.py | 66 ++++++++++++++ NaviGator/utils/navigator_msgs/CMakeLists.txt | 1 + .../utils/navigator_msgs/srv/DroneMission.srv | 2 + 8 files changed, 176 insertions(+) create mode 100644 NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/driver.py create mode 100644 NaviGator/hardware_drivers/navigator_drone_comm/nodes/driver.py create mode 100644 NaviGator/hardware_drivers/navigator_drone_comm/test/simulated.test create mode 100755 NaviGator/hardware_drivers/navigator_drone_comm/test/test_simulated_drone.py create mode 100644 NaviGator/utils/navigator_msgs/srv/DroneMission.srv diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt b/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt index 7672c7baf..1a127acb1 100644 --- a/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt +++ b/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt @@ -12,3 +12,8 @@ include_directories( # include ${catkin_INCLUDE_DIRS} ) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(test/simulated.test) +endif() 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 9c356af2d..7fe9db719 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,3 +1,4 @@ +from .driver import DroneCommDevice from .packets import ( Color, EStopPacket, 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 new file mode 100644 index 000000000..273b2bc85 --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/driver.py @@ -0,0 +1,87 @@ +import threading +from typing import Union + +import rospy +from electrical_protocol import ROSSerialDevice +from navigator_msgs.srv import DroneMission, DroneMissionRequest +from std_srvs.srv import Empty, EmptyRequest + +from navigator_drone_comm import ( + EStopPacket, + GPSDronePacket, + HeartbeatReceivePacket, + HeartbeatSetPacket, + StartPacket, + StopPacket, + TargetPacket, +) + + +class DroneCommDevice( + ROSSerialDevice[ + Union[HeartbeatSetPacket, EStopPacket, StartPacket, StopPacket], + Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket], + ], +): + def __init__(self, port: str): + super().__init__(port, 57600) + 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) + self.boat_heartbeat_timer = rospy.Timer(rospy.Duration(1), self.heartbeat_send) + self.drone_heartbeat_event = threading.Event() + self.drone_heartbeat_timer = rospy.Timer( + rospy.Duration(1), + self.heartbeat_check, + ) + rospy.loginfo("DroneCommDevice initialized") + + def estop(self, _: EmptyRequest): + self.estop_send() + return {} + + def start(self, request: DroneMissionRequest): + self.start_send(mission_name=request.mission) + return {} + + def stop(self, _: EmptyRequest): + self.stop_send() + return {} + + def heartbeat_send(self, _): + # rospy.loginfo("sending heartbeat") + self.send_packet(HeartbeatSetPacket()) + + def heartbeat_check(self, _): + passed_check = self.drone_heartbeat_event.wait(1) + if passed_check: + self.drone_heartbeat_event.clear() + else: + # self.stop_send() # Uncomment to stop drone if no heartbeat is received + rospy.logerr("No heartbeat received from drone") + + 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( + self, + packet: Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket], + ): + if isinstance(packet, HeartbeatReceivePacket): + self.drone_heartbeat_event.set() + elif isinstance(packet, GPSDronePacket): + rospy.loginfo("Received GPS packet: %s", packet) + elif isinstance(packet, TargetPacket): + rospy.loginfo("Received Target packet: %s", packet) + else: + rospy.logerr("Received unexpected packet type") + return diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/nodes/driver.py b/NaviGator/hardware_drivers/navigator_drone_comm/nodes/driver.py new file mode 100644 index 000000000..c7e91e5c4 --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/nodes/driver.py @@ -0,0 +1,9 @@ +#! /usr/bin/env python3 +import rospy +from navigator_drone_comm.driver import DroneCommDevice + +if __name__ == "__main__": + rospy.init_node("navigator_drone_comm") + port = str(rospy.get_param("~port")) + device = DroneCommDevice(port) + rospy.spin() diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/test/simulated.test b/NaviGator/hardware_drivers/navigator_drone_comm/test/simulated.test new file mode 100644 index 000000000..eb36faa90 --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/test/simulated.test @@ -0,0 +1,5 @@ + + + + + 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 new file mode 100755 index 000000000..05887fe69 --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/test/test_simulated_drone.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 + +import os +import pty +import time +import unittest + +import rospy +import rostest +from navigator_drone_comm.driver import DroneCommDevice +from navigator_drone_comm.packets import ( + GPSDronePacket, + HeartbeatReceivePacket, + TargetPacket, +) + + +class SimulatedBasicTest(unittest.TestCase): + def __init__(self, *args): + super().__init__(*args) + + @classmethod + def setUpClass(cls): + cls.master, cls.slave = pty.openpty() + serial_name = os.ttyname(cls.slave) + cls.device = DroneCommDevice(serial_name) + + def test_device_initialization(self): + self.assertIsNotNone(self.device) + + 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) + + def test_target_receive(self): + target_packet = TargetPacket(lat=-67.7745, lon=12.654, color="r") + self.device.on_packet_received(target_packet) + + def test_z_heartbeat_receive(self): + rospy.loginfo("Testing receiving heartbeats for 5 secs...") + for i in range(5): + 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) + os.close(cls.slave) + + +if __name__ == "__main__": + rospy.init_node("test_simulated_drone") + rostest.rosrun( + "navigator_drone_comm", + "test_simulated_drone", + SimulatedBasicTest, + ) + unittest.main() diff --git a/NaviGator/utils/navigator_msgs/CMakeLists.txt b/NaviGator/utils/navigator_msgs/CMakeLists.txt index 54c1c2ebf..bcecf0405 100644 --- a/NaviGator/utils/navigator_msgs/CMakeLists.txt +++ b/NaviGator/utils/navigator_msgs/CMakeLists.txt @@ -63,6 +63,7 @@ add_service_files( TwoClosestCones.srv # 2024 RobotX service files BallLauncherDrops.srv + DroneMission.srv ) add_action_files( diff --git a/NaviGator/utils/navigator_msgs/srv/DroneMission.srv b/NaviGator/utils/navigator_msgs/srv/DroneMission.srv new file mode 100644 index 000000000..dcc61851e --- /dev/null +++ b/NaviGator/utils/navigator_msgs/srv/DroneMission.srv @@ -0,0 +1,2 @@ +string mission +---