1299 alexj boat drone comms (#1304)

* wrote navigator_drone_comm driver and simulated tests

* fixing imports

* Specify default param for radio port

* wrote navigator_drone_comm driver and simulated tests

* fixing imports

* Specify default param for radio port

* Received messages from drone publish to rostopics

* Made tests check received packets

* matches drone more, tin + target + status

* bug fix with import dependency

* working alex laptop

* boat time

* navigator_drone_comm: stylistic changes

* navigator_drone_comm: disable logo test

* fixing docs build error

---------

Co-authored-by: Cameron Brown <[email protected]>

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()

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,
 )

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

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="<i",
 ):
     """
     Heartbeat packet sent from the drone.
+
+    Attributes:
+        status (int): The status of the drone (1=stowed, 2=deloyed, 3=faulted)
     """
 
+    status: int
+
 
 @dataclass
 class HeartbeatSetPacket(Packet, class_id=0x20, subclass_id=0x01, payload_format=""):
@@ -75,6 +80,29 @@ class Color(Enum):
     RED = "r"
 
 
+class Logo(Enum):
+    """
+    Whether the drone detected the "R" logo pad, or the "N" logo pad.
+    """
+
+    R_LOGO = "R"
+    N_LOGO = "N"
+
+
+@dataclass
+class TinPacket(Packet, class_id=0x20, subclass_id=0x07, payload_format="<ic"):
+    """
+    Tin packet and status of tin
+
+    Attributes:
+        status(int): status of drone for the tin
+        color(Color): color of tin
+    """
+
+    status: int
+    color: Color
+
+
 @dataclass
 class TargetPacket(Packet, class_id=0x20, subclass_id=0x06, payload_format="<ffc"):
     """
@@ -83,9 +111,9 @@ class TargetPacket(Packet, class_id=0x20, subclass_id=0x06, payload_format="<ffc
     Attributes:
         lat (float): The latitude of the target.
         lon (float): The longitude of the target.
-        color (Color): The color of the target.
+        logo (Logo): The logo of the target.
     """
 
     lat: float
     lon: float
-    color: Color
+    logo: Logo

NaviGator/hardware_drivers/navigator_drone_comm/nodes/driver.py

@@ -0,0 +1,14 @@
+#! /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",
+            "/dev/serial/by-id/usb-FTDI_TTL232R-3V3_FTDCKG37-if00-port0",
+        ),
+    )
+    device = DroneCommDevice(port)
+    rospy.spin()

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/simulated.test

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <param name="/is_simulation" value="True" />
+    <test pkg="navigator_drone_comm" test-name="test_simulated_drone" type="test_simulated_drone.py" />
+</launch>