mil_usb_to_can.sub9: Use electrical_protocol packets for communicatio…

…n with sub9 USB to CAN board

SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py

@@ -2,8 +2,9 @@
 from __future__ import annotations
 
 import rospy
+from electrical_protocol import AckPacket, NackPacket
 from mil_misc_tools import rospy_to_datetime
-from mil_usb_to_can.sub9 import AckPacket, CANDeviceHandle, NackPacket
+from mil_usb_to_can.sub9 import CANDeviceHandle
 from ros_alarms import AlarmBroadcaster, AlarmListener
 from ros_alarms_msgs.msg import Alarm
 from rospy.timer import TimerEvent

SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py

@@ -1,18 +1,23 @@
 from dataclasses import dataclass
 from enum import IntEnum
 
-from mil_usb_to_can.sub9 import Packet
+from electrical_protocol import Packet
 
 
 @dataclass
-class HeartbeatSetPacket(Packet, msg_id=0x02, subclass_id=0x00, payload_format=""):
+class HeartbeatSetPacket(Packet, class_id=0x02, subclass_id=0x00, payload_format=""):
     """
     Heartbeat packet sent by the motherboard to the thrust/kill board.
     """
 
 
 @dataclass
-class HeartbeatReceivePacket(Packet, msg_id=0x02, subclass_id=0x01, payload_format=""):
+class HeartbeatReceivePacket(
+    Packet,
+    class_id=0x02,
+    subclass_id=0x01,
+    payload_format="",
+):
     """
     Heartbeat packet sent by the thrust/kill board to the motherboard.
     """
@@ -42,7 +47,7 @@ class ThrusterId(IntEnum):
 
 
 @dataclass
-class ThrustSetPacket(Packet, msg_id=0x02, subclass_id=0x02, payload_format="<Bf"):
+class ThrustSetPacket(Packet, class_id=0x02, subclass_id=0x02, payload_format="<Bf"):
     """
     Packet to set the speed of a specific thruster.
 
@@ -74,7 +79,7 @@ class KillStatus(IntEnum):
 
 
 @dataclass
-class KillSetPacket(Packet, msg_id=0x02, subclass_id=0x03, payload_format="<BB"):
+class KillSetPacket(Packet, class_id=0x02, subclass_id=0x03, payload_format="<BB"):
     """
     Packet sent by the motherboard to set/unset the kill.
 
@@ -88,7 +93,7 @@ class KillSetPacket(Packet, msg_id=0x02, subclass_id=0x03, payload_format="<BB")
 
 
 @dataclass
-class KillReceivePacket(Packet, msg_id=0x02, subclass_id=0x04, payload_format="<BB"):
+class KillReceivePacket(Packet, class_id=0x02, subclass_id=0x04, payload_format="<BB"):
     """
     Packet sent by the motherboard to set/unset the kill.
 

SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py

@@ -2,7 +2,8 @@
 from __future__ import annotations
 
 import rospy
-from mil_usb_to_can.sub9 import AckPacket, NackPacket, SimulatedCANDeviceHandle
+from electrical_protocol import AckPacket, NackPacket
+from mil_usb_to_can.sub9 import SimulatedCANDeviceHandle
 from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse
 
 from .packets import (

SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py

@@ -2,7 +2,8 @@
 from __future__ import annotations
 
 import rospy
-from mil_usb_to_can.sub9 import AckPacket, CANDeviceHandle
+from electrical_protocol import AckPacket
+from mil_usb_to_can.sub9 import CANDeviceHandle
 
 from sub_actuator_board.srv import (
     GetValve,

SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py

@@ -1,7 +1,7 @@
 from dataclasses import dataclass
 from enum import IntEnum
 
-from mil_usb_to_can.sub9 import Packet
+from electrical_protocol import Packet
 
 
 class ActuatorPacketId(IntEnum):
@@ -18,7 +18,7 @@ class ActuatorPacketId(IntEnum):
 
 
 @dataclass
-class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="<BB"):
+class ActuatorSetPacket(Packet, class_id=0x03, subclass_id=0x00, payload_format="<BB"):
     """
     Packet used by the actuator board to set a specific valve.
 
@@ -35,7 +35,7 @@ class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="<
 @dataclass
 class ActuatorPollRequestPacket(
     Packet,
-    msg_id=0x03,
+    class_id=0x03,
     subclass_id=0x01,
     payload_format="",
 ):
@@ -47,9 +47,9 @@ class ActuatorPollRequestPacket(
 @dataclass
 class ActuatorPollResponsePacket(
     Packet,
-    msg_id=0x03,
+    class_id=0x03,
     subclass_id=0x02,
-    payload_format="B",
+    payload_format="<B",
 ):
     """
     Packet used by the actuator board to return the status of all valves.

SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py

@@ -4,7 +4,8 @@
 import random
 
 import rospy
-from mil_usb_to_can.sub9 import AckPacket, SimulatedCANDeviceHandle
+from electrical_protocol import AckPacket
+from mil_usb_to_can.sub9 import SimulatedCANDeviceHandle
 
 from .packets import (
     ActuatorPollRequestPacket,

mil_common/drivers/electrical_protocol/electrical_protocol/packet.py

@@ -1,7 +1,7 @@
 from __future__ import annotations
 
 import struct
-from dataclasses import dataclass
+from dataclasses import dataclass, fields
 from enum import Enum
 from functools import lru_cache
 from typing import ClassVar, TypeVar, get_type_hints
@@ -123,6 +123,31 @@ def __post_init__(self):
                 and issubclass(field_type, Enum)
             ):
                 setattr(self, name, field_type(self.__dict__[name]))
+        if self.payload_format and not self.payload_format.startswith(
+            ("<", ">", "=", "!"),
+        ):
+            raise ValueError(
+                "The payload format does not start with a standard size character: ('<', '>', '!', '=').",
+            )
+        available_chars: dict[type, list[str]] = {
+            bool: ["c", "b", "B", "?"],
+            int: ["b", "B", "h", "H", "i", "I", "l", "L", "q", "Q"],
+            float: ["f", "d"],
+        }
+        stripped_format = self.payload_format.lstrip("<>=!@")
+        for i, field in enumerate(fields(self)):
+            if field.type not in available_chars:
+                continue
+            chars = available_chars[field.type]
+            if i >= len(stripped_format):
+                raise ValueError(
+                    f"The payload format for the packet is too short to support all dataclass fields; expected: {len(fields(self))}, found: {len(self.payload_format)}.",
+                )
+            represented_char = stripped_format[i]
+            if represented_char not in chars:
+                raise ValueError(
+                    f"The type of {field.name} in the payload format is '{represented_char}', which does not correspond to its dataclass type of {field.type}.",
+                )
 
     @classmethod
     def _calculate_checksum(cls, data: bytes) -> tuple[int, int]:
@@ -147,11 +172,12 @@ def __bytes__(self):
         return data + struct.pack("<BB", *checksum)
 
     def __len__(self) -> int:
-        return struct.calcsize(f"<BBBBH{self.payload_format}BB")
+        return self.__class__._expected_len()
 
     @classmethod
     def _expected_len(cls) -> int:
-        return struct.calcsize(f"<BBBBH{cls.payload_format}BB")
+        # We cannot use one calcsize since payload_format should start with a standard size character
+        return struct.calcsize("<BBBBHBB") + struct.calcsize(cls.payload_format)
 
     @classmethod
     def from_bytes(

mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/__init__.py

@@ -1,2 +1 @@
 from .device import CANDeviceHandle, SimulatedCANDeviceHandle
-from .packet import AckPacket, ChecksumException, NackPacket, Packet

mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py

@@ -2,8 +2,9 @@
 
 from typing import TYPE_CHECKING
 
+from electrical_protocol import Packet
+
 if TYPE_CHECKING:
-    from .packet import Packet
     from .sub9_driver import SimulatedUSBtoCANStream, USBtoCANDriver
 
 

mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py

@@ -3,39 +3,39 @@
 from dataclasses import dataclass
 
 import rospy
+from electrical_protocol import Packet
 from rospy_tutorials.srv import AddTwoInts
 from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
 
 from .device import CANDeviceHandle, SimulatedCANDeviceHandle
-from .packet import Packet
 
 
 @dataclass
 class ExampleEchoRequestPacket(
     Packet,
-    msg_id=0x99,
+    class_id=0x99,
     subclass_id=0x00,
-    payload_format="10s",
+    payload_format="<10s",
 ):
     my_special_string: bytes
 
 
 @dataclass
 class ExampleEchoResponsePacket(
     Packet,
-    msg_id=0x99,
+    class_id=0x99,
     subclass_id=0x01,
-    payload_format="10s",
+    payload_format="<10s",
 ):
     my_special_string: bytes
 
 
 @dataclass
 class ExampleAdderRequestPacket(
     Packet,
-    msg_id=0x99,
+    class_id=0x99,
     subclass_id=0x02,
-    payload_format="BB",
+    payload_format="<BB",
 ):
     num_one: int
     num_two: int
@@ -44,9 +44,9 @@ class ExampleAdderRequestPacket(
 @dataclass
 class ExampleAdderResponsePacket(
     Packet,
-    msg_id=0x99,
+    class_id=0x99,
     subclass_id=0x03,
-    payload_format="B",
+    payload_format="<B",
 ):
     response: int