add suspend heartbeat monitoring packet

NaviGator/hardware_drivers/navigator_light_kill_board/navigator_light_kill_board/driver.py

@@ -6,14 +6,21 @@
 from electrical_protocol import AckPacket, NackPacket, ROSSerialDevice
 from ros_alarms import AlarmBroadcaster, AlarmListener
 from std_msgs.msg import String
+from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse
 
-from .packets import KillReceivePacket, KillSetPacket, KillStatus, SetMovementModePacket
+from .packets import (
+    KillReceivePacket,
+    KillSetPacket,
+    KillStatus,
+    SetMovementModePacket,
+    SuspendHeartbeatPacket,
+)
 
-SendPackets = Union[KillSetPacket, SetMovementModePacket]
+SendPackets = Union[KillSetPacket, SetMovementModePacket, SuspendHeartbeatPacket]
 RecvPackets = Union[KillReceivePacket, AckPacket, NackPacket]
 
 
-class LightKillBoardDevice(
+class KillLightBoardDevice(
     ROSSerialDevice[
         SendPackets,
         RecvPackets,
@@ -60,6 +67,13 @@ def __init__(self, port: str):
         self._wrench_sub = rospy.Subscriber("/wrench/selected", String, self._wrench_cb)
         self._prev_wrench = None
 
+        # Ignore missing heartbeat messages service
+        self._ignore_heartbeat_srv = rospy.Service(
+            "ignore_heartbeat",
+            SetBool,
+            self._ignore_heartbeat,
+        )
+
     def _wrench_cb(self, wrench: String):
         if self._prev_wrench != wrench.data:
             if wrench.data in self.REMOTE_CONTROL_WRENCH_STATES:
@@ -68,6 +82,17 @@ def _wrench_cb(self, wrench: String):
                 self.send_packet(SetMovementModePacket(True))
         self._prev_wrench = wrench.data
 
+    def _ignore_heartbeat(self, data: SetBoolRequest):
+        response = None
+        if data.data:
+            response = "Instructing RF Kill system to ignore missing heartbeat packets."
+            rospy.logwarn(response)
+        else:
+            response = "RF Kill system monitoring missing heartbeat packets once again."
+            rospy.loginfo(response)
+        self.send_packet(SuspendHeartbeatPacket(data.data))
+        return SetBoolResponse(True, response)
+
     def _network_kill_alarm_cb(self, alarm):
         """
         If the network loss alarm is raised, we want to trigger a software kill

NaviGator/hardware_drivers/navigator_light_kill_board/navigator_light_kill_board/packets.py

@@ -30,3 +30,21 @@ class SetMovementModePacket(
     payload_format="<?",
 ):
     autonomous: bool
+
+
+@dataclass
+class SuspendHeartbeatPacket(
+    Packet,
+    class_id=0x10,
+    subclass_id=0x03,
+    payload_format="<?",
+):
+    """
+    Tells the RF Kill board to suspend listening for heartbeats and killing the boat
+    if none are received.
+
+    Attributes:
+        ignore (bool): Whether to ignore missing heartbeat messages.
+    """
+
+    ignore: bool