From 389270e7e5cac16a852f7f43685a0a0dc21c3b4d Mon Sep 17 00:00:00 2001
From: Alex Johnson <alexjohnson@ufl.edu>
Date: Fri, 8 Nov 2024 06:41:11 +0000
Subject: [PATCH] matches drone more, tin + target + status

---
 .../navigator_drone_comm/__init__.py          |  1 +
 .../navigator_drone_comm/driver.py            | 35 +++++++++++++------
 .../navigator_drone_comm/packets.py           | 32 +++++++++++++++--
 .../navigator_drone_comm/nodes/driver.py      |  0
 .../test/test_simulated_drone.py              |  4 +--
 .../utils/navigator_msgs/msg/DroneTarget.msg  |  2 +-
 .../utils/navigator_msgs/msg/DroneTin.msg     |  2 ++
 7 files changed, 60 insertions(+), 16 deletions(-)
 mode change 100644 => 100755 NaviGator/hardware_drivers/navigator_drone_comm/nodes/driver.py
 create mode 100644 NaviGator/utils/navigator_msgs/msg/DroneTin.msg

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 9ad2cbe04..0e4ae33a4 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
@@ -15,4 +15,5 @@
     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
index 2ba1ac5b6..3ca360890 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
@@ -4,9 +4,9 @@
 import rospy
 from electrical_protocol import ROSSerialDevice
 from geometry_msgs.msg import Point
-from navigator_msgs.msg import DroneTarget
+from navigator_msgs.msg import DroneTarget, DroneTin
 from navigator_msgs.srv import DroneMission, DroneMissionRequest
-from std_msgs.msg import Header
+from std_msgs.msg import Int8
 from std_srvs.srv import Empty, EmptyRequest
 
 from .packets import (
@@ -17,29 +17,38 @@
     StartPacket,
     StopPacket,
     TargetPacket,
+    TinPacket,
 )
 
 
 class DroneCommDevice(
     ROSSerialDevice[
         Union[HeartbeatSetPacket, EStopPacket, StartPacket, StopPacket],
-        Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket],
+        Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket, TinPacket],
     ],
 ):
     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.target_pub = rospy.Publisher(
+            "~target",
+            DroneTarget,
+            queue_size=5,
+        )  # TODO replace with service call
+        self.tin_pub = rospy.Publisher("~tin", DroneTin, queue_size=5)
         self.drone_heartbeat_pub = rospy.Publisher(
             "~drone_heartbeat",
-            Header,
+            Int8,
             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)
-        self.boat_heartbeat_timer = rospy.Timer(rospy.Duration(1), self.heartbeat_send)
+        # 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),
@@ -84,20 +93,24 @@ def stop_send(self):
 
     def on_packet_received(
         self,
-        packet: Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket],
+        packet: Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket, TinPacket],
     ):
         if isinstance(packet, HeartbeatReceivePacket):
+            rospy.loginfo(packet.status)
             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
+            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):
-            target_msg = DroneTarget(packet.lat, packet.lon, packet.color.name)
+            target_msg = DroneTarget(packet.lat, packet.lon, packet.logo.name)
             self.target_pub.publish(target_msg)
+        elif isinstance(packet, TinPacket):
+            tin_msg = DroneTin(packet.status, packet.color)
+            self.tin_pub.publish(tin_msg)
         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..1fc758f97 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="<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):
+    """
+    R or N pad enum
+    """
+
+    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(Enum): color of tin
+    """
+
+    status: int
+    color: Color
+
+
 @dataclass
 class TargetPacket(Packet, class_id=0x20, subclass_id=0x06, payload_format="<ffc"):
     """
@@ -88,4 +116,4 @@ class TargetPacket(Packet, class_id=0x20, subclass_id=0x06, payload_format="<ffc
 
     lat: float
     lon: float
-    color: Color
+    logo: Logo
diff --git a/NaviGator/hardware_drivers/navigator_drone_comm/nodes/driver.py b/NaviGator/hardware_drivers/navigator_drone_comm/nodes/driver.py
old mode 100644
new mode 100755
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 d4fcbe74e..73dd6f183 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
@@ -10,7 +10,6 @@
 from geometry_msgs.msg import Point
 from navigator_drone_comm.driver import DroneCommDevice
 from navigator_drone_comm.packets import (
-    Color,
     EStopPacket,
     GPSDronePacket,
     HeartbeatReceivePacket,
@@ -115,9 +114,10 @@ def test_gps_drone_receive(self):
         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)
+        target_packet = TargetPacket(lat=-67.7745, lon=12.654, color="b")
         os.write(self.master, bytes(target_packet))
         rospy.sleep(0.5)
+        rospy.loginfo(self.target.color)
         self.assertEqual(self.target.color, "BLUE")
 
     # tests that a heartbeat is sent from the boat every second
diff --git a/NaviGator/utils/navigator_msgs/msg/DroneTarget.msg b/NaviGator/utils/navigator_msgs/msg/DroneTarget.msg
index 675aac0e1..8b1626945 100644
--- a/NaviGator/utils/navigator_msgs/msg/DroneTarget.msg
+++ b/NaviGator/utils/navigator_msgs/msg/DroneTarget.msg
@@ -1,3 +1,3 @@
 float64 lat
 float64 lon
-string color
+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..40631edc6
--- /dev/null
+++ b/NaviGator/utils/navigator_msgs/msg/DroneTin.msg
@@ -0,0 +1,2 @@
+int status
+string color