diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt b/NaviGator/hardware_drivers/navigator_drone_comm/CMakeLists.txt index 7672c7baf..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() @@ -12,3 +14,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..f5ee1fda0 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,10 +1,21 @@ +""" +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 from .packets import ( Color, EStopPacket, GPSDronePacket, HeartbeatReceivePacket, HeartbeatSetPacket, + Logo, StartPacket, StopPacket, TargetPacket, + TinPacket, ) 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..abad98680 --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/driver.py @@ -0,0 +1,173 @@ +import threading +from typing import Union + +import rospy +from electrical_protocol import ROSSerialDevice +from geographic_msgs.msg import GeoPoint +from geometry_msgs.msg import Point +from navigator_msgs.msg import DroneTarget, DroneTin +from navigator_msgs.srv import ( + DroneMission, + DroneMissionRequest, + MessageUAVReplenishment, + MessageUAVReplenishmentRequest, + MessageUAVSearchReport, + MessageUAVSearchReportRequest, +) +from std_msgs.msg import Int8 +from std_srvs.srv import Empty, EmptyRequest + +from .packets import ( + EStopPacket, + GPSDronePacket, + HeartbeatReceivePacket, + HeartbeatSetPacket, + StartPacket, + StopPacket, + TargetPacket, + TinPacket, +) + +SendPackets = Union[HeartbeatSetPacket, EStopPacket, StartPacket, StopPacket] +RecvPackets = Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket, TinPacket] + + +class DroneCommDevice( + ROSSerialDevice[SendPackets, RecvPackets], +): + 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, + ) # TODO replace with service call + self.target_msg = {} + self.tin_pub = rospy.Publisher("~tin", DroneTin, queue_size=5) + self.drone_heartbeat_pub = rospy.Publisher( + "~drone_heartbeat", + Int8, + queue_size=10, + ) + self.received_seq_num = 0 + self.num_logos = 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) + # self.boat_heartbeat_timer = rospy.Timer(rospy.Duration(1), self.heartbeat_send) + # self.heartbeat_service = self.nh.get_service_client( + # "" + # ) + 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()) + pass + + 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") + pass + + def estop_send(self): + self.send_packet(EStopPacket()) + + def start_send(self, mission_name: str): + self.send_packet(StartPacket(name=mission_name)) + + def stop_send(self): + self.send_packet(StopPacket()) + + def on_packet_received( + self, + packet: RecvPackets, + ): + if isinstance(packet, HeartbeatReceivePacket): + rospy.loginfo("status %s", packet.status) + self.drone_heartbeat_event.set() + hbt_msg = Int8(packet.status) + # hbt_msg = Header(self.received_seq_num, rospy.Time.now(), "drone_heartbeat") # TODO: publish int + # 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) + elif isinstance(packet, TargetPacket): + rospy.loginfo(str(packet)) + self.target_msg[self.num_logos] = [ + packet.lat, + packet.lon, + packet.logo.value, + ] + self.num_logos += 1 + rospy.loginfo(str(self.target_msg)) + # self.target_pub.publish(target_msg) + if self.num_logos > 1: + drone_target_proxy = rospy.ServiceProxy( + "uav_search_report_message", + MessageUAVSearchReport, + ) + drone_target_proxy.wait_for_service() + try: + drone_msg = MessageUAVSearchReportRequest() + drone_msg.object1 = self.target_msg[0][2] + drone_msg.object2 = self.target_msg[1][2] + drone_msg.uav_status = 2 + point1 = GeoPoint() + point1.latitude = self.target_msg[0][0] + point1.longitude = self.target_msg[0][1] + point1.altitude = 0.0 + point2 = GeoPoint() + point2.latitude = self.target_msg[1][0] + point2.longitude = self.target_msg[1][1] + point2.altitude = 0.0 + drone_msg.object1_location = point1 + drone_msg.object2_location = point2 + rospy.loginfo(str(drone_msg)) + response = drone_target_proxy(drone_msg) + rospy.loginfo(response) + except Exception: + pass + elif isinstance(packet, TinPacket): + rospy.wait_for_service("uav_replenishment_message") + try: + uav_replenish_msg = MessageUAVReplenishmentRequest() + uav_replenish_msg.uav_status = 2 + uav_replenish_msg.item_status = packet.status + uav_replenish_msg.item_color = packet.color.name[0] + + rospy.loginfo(str(uav_replenish_msg)) + drone_tin_proxy = rospy.ServiceProxy( + "uav_replenishment_message", + MessageUAVReplenishment, + ) + response = drone_tin_proxy(uav_replenish_msg) + rospy.loginfo(response) + except Exception: + pass + else: + rospy.logerr("Received unexpected packet type from drone") + return diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/packets.py b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/packets.py index a60003dd3..e092a0722 100644 --- a/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/packets.py +++ b/NaviGator/hardware_drivers/navigator_drone_comm/navigator_drone_comm/packets.py @@ -9,12 +9,17 @@ class HeartbeatReceivePacket( Packet, class_id=0x20, subclass_id=0x00, - payload_format="", + payload_format="TODO catkin + message_generation rospy rospy + message_runtime rospy 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..abcf17ce0 --- /dev/null +++ b/NaviGator/hardware_drivers/navigator_drone_comm/test/test_simulated_drone.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 + +import os +import pty +import time +import unittest + +import rospy +import rostest +from geometry_msgs.msg import Point +from navigator_drone_comm.driver import DroneCommDevice +from navigator_drone_comm.packets import ( + EStopPacket, + GPSDronePacket, + HeartbeatReceivePacket, + StartPacket, + StopPacket, +) +from navigator_msgs.msg import DroneTarget +from navigator_msgs.srv import DroneMission, DroneMissionRequest +from std_msgs.msg import Int8 +from std_srvs.srv import Empty, EmptyRequest + + +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() + 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() + 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", Int8, cls.heartbeat_drone_callback) + + 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("STARTB")) + # 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 + pass + self.assertIsInstance(packet, StartPacket) + + def test_gps_drone_receive(self): + 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) + + # Disabling this test because it was changed at competition and shouldn't work anymore! + # def test_target_receive(self): + # target_packet = TargetPacket(lat=-67.7745, lon=12.654, logo="R") + # os.write(self.master, bytes(target_packet)) + # rospy.sleep(1) + # rospy.loginfo(self.target.logo) + # self.assertEqual(self.target.logo, "R_LOGO") + + # 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) + + # test that the boat can receive drone heartbeats + def test_z_heartbeat_receive(self): + for i in range(3): + heartbeat_packet = HeartbeatReceivePacket(status=1) + self.device.on_packet_received(heartbeat_packet) + time.sleep(1) + self.assertEqual(self.last_drone_heartbeat.data, 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 dffa1e0f2..35e01226f 100644 --- a/NaviGator/utils/navigator_msgs/CMakeLists.txt +++ b/NaviGator/utils/navigator_msgs/CMakeLists.txt @@ -29,6 +29,8 @@ add_message_files( Hosts.msg ColoramaDebug.msg ScanTheCode.msg + DroneTarget.msg + DroneTin.msg ) add_service_files( @@ -64,6 +66,7 @@ add_service_files( TwoClosestCones.srv # 2024 RobotX service files BallLauncherDrops.srv + DroneMission.srv ) add_action_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..8b1626945 --- /dev/null +++ b/NaviGator/utils/navigator_msgs/msg/DroneTarget.msg @@ -0,0 +1,3 @@ +float64 lat +float64 lon +string logo diff --git a/NaviGator/utils/navigator_msgs/msg/DroneTin.msg b/NaviGator/utils/navigator_msgs/msg/DroneTin.msg new file mode 100644 index 000000000..8ff593dda --- /dev/null +++ b/NaviGator/utils/navigator_msgs/msg/DroneTin.msg @@ -0,0 +1,2 @@ +int8 status +string color 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 +--- diff --git a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py index 15abcca72..cecb492cc 100755 --- a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py +++ b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py @@ -51,7 +51,7 @@ ) from ros_alarms import AlarmListener from ros_alarms_msgs.msg import Alarm -from std_msgs.msg import String +from std_msgs.msg import Int8, String lock = threading.Lock() @@ -126,6 +126,11 @@ def __init__(self): rospy.Subscriber("odom", Odometry, self.gps_odom_callback) rospy.Subscriber("/wrench/selected", String, self.wrench_callback) rospy.Subscriber("/scan_the_code", ScanTheCode, self.scan_the_code_callback) + rospy.Subscriber( + "/navigator_drone_comm/drone_heartbeat", + Int8, + self.uav_status_callback, + ) # TODO: setup subscriber for uav status callback update @@ -211,11 +216,11 @@ def gps_odom_callback(self, odom: Odometry): """ self.odom = odom - def uav_status_callback(self, uav_status: int): + def uav_status_callback(self, uav_status: Int8): """ Stores the most recent AUV status experienced by the boat. """ - self.uav_status = uav_status + self.uav_status = uav_status.data def system_mode_callback(self, system_mode: int): """ 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(