From afeca539e268891d370ebdd024079f1f216e0e35 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Thu, 19 Jan 2023 18:42:48 -0500 Subject: [PATCH 01/38] Rewrite basic packets --- docs/reference/can.rst | 19 +++-- .../mil_usb_to_can/mil_usb_to_can/packet.py | 80 +++++++++++++++++++ 2 files changed, 91 insertions(+), 8 deletions(-) create mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py diff --git a/docs/reference/can.rst b/docs/reference/can.rst index 22d1f0cd4..8be592751 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -170,16 +170,19 @@ Packet .. autoclass:: mil_usb_to_can.Packet :members: -ReceivePacket -^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.ReceivePacket +Specific Packets +^^^^^^^^^^^^^^^^ + +NackPacket +~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.NackPacket -.. autoclass:: mil_usb_to_can.ReceivePacket +.. autoclass:: mil_usb_to_can.NackPacket :members: -CommandPacket -^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.CommandPacket +AckPacket +~~~~~~~~~ +.. attributetable:: mil_usb_to_can.AckPacket -.. autoclass:: mil_usb_to_can.CommandPacket +.. autoclass:: mil_usb_to_can.AckPacket :members: diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py new file mode 100644 index 000000000..db9b68ce8 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py @@ -0,0 +1,80 @@ +from __future__ import annotations + +import struct +from dataclasses import dataclass +from typing import ClassVar + + +@dataclass +class Packet: + """ + Represents one packet sent or received by a device handle communicating to/from + the USB to CAN board. This class is able to handle packaging unique data + values into a :class:`bytes` object for sending over a data stream. + + This class should be overridden to implement unique packet payloads. Note + that this class supports three subclass arguments to assign unique message IDs, + subclass IDs, and payload formats. + + .. code-block:: python + + class ExamplePacket(Packet, msg_id = 0x02, subclass_id = 0x01, payload_format = "BHHf"): + example_char: int + example_short: int + example_short_two: int + example_float: float + + .. container:: operations + + .. describe:: bytes(x) + + Returns a :class:`bytes` object representing the data of the packet + in the specified packet format. + + Arguments: + msg_id (int): The message ID. Can be between 0 and 255. + subclass_id (int): The message subclass ID. Can be between 0 and 255. + payload_format (str): The format for the payload. This determines how + the individual payload is assembled. Each character in the format + string represents the position of one class variable. The class variables + are assembled in the order they are defined in. + """ + + msg_id: ClassVar[int] + subclass_id: ClassVar[int] + payload_format: ClassVar[str] + + def __init_subclass__(cls, msg_id: int, subclass_id: int, payload_format: str = ""): + cls.msg_id = msg_id + cls.subclass_id = subclass_id + cls.payload_format = payload_format + + def __bytes__(self): + payload = struct.pack(self.payload_format, *self.__dict__.values()) + return struct.pack( + f"BBBBH{len(payload)}s", + 0x37, + 0x01, + self.msg_id, + self.subclass_id, + len(payload), + payload, + ) + + +@dataclass +class AckPacket(Packet, msg_id=0x00, subclass_id=0x01, payload_format=""): + """ + Common acknowledgment packet. Should only be found in response operations. + """ + + pass + + +@dataclass +class NackPacket(Packet, msg_id=0x00, subclass_id=0x00, payload_format=""): + """ + Common not-acknowledged packet. Should only be found in response operations. + """ + + pass From 51f415ea6cadd7900e432d8013f24cdc28edd713 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sat, 21 Jan 2023 18:17:30 -0500 Subject: [PATCH 02/38] First edition of rewrite --- docs/reference/can.rst | 144 +----- .../mil_usb_to_can/mil_usb_to_can/__init__.py | 76 +-- .../mil_usb_to_can/application_packet.py | 123 ----- .../mil_usb_to_can/mil_usb_to_can/board.py | 71 --- .../mil_usb_to_can/mil_usb_to_can/device.py | 185 +++---- .../mil_usb_to_can/mil_usb_to_can/driver.py | 225 ++++++-- .../mil_usb_to_can/mil_usb_to_can/example.py | 136 +++++ .../mil_usb_to_can/mil_usb_to_can/packet.py | 53 +- .../mil_usb_to_can/simulation.py | 129 ----- .../mil_usb_to_can/mil_usb_to_can/utils.py | 487 ------------------ .../mil_usb_to_can/test/adder_device.test | 4 +- .../mil_usb_to_can/test/echo_device.test | 4 +- .../test/test_application_packets.py | 50 -- .../mil_usb_to_can/test/test_packets.py | 54 ++ 14 files changed, 495 insertions(+), 1246 deletions(-) delete mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py delete mode 100755 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/board.py create mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/example.py delete mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/simulation.py delete mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/utils.py delete mode 100755 mil_common/drivers/mil_usb_to_can/test/test_application_packets.py create mode 100755 mil_common/drivers/mil_usb_to_can/test/test_packets.py diff --git a/docs/reference/can.rst b/docs/reference/can.rst index 8be592751..d5759225d 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -17,60 +17,31 @@ sent over the serial connection to the USB to CAN board from ROS. * - Name - Length - Description - * - Start flag (``0xC0``) + * - Sync character 1 (``0x37``) - 1 byte - - Flag indicating the start of a new board message - * - Payload - - 4-11 bytes - - Actual data being transmitted. Often created through application packets. - The two following tables show the payloads of command and receiving packets. - * - End flag (``0xC1``) - - 1 byte - - Flag indicating end of previous board message - -Below are the payload specifications for each type of transmission. Command packets -are packets sent out by the computer to complete an action (sending or requesting -information), whereas receiving packets are packets that listen to data coming from -other devices. - -.. list-table:: Command Packet (:class:`.CommandPacket`) - :header-rows: 1 - - * - Name - - Length - - Description - * - Length + - First sync character indicating the start of packets. + * - Sync character 2 (``0x01``) - 1 byte - - Byte indicating the length of the data being sent, or the length of the data - expected to be received, in bytes. Notably, bit 7 of this byte determines whether - the command packet is sending a command, or receiving data through a command. - If bit 7 is 1, then the command packet is receiving. - * - CAN ID + - Second sync character indicating the start of packets. + * - Class ID - 1 byte - - ID of the sender, or ID of who to request from - * - Data - - 1-8 bytes - - For sending command packets, the actual data being sent. For requesting command - packets, an empty binary string. - -.. list-table:: Receiving Packet (:class:`.ReceivePacket`) - :header-rows: 1 - - * - Name - - Length - - Description - * - Device ID + - Message class. Determines the family of messages the packet belongs to. + * - Subclass ID - 1 byte - - The CAN ID of the device to receive data from. - * - Payload length + - Message subclass. In combination with class, determines specific qualities + of message. + * - Payload Length + - 2 bytes + - Length of payload. + * - Payload + - 0-65535 bytes + - Payload. Meaning of payload is determined by specific packet class/subclass. + * - Checksum A - 1 byte - - The amount of data to listen to. - * - Data - - 1-8 bytes - - The data that was received. - * - Checksum + - First byte of Fletcher's checksum. + * - Checksum B - 1 byte - - The checksum for the data. + - Second byte of Fletcher's checksum. Checksums ^^^^^^^^^ @@ -80,61 +51,6 @@ packets also have a special byte containing a slightly modified checksum formula The checksum in all packets is found by adding up all bytes in the byte string, including the start/end flags, and then using modulo 16 on this result. -Exceptions -^^^^^^^^^^ - -Exception Hierarchy -~~~~~~~~~~~~~~~~~~~ -.. currentmodule:: mil_usb_to_can - -.. exception_hierarchy:: - - - :exc:`Exception` - - :exc:`ApplicationPacketWrongIdentifierException` - - :exc:`USB2CANException` - - :exc:`ChecksumException` - - :exc:`PayloadTooLargeException` - - :exc:`InvalidFlagException` - - :exc:`InvalidStartFlagException` - - :exc:`InvalidEndFlagException` - -Exception List -~~~~~~~~~~~~~~~~~~~ -.. autoclass:: ApplicationPacketWrongIdentifierException - :members: - -.. autoclass:: USB2CANException - :members: - -.. autoclass:: ChecksumException - :members: - -.. autoclass:: PayloadTooLargeException - :members: - -.. autoclass:: InvalidFlagException - :members: - -.. autoclass:: InvalidStartFlagException - :members: - -.. autoclass:: InvalidEndFlagException - :members: - -ApplicationPacket -^^^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.ApplicationPacket - -.. autoclass:: mil_usb_to_can.ApplicationPacket - :members: - -USBtoCANBoard -^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.USBtoCANBoard - -.. autoclass:: mil_usb_to_can.USBtoCANBoard - :members: - CANDeviceHandle ^^^^^^^^^^^^^^^ .. attributetable:: mil_usb_to_can.CANDeviceHandle @@ -142,25 +58,11 @@ CANDeviceHandle .. autoclass:: mil_usb_to_can.CANDeviceHandle :members: -USBtoCANDriver -^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.USBtoCANDriver - -.. autoclass:: mil_usb_to_can.USBtoCANDriver - :members: - -SimulatedCANDevice -^^^^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.SimulatedCANDevice - -.. autoclass:: mil_usb_to_can.SimulatedCANDevice - :members: - -SimulatedUSBtoCAN -^^^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.SimulatedUSBtoCAN +SimulatedCANDeviceHandle +^^^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: mil_usb_to_can.SimulatedCANDeviceHandle -.. autoclass:: mil_usb_to_can.SimulatedUSBtoCAN +.. autoclass:: mil_usb_to_can.SimulatedCANDeviceHandle :members: Packet diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py index 86cce2fee..2501f364e 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py @@ -1,74 +1,2 @@ -#!/usr/bin/python3 -""" -The ``mil_usb_to_can`` package implements a driver and helper class for the -USB to CAN driver board. The package provides full functionality for communication -with the board, along with helper classes to provide a better, more structured use -of data being sent to and received from the board. - -To launch the driver, start ``driver.py``, an executable Python file. This file -will spin up a driver and interface to the board. If you are starting the driver -from a launch file, you can additionally provide information for the embedded -:class:`USBtoCANBoard` class, which handles connecting to the board. This information -can be provided through local ROS parameters: - -.. code-block:: xml - - - - - - # Path of serial device (default: "/dev/tty0") - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0 - # Baudrate of device (default: 115200) - baudrate: 115200 - # The CAN device id of the board (default: 0) - can_id: 0 - # List of python device handle classes (list of device id: python class implementation) - device_handles: - "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard - "83": sub8_actuator_board.ActuatorBoard - simulated_devices: - "18": sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation - "83": sub8_actuator_board.ActuatorBoardSimulation - - - - -Many of the parameters shown are used to connect the driver to the physical board. -However, the final two parameters, ``device_handles`` and ``simulated_devices`` -are used to create device handles for specific devices. These device handles can -send and receive data from the board, and many be used to do both, or one or the -other. - -As suggested in the above XML, the package implements drivers for a physical run, -as well as a simulated run. Whether the simulated drivers are used is controlled -by the global ``/is_simulation`` parameter. The physical drivers implement -:class:`CANDeviceHandle`, whereas the simulated drivers implement from -:class:`SimulatedCANDevice`. More information on writing device handles can be -found in the documentation of each type of class. -""" - -from .application_packet import ( - ApplicationPacket, - ApplicationPacketWrongIdentifierException, -) -from .board import USBtoCANBoard -from .device import CANDeviceHandle, ExampleAdderDeviceHandle, ExampleEchoDeviceHandle -from .driver import USBtoCANDriver -from .simulation import ( - ExampleSimulatedAdderDevice, - ExampleSimulatedEchoDevice, - SimulatedCANDevice, - SimulatedUSBtoCAN, -) -from .utils import ( - ChecksumException, - CommandPacket, - InvalidEndFlagException, - InvalidFlagException, - InvalidStartFlagException, - Packet, - PayloadTooLargeException, - ReceivePacket, - USB2CANException, -) +from .device import CANDeviceHandle, SimulatedCANDeviceHandle +from .packet import AckPacket, NackPacket, Packet diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py deleted file mode 100644 index 1965cc022..000000000 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py +++ /dev/null @@ -1,123 +0,0 @@ -#! /usr/bin/env python3 -from __future__ import annotations - -import struct -from typing import TypeVar - -T = TypeVar("T", bound="ApplicationPacket") - - -class ApplicationPacketWrongIdentifierException(Exception): - """ - Exception thrown when the identifier for a MIL application level CAN packet - had a different identifier from what was expected. - - Inherits from :class:`Exception`. - """ - - def __init__(self, received: int, expected: int): - """ - Attributes: - received (int): A value representing what received found. - expected (int): What the found value should have been. - """ - super().__init__(f"Expected identified {expected}, got {received}") - - -class ApplicationPacket: - """ - Represents an application-specific packet structure used by a CAN device. This - class does not implement the entire packet that will be sent over the CAN bus; - the packet only includes the identifier and payload in the packet. - - This class should be used to generate packets to send to the board. This packet - does not handle communication with the board; this is instead handled by other - packet classes. - - This class can be inherited from to implement packet structures for specific - applications. - - .. code-block:: python - - class SendALetterMessage(ApplicationPacket): - - IDENTIFIER = 0xAA - STRUCT_FORMAT = "B" - - def __init__(self, letter: str): - self.letter = letter - super().__init__(self.IDENTIFIER, struct.pack(self.STRUCT_FORMAT, ord(letter))) - - @classmethod - def from_bytes(cls, data: bytes) -> SendALetterMessage: - return cls(*struct.unpack(self.STRUCT_FORMAT, data)) - - .. container:: operations - - .. describe:: bytes(x) - - Assembles the packet into a form suitable for sending through a data - stream. Packs :attr:`~.identifier` and :attr:`~.payload` into a single - :class:`bytes` object. - - Attributes: - identifier (int): The identifier for the packet. Allowed range is between 0 - and 255. - payload (bytes): The payload of bytes to be sent in the packet. - """ - - def __init__(self, identifier: int, payload: bytes): - self.identifier = identifier - self.payload = payload - - def __bytes__(self) -> bytes: - """ - Packs the packet into a series of bytes using :meth:`struct.Struct.pack`. - The identifier is packed as an unsigned integer, while the payload of bytes - is packed as a sequence of bytes equal to the length of the payload. - - Returns: - bytes: The packed bytes. - """ - return struct.pack(f"B{len(self.payload)}s", self.identifier, self.payload) - - @classmethod - def from_bytes( - cls: type[T], data: bytes, expected_identifier: int | None = None - ) -> T: - """ - Unpacks a series of packed bytes representing an application packet using - :meth:`struct.Struct.unpack`, which produces the packet identifier and array of data. - These values are then used to produce the new instance of the class. - - Args: - data (bytes): The packed packet. - expected_identifier (Optional[int]): The identifier that is expected to - result from the packet. If ``None``, then the identifier is not verified. - If this value is passed and the identifiers do not match, then the below - error is thrown. - - Raises: - ApplicationPacketWrongIdentifierException: If the ``expected_identifier`` - does not match the identifier found in the packet, then this is raised. - - Returns: - ApplicationPacket: The data represented as an application packet. - """ - payload_len = len(data) - 1 - packet = cls(*struct.unpack(f"B{payload_len}s", data)) - if expected_identifier is not None and expected_identifier != packet.identifier: - raise ApplicationPacketWrongIdentifierException( - packet.identifier, expected_identifier - ) - return packet - - def __repr__(self): - return "{}(identifier={}, payload={})".format( - self.__class__.__name__, self.identifier, self.payload - ) - - def __eq__(self, other): - if not isinstance(other, ApplicationPacket): - raise NotImplemented() - return self.identifier == other.identifier and self.payload == other.payload diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/board.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/board.py deleted file mode 100755 index 63b60226b..000000000 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/board.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python3 -from threading import Lock - -# from mil_tools import hexify -from typing import Optional - -import serial -from mil_usb_to_can.simulation import SimulatedUSBtoCAN -from mil_usb_to_can.utils import ( # causes error if relative import used - GH-731 - CommandPacket, - ReceivePacket, -) - - -class USBtoCANBoard: - """ - ROS-independent wrapper which provides an interface to connect to the USB to CAN board - via a serial (or simulated serial) device. Provides thread-safe functionality. - - Attributes: - lock (threading.Lock): The thread lock. - ser (Union[:class:`SimulatedUSBtoCAN`, :class:`serial.Serial`]): The serial connection. - """ - - def __init__(self, port: str, baud: int = 9600, simulated: bool = False, **kwargs): - """ - Args: - port (str): Path to serial device, such as ``/dev/ttyUSB0``. - baud (int): Baud rate of serial device to connect to. Defaults to 9600. - simulated (bool): If True, use a simulated serial device rather than a real device. Defaults to ``False``. - """ - self.lock = Lock() - if simulated: - self.ser = SimulatedUSBtoCAN(**kwargs) - else: - self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.1, **kwargs) - self.ser.flushOutput() - self.ser.flushInput() - - def read_packet(self) -> Optional[ReceivePacket]: - """ - Read a packet from the board, if available. Returns a :class:`ReceivePacket` - instance if one was succefully read, or ``None`` if the in buffer is empty. - - Returns: - Optional[:class:`ReceivePacket`]: The packet, if found, otherwise ``None``. - """ - # TODO Does this actually return ReceivePacket? Appears it might only be - # able to return Packet. - with self.lock: - if self.ser.in_waiting == 0: - return None - return ReceivePacket.read_packet(self.ser) - - def send_data(self, data: bytes, can_id: int = 0) -> None: - """ - Sends data to a CAN device using the thread lock. Writes using the :meth:`write` - method of the :attr:`.ser` attribute. - - Args: - device_id (int): CAN device ID to send data to. - data (bytes): Data (represented as bytes) to send to the device. - - Raises: - PayloadTooLargeException: The payload is larger than 8 bytes. - """ - p = CommandPacket.create_send_packet(data, can_id=can_id) - with self.lock: - p_bytes = bytes(p) - # print 'SENDING ', hexify(p_bytes) - self.ser.write(p_bytes) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py index ca0e25293..a2ae1ca0a 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py @@ -1,15 +1,48 @@ -#!/usr/bin/python3 -import random -import string -import struct +from __future__ import annotations -import rospy -from rospy_tutorials.srv import AddTwoInts -from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse +from typing import TYPE_CHECKING -from .application_packet import ApplicationPacket -from .board import USBtoCANBoard -from .utils import PayloadTooLargeException +if TYPE_CHECKING: + from .driver import SimulatedUSBtoCANStream, USBtoCANDriver + from .packet import Packet + + +class SimulatedCANDeviceHandle: + """ + Simulates a CAN device, with functions to be overridden to handle data requests + and sends from motherboard. + + Child classes can inherit from this class to implement a simulated CAN device. + + Attributes: + inbound_packets (type[Packet]): The types of packets listened to by this device. + Packets of this type will be routed to the :meth:`~.on_data` method of + the device handle. + """ + + def __init__( + self, sim_board: SimulatedUSBtoCANStream, inbound_packets: list[type[Packet]] + ): + self._sim_board = sim_board + self.inbound_packets = inbound_packets + + def send_data(self, data: bytes): + """ + Sends data over the serial connection. + """ + self._sim_board.send_to_bus(data) + + def on_data(self, packet: Packet): + """ + Called when an relevant incoming packet is received over the serial + connection. Relevant packets are those listed in :attr:`~.inbound_packets`. + + Partial data (ie, incomplete packets) are not sent through this event. + + Args: + packet (Packet): The incoming packet. + """ + del packet class CANDeviceHandle: @@ -17,136 +50,36 @@ class CANDeviceHandle: Base class to allow developers to write handles for communication with a particular CAN device. The two methods of the handle allow the handle to listen to incoming data, as well as send data. - - .. code-block:: python - - class ExampleEchoDeviceHandle(CANDeviceHandle): - def __init__(self, driver, device_id): - self.last_sent = None - self.send_new_string() - super().__init__(driver, device_id) - - def on_data(self, data: bytes): - if self.last_sent is not None and data == self.last_sent[0]: - print( - "SUCCESSFULLY echoed {} in {}seconds".format( - self.last_sent[0], (rospy.Time.now() - self.last_sent[1]).to_sec() - ) - ) - rospy.sleep(0.15) - self.send_new_string() - - def send_new_string(self): - test = "".join([random.choice(string.ascii_letters) for _ in range(4)]) - self.last_sent = (test, rospy.Time.now()) - self.send_data(test.encode()) """ - def __init__(self, driver: USBtoCANBoard, device_id: int): + def __init__(self, driver: USBtoCANDriver): """ Args: - driver (USBtoCANBoard): Driver that is used to communicate with the board. - device_id (int): The CAN ID of the device this class will handle. Not currently used. + driver (USBtoCANBoard): Driver that is used to communicate with the board. + device_id (int): The CAN ID of the device this class will handle. Not currently used. """ self._driver = driver - self._device_id = device_id - def on_data(self, data: bytes): + def on_data(self, data: Packet): """ - Called when data is received from the device this handle is registered for. + Called when a return packet is sent over the serial connection. In the + USB to CAN protocol, it is assumed that packets will be returned to the + motherboard in the order they are sent out. Therefore, the type of packet + sent through this event is not guaranteed, and is only determined by the + message and subclass ID sent by the individual board. + + Partial data (ie, incomplete packets) are not sent through this event. Args: - data (bytes): The data received. + packet (Packet): The incoming packet. """ del data - def send_data(self, data: bytes, can_id: int = 0): + def send_data(self, data: Packet): """ - Sends data to the device. + Sends a packet over the serial connection. Args: - data (bytes): The data payload to send to the device. - can_id (int): The CAN device ID to send data to. Defaults to 0. - - Raises: - PayloadTooLargeException: The payload is larger than 8 bytes. + data (Packet): The packet to send. """ - return self._driver.send_data(data, can_id=can_id) - - -class ExampleEchoDeviceHandle(CANDeviceHandle): - """ - An example implementation of a CANDeviceHandle which will handle - a device that echos back any data sent to it. - """ - - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.last_sent = None - self.count = 0 - self._srv = rospy.Service("start_echo", Trigger, self.srv_req) - - def srv_req(self, req: TriggerRequest): - while self.count < 10: - if not self.send_new_string(4): - return TriggerResponse(False, "Unable to send string of length four.") - - try: - self.send_new_string(8) - except PayloadTooLargeException: - pass - except: - return TriggerResponse(False, "Testing large strings failed.") - return TriggerResponse(True, "Complete!") - - def on_data(self, data): - data = ApplicationPacket.from_bytes(data, expected_identifier=37).payload - if self.last_sent is None: - raise RuntimeError(f"Received {data} but have not yet sent anything") - elif data.decode() != self.last_sent[0]: - raise ValueError( - f"ERROR! Received {data.decode()} but last sent {self.last_sent}" - ) - else: - self.count += 1 - - def send_new_string(self, length: int = 4): - # Example string to test with - test = "".join([random.choice(string.ascii_letters) for _ in range(length)]) - self.last_sent = (test, rospy.Time.now()) - self.send_data(bytes(ApplicationPacket(37, test.encode()))) - start = rospy.Time.now() - count_now = self.count - while self.count == count_now: - if rospy.Time.now() - start > rospy.Duration(1): - return False - return True - - -class ExampleAdderDeviceHandle(CANDeviceHandle): - """ - An example implementation of a CANDeviceHandle which will handle - a device that echos back any data sent to it. - """ - - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.response_received = None - self._srv = rospy.Service("add_two_ints", AddTwoInts, self.on_service_req) - - def on_service_req(self, req): - payload = struct.pack("hh", req.a, req.b) - self.response_received = None - self.send_data(bytes(ApplicationPacket(37, payload))) - start = rospy.Time.now() - while self.response_received is None: - if rospy.Time.now() - start > rospy.Duration(1): - return -1 - res = ApplicationPacket.from_bytes( - self.response_received, expected_identifier=37 - ) - my_sum = struct.unpack("i", res.payload) - return my_sum - - def on_data(self, data): - self.response_received = data + return self._driver.send_data(self, data) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py index ef32f5dbc..eda88e3b5 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py @@ -1,16 +1,81 @@ #!/usr/bin/python3 +from __future__ import annotations + import importlib -from typing import TYPE_CHECKING, Any, Dict, Generator, Optional, Tuple +from collections import deque +from threading import Lock +from typing import TYPE_CHECKING, Literal, overload import rospy -from mil_usb_to_can.board import ( - USBtoCANBoard, # relative import causes import error with rosrun - GH-731 -) -from mil_usb_to_can.utils import USB2CANException +import serial +from mil_misc_tools.serial_tools import SimulatedSerial +from mil_usb_to_can.device import CANDeviceHandle, SimulatedCANDeviceHandle +from mil_usb_to_can.packet import SYNC_CHAR_1, Packet from serial import SerialException if TYPE_CHECKING: - from .device import CANDeviceHandle + HandlePacketListing = tuple[ + type[SimulatedCANDeviceHandle | CANDeviceHandle], list[Packet] + ] + + +class SimulatedUSBtoCANStream(SimulatedSerial): + """ + Simulates the USB to CAN board. Is supplied with a dictionary of simulated + CAN devices to simulate the behavior of the whole CAN network. + """ + + _devices: list[SimulatedCANDeviceHandle] + + def __init__( + self, + devices: list[tuple[type[SimulatedCANDeviceHandle], list[type[Packet]]]] + | None = None, + ): + """ + Args: + devices (List[Tuple[Type[SimulatedCANDeviceHandle], List[Type[Packet]]]]): List of + the simulated device handles, along with the list of packets each handle + is listening for. + """ + if devices is None: + devices = [] + + self._devices = [device(self, packet_list) for device, packet_list in devices] + super().__init__() + + def send_to_bus(self, data: bytes, *, from_mobo=False): + """ + Sends data onto the simulated bus from a simulated device. + + Args: + data (bytes): The payload to send on to the bus. + from_mobo (bool): Whether the data is from the motherboard. Defaults to + False. + """ + # Deconstruct packet + p = Packet.from_bytes(data) + + if not from_mobo: + self.buffer += data + + # Send packet to appropriate handle + sent = False + for device in self._devices: + if type(p) in device.inbound_packets: + device.on_data(p) + sent = True + + if not sent and from_mobo: + rospy.logerr( + f"{sent}, {from_mobo}: Received packet of type {p.__class__.__qualname__} on simulated bus, but no one is listening for it..." + ) + + def write(self, data: bytes) -> None: + """ + Sends data to the bus. This method should only be called by the driver. + """ + self.send_to_bus(data, from_mobo=True) class USBtoCANDriver: @@ -18,51 +83,62 @@ class USBtoCANDriver: ROS Driver which implements the USB to CAN board. Allow users to specify a dictionary of device handle classes to be loaded at runtime to handle communication with specific devices. - - Attributes: - board (USBtoCANBoard): The board the driver is implementing. - handles (dict[int, CANDeviceHandle]): The handles served by the driver. Each key represents - a unique device ID, and each corresponding value represents an instance of - a child class inheriting from :class:`CANDeviceHandle`. Upon initialization, - each class is constructed after being parsed from dynamic reconfigure. - timer (rospy.Timer): The timer controlling when buffers are processed. """ + _packet_deque: deque[ + SimulatedCANDeviceHandle | CANDeviceHandle + ] # Used to keep track of who gets incoming packets + def __init__(self): port = rospy.get_param("~port", "/dev/tty0") baud = rospy.get_param("~baudrate", 115200) can_id = rospy.get_param("~can_id", 0) simulation = rospy.get_param("/is_simulation", False) + self.lock = Lock() + self._packet_deque = deque() # If simulation mode, load simulated devices if simulation: rospy.logwarn( "CAN2USB driver in simulation! Will not talk to real hardware." ) - devices = dict( - list( - self.parse_module_dictionary(rospy.get_param("~simulated_devices")) - ) - ) - self.board = USBtoCANBoard( - port=port, - baud=baud, - simulated=simulation, - devices=devices, - can_id=can_id, - ) + devices = self.read_devices(simulated=True) + self.stream = SimulatedUSBtoCANStream(devices=devices) else: - self.board = USBtoCANBoard(port=port, baud=baud, simulated=simulation) + self.stream = serial.Serial(port=port, baudrate=baud, timeout=0.1) - # Add device handles from the modules specified in ROS params - self.handles: Dict[int, CANDeviceHandle] = { - device_id: cls(self, device_id) - for device_id, cls in self.parse_module_dictionary( - rospy.get_param("~device_handles") - ) - } + self.stream.reset_output_buffer() + self.stream.reset_input_buffer() + + self.handles = [ + device[0](self) for device in self.read_devices(simulated=False) + ] self.timer = rospy.Timer(rospy.Duration(1.0 / 20.0), self.process_in_buffer) + def read_from_stream(self) -> bytes | None: + # Read until SOF is encourntered in case buffer contains the end of a previous packet + sof = None + for _ in range(10): + sof = self.stream.read(1) + if sof is None or len(sof) == 0: + return None + sof_int = int.from_bytes(sof, byteorder="big") + if sof_int == SYNC_CHAR_1: + break + assert isinstance(sof, bytes) + sof_int = int.from_bytes(sof, byteorder="big") + if sof_int != SYNC_CHAR_1: + print("Where da start char at?") + data = sof + # Read sync char 2, msg ID, subclass ID + data += self.stream.read(3) + length = self.stream.read(2) # read payload length + data += length + data += self.stream.read( + int.from_bytes(length, byteorder="little") + 2 + ) # read data and checksum + return data + def read_packet(self) -> bool: """ Attempt to read a packet from the board. If the packet has an appropriate device @@ -72,20 +148,23 @@ def read_packet(self) -> bool: bool: The success in reading a packet. """ try: - packet = self.board.read_packet() - except (SerialException, USB2CANException) as e: + with self.lock: + if self.stream.in_waiting == 0: + return False + packed_packet = self.read_from_stream() + assert isinstance(packed_packet, bytes) + packet = Packet.from_bytes(packed_packet) + except (SerialException) as e: rospy.logerr(f"Error reading packet: {e}") return False if packet is None: return False - if packet.device in self.handles: - self.handles[packet.device].on_data(packet.data) - else: + if not self._packet_deque: rospy.logwarn( - "Message received for device {}, but no handle registered".format( - packet.device - ) + f"Message of type {packet.__class__.__qualname__} received, but no device ready to accept" ) + else: + self._packet_deque.popleft().on_data(packet) return True def process_in_buffer(self, *args) -> None: @@ -95,7 +174,9 @@ def process_in_buffer(self, *args) -> None: while self.read_packet(): pass - def send_data(self, *args, **kwargs) -> Optional[Exception]: + def send_data( + self, handle: CANDeviceHandle | SimulatedCANDeviceHandle, packet: Packet + ) -> Exception | None: """ Sends data using the :meth:`USBtoCANBoard.send_data` method. @@ -104,16 +185,33 @@ def send_data(self, *args, **kwargs) -> Optional[Exception]: Otherwise, the exception that was raised in sending is returned. """ try: - self.board.send_data(*args, **kwargs) + with self.lock: + self.stream.write(bytes(packet)) + self._packet_deque.append(handle) return None - except (SerialException, USB2CANException) as e: + except (SerialException) as e: rospy.logerr(f"Error writing packet: {e}") return e - @staticmethod - def parse_module_dictionary( - d: Dict[str, Any] - ) -> Generator[Tuple[int, Any], None, None]: + @overload + def read_devices( + self, *, simulated: Literal[True] + ) -> list[tuple[type[SimulatedCANDeviceHandle], list[type[Packet]]]]: + ... + + @overload + def read_devices( + self, *, simulated: Literal[False] + ) -> list[tuple[type[CANDeviceHandle], list[type[Packet]]]]: + ... + + def read_devices( + self, *, simulated: bool + ) -> list[ + tuple[ + type[SimulatedCANDeviceHandle] | type[CANDeviceHandle], list[type[Packet]] + ], + ]: """ Generator to load classes from module strings specified in a dictionary. Imports all found classes. @@ -122,14 +220,31 @@ def parse_module_dictionary( Generator[Tuple[int, Any], None, None]: Yields tuples containing the device ID and the associated class. """ - for device_id, module_name in d.items(): - device_id = int(device_id) - # Split module from class name + d = {} + res: list[ + tuple[type[SimulatedCANDeviceHandle | CANDeviceHandle], list[type[Packet]]] + ] = [] + if simulated: + d = rospy.get_param("~simulated_devices") + else: + d = rospy.get_param("~device_handles") + + for module_name, packet_list in d.items(): + # Split module from class name, import module, and get class from module module_name, cls = module_name.rsplit(".", 1) - # import module module = importlib.import_module(module_name) - # Yield a tuple (device_id, imported_class) - yield device_id, getattr(module, cls) + imported_class = getattr(module, cls) + if simulated: + assert issubclass(imported_class, SimulatedCANDeviceHandle) + else: + assert issubclass(imported_class, CANDeviceHandle) + packets: list[type[Packet]] = [] + for packet in packet_list: + module_name, cls = packet.rsplit(".", 1) + module = importlib.import_module(module_name) + packets.append(getattr(module, cls)) + res.append((imported_class, packets)) + return res if __name__ == "__main__": diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/example.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/example.py new file mode 100644 index 000000000..d8bffdb45 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/example.py @@ -0,0 +1,136 @@ +import random +import string +from dataclasses import dataclass + +import rospy +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, subclass_id=0x00, payload_format="10s" +): + my_special_string: bytes + + +@dataclass +class ExampleEchoResponsePacket( + Packet, msg_id=0x99, subclass_id=0x01, payload_format="10s" +): + my_special_string: bytes + + +@dataclass +class ExampleAdderRequestPacket( + Packet, msg_id=0x99, subclass_id=0x02, payload_format="BB" +): + num_one: int + num_two: int + + +@dataclass +class ExampleAdderResponsePacket( + Packet, msg_id=0x99, subclass_id=0x03, payload_format="B" +): + response: int + + +class ExampleEchoDeviceHandle(CANDeviceHandle): + """ + An example implementation of a CANDeviceHandle which will handle + a device that echos back any data sent to it. + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.last_sent = None + self.count = 0 + self._srv = rospy.Service("start_echo", Trigger, self.srv_req) + + def srv_req(self, req: TriggerRequest): + while self.count < 10: + if not self.send_new_string(10): + return TriggerResponse(False, "Unable to send string of length ten.") + return TriggerResponse(True, "Complete!") + + def on_data(self, data: ExampleEchoRequestPacket): + response = data.my_special_string.decode() + if self.last_sent is None: + raise RuntimeError(f"Received {data} but have not yet sent anything") + elif response != self.last_sent[0]: + raise ValueError( + f"ERROR! Received {response} but last sent {self.last_sent}" + ) + else: + self.count += 1 + + def send_new_string(self, length: int = 10): + # Example string to test with + test = "".join([random.choice(string.ascii_letters) for _ in range(length)]) + self.last_sent = (test, rospy.Time.now()) + self.send_data(ExampleEchoRequestPacket(test.encode())) + start = rospy.Time.now() + count_now = self.count + while self.count == count_now: + if rospy.Time.now() - start > rospy.Duration(1): + return False + return True + + +class ExampleAdderDeviceHandle(CANDeviceHandle): + """ + An example implementation of a CANDeviceHandle which will handle + a device that echos back any data sent to it. + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.response_received = None + self._srv = rospy.Service("add_two_ints", AddTwoInts, self.on_service_req) + + def on_service_req(self, req): + self.response_received = None + self.send_data(ExampleAdderRequestPacket(req.a, req.b)) + start = rospy.Time.now() + while self.response_received is None: + if rospy.Time.now() - start > rospy.Duration(1): + return -1 + return self.response_received.response + + def on_data(self, data: ExampleAdderResponsePacket): + self.response_received = data + + +class ExampleSimulatedEchoDevice(SimulatedCANDeviceHandle): + """ + Example implementation of a SimulatedCANDevice. + On sends, stores the transmitted data in a buffer. + When data is requested, it echos this data back. + """ + + def __init__(self, handle, inbound_packets): + # Call parent classes constructor + super().__init__(handle, inbound_packets) + + def on_data(self, data: ExampleEchoRequestPacket): + # Echo data received back onto the bus + self.send_data(bytes(ExampleEchoResponsePacket(data.my_special_string))) + + +class ExampleSimulatedAdderDevice(SimulatedCANDeviceHandle): + """ + Example implementation of a SimulatedCANDevice. + On sends, stores the transmitted data in a buffer. + When data is requested, it echos this data back. + """ + + def __init__(self, handle, inbound_packets): + # Call parent classes constructor + super().__init__(handle, inbound_packets) + + def on_data(self, data: ExampleAdderRequestPacket): + self.send_data(bytes(ExampleAdderResponsePacket(data.num_one + data.num_two))) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py index db9b68ce8..d99438baf 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py @@ -4,6 +4,11 @@ from dataclasses import dataclass from typing import ClassVar +SYNC_CHAR_1 = 0x37 +SYNC_CHAR_2 = 0x01 + +_packet_registry: dict[int, dict[int, type[Packet]]] = {} + @dataclass class Packet: @@ -14,10 +19,14 @@ class Packet: This class should be overridden to implement unique packet payloads. Note that this class supports three subclass arguments to assign unique message IDs, - subclass IDs, and payload formats. + subclass IDs, and payload formats. Note that all subclasses must be decorated + with :meth:`dataclasses.dataclass`. .. code-block:: python + from dataclasses import dataclass + + @dataclass class ExamplePacket(Packet, msg_id = 0x02, subclass_id = 0x01, payload_format = "BHHf"): example_char: int example_short: int @@ -48,17 +57,53 @@ def __init_subclass__(cls, msg_id: int, subclass_id: int, payload_format: str = cls.msg_id = msg_id cls.subclass_id = subclass_id cls.payload_format = payload_format + packets = [p for mid in _packet_registry.values() for p in mid.values()] + for packet in packets: + if packet.msg_id == msg_id and packet.subclass_id == subclass_id: + raise ValueError( + f"Cannot reuse msg_id 0x{msg_id:0x} and subclass_id 0x{subclass_id}, already used by {packet.__qualname__}" + ) + _packet_registry.setdefault(msg_id, {})[subclass_id] = cls + + def _calculate_checksum(self): + return (0, 0) def __bytes__(self): payload = struct.pack(self.payload_format, *self.__dict__.values()) + checksum = self._calculate_checksum() return struct.pack( - f"BBBBH{len(payload)}s", + f"BBBBH{len(payload)}sBB", 0x37, 0x01, self.msg_id, self.subclass_id, len(payload), payload, + checksum[0], + checksum[1], + ) + + @classmethod + def from_bytes(cls, packed: bytes) -> Packet: + """ + Constructs a packet from a packed packet in a :class:`bytes` object. + If a packet is found with the corresponding message and subclass ID, + then an instance of that packet class will be returned, else :class:`Packet` + will be returned. + """ + msg_id = packed[2] + subclass_id = packed[3] + # for subclass in cls.__subclasses__(): + # if subclass.msg_id == msg_id and subclass.subclass_id == subclass_id: + # payload = packed[6:-2] + # unpacked = struct.unpack(subclass.payload_format, payload) + if msg_id in _packet_registry and subclass_id in _packet_registry[msg_id]: + subclass = _packet_registry[msg_id][subclass_id] + payload = packed[6:-2] + unpacked = struct.unpack(subclass.payload_format, payload) + return subclass(*unpacked) + raise LookupError( + f"Attempted to reconstruct packet with msg_id 0x{msg_id:02x} and subclass_id 0x{subclass_id:02x}, but no packet with IDs was found." ) @@ -68,13 +113,9 @@ class AckPacket(Packet, msg_id=0x00, subclass_id=0x01, payload_format=""): Common acknowledgment packet. Should only be found in response operations. """ - pass - @dataclass class NackPacket(Packet, msg_id=0x00, subclass_id=0x00, payload_format=""): """ Common not-acknowledged packet. Should only be found in response operations. """ - - pass diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/simulation.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/simulation.py deleted file mode 100644 index 40e259b4f..000000000 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/simulation.py +++ /dev/null @@ -1,129 +0,0 @@ -#!/usr/bin/python3 -from __future__ import annotations - -import struct - -from mil_misc_tools.serial_tools import SimulatedSerial - -from .application_packet import ApplicationPacket -from .utils import CommandPacket, ReceivePacket - - -class SimulatedCANDevice: - """ - Simulates a CAN device, with functions to be overridden to handle data requests - and sends from motherboard. - - Child classes can inherit from this class to implement a simulated CAN device. - """ - - def __init__(self, sim_board: SimulatedUSBtoCAN, can_id: int): - self._sim_board = sim_board - self._can_id = can_id - - def send_data(self, data: bytes): - """ - Send data onto the bus, delivering it to other simulated devices and to - the driver node. - """ - self._sim_board.send_to_bus(self._can_id, data) - - def on_data(self, data: bytes, can_id: int): - """ - Called when the motherboard or another simulated device sends data onto the bus. - - .. note:: - - Because the CAN bus is shared, you must verify that the received data - is appropriate for your device. - - Args: - data (bytes): The data payload as a string/bytes object. - """ - - -class ExampleSimulatedEchoDevice(SimulatedCANDevice): - """ - Example implementation of a SimulatedCANDevice. - On sends, stores the transmitted data in a buffer. - When data is requested, it echos this data back. - """ - - def __init__(self, *args, **kwargs): - # Call parent classes constructor - super().__init__(*args, **kwargs) - - def on_data(self, data, can_id): - # Echo data received back onto the bus - packet = ApplicationPacket.from_bytes(data, expected_identifier=37) - self.send_data(bytes(ApplicationPacket(37, packet.payload))) - - -class ExampleSimulatedAdderDevice(SimulatedCANDevice): - """ - Example implementation of a SimulatedCANDevice. - On sends, stores the transmitted data in a buffer. - When data is requested, it echos this data back. - """ - - def __init__(self, *args, **kwargs): - # Call parent classes constructor - super().__init__(*args, **kwargs) - - def on_data(self, data, can_id): - packet = ApplicationPacket.from_bytes(data, expected_identifier=37) - a, b = struct.unpack("hh", packet.payload) - c = a + b - res = struct.pack("i", c) - self.send_data(bytes(ApplicationPacket(37, res))) - - -class SimulatedUSBtoCAN(SimulatedSerial): - """ - Simulates the USB to CAN board. Is supplied with a dictionary of simulated - CAN devices to simulate the behavior of the whole CAN network. - """ - - def __init__( - self, devices: dict[int, type[SimulatedCANDevice]] | None = None, can_id=-1 - ): - """ - Args: - devices (Dict[:class:`int`, Any]): Dictionary containing CAN IDs and - their associated simulated classes inheriting from :class:`SimulatedCANDevice`. - can_id (int): ID of the CAN2USB device. Defaults to -1. - """ - if devices is None: - devices = {0: SimulatedCANDevice} - - self._my_id = can_id - self._devices = { - can_id: device(self, can_id) for can_id, device in devices.items() - } - super().__init__() - - def send_to_bus(self, can_id: int, data: bytes, from_mobo: bool = False): - """ - Sends data onto the simulated bus from a simulated device. - - Args: - can_id (int): ID of sender. - data (bytes): The payload to send. - from_mobo (bool): Whether the data is from the motherboard. Defaults to - False. - """ - # If not from the motherboard, store this for future requests from motherboard - if not from_mobo: - self.buffer += bytes(ReceivePacket.create_receive_packet(can_id, data)) - # Send data to all simulated devices besides the sender - for device_can_id, device in self._devices.items(): - if device_can_id != can_id: - device.on_data(data, can_id) - - def write(self, data: bytes) -> int: - """ - Parse incoming data as a command packet from the motherboard. - """ - p = CommandPacket.from_bytes(data) - self.send_to_bus(p.filter_id, p.data, from_mobo=True) - return len(data) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/utils.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/utils.py deleted file mode 100644 index 7867020b1..000000000 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/utils.py +++ /dev/null @@ -1,487 +0,0 @@ -#!/usr/bin/python3 -from __future__ import annotations - -import struct -from typing import TYPE_CHECKING, Literal, TypeVar, overload - -import serial - -if TYPE_CHECKING: - from .simulation import SimulatedUSBtoCAN - -T = TypeVar("T", bound="Packet") - - -class USB2CANException(Exception): - """ - Base class for exception in USB2CAN board functionality. Inherits from :class:`Exception`. - """ - - -class ChecksumException(USB2CANException): - """ - Exception thrown when the checksum between motherboard and CANtoUSB board is invalid. - Inherits from :class:`USB2CANException`. - """ - - def __init__(self, calculated, expected): - super().__init__( - "Checksum was calculated as {} but reported as {}".format( - calculated, expected - ) - ) - - -class PayloadTooLargeException(USB2CANException): - """ - Exception thrown when payload of data sent/received from CAN2USB is too large. - Inherits from :class:`USB2CANException`. - """ - - def __init__(self, length): - super().__init__( - f"Payload is {length} bytes, which is greater than the maximum of 8" - ) - - -class InvalidFlagException(USB2CANException): - """ - Exception thrown when a constant flag in the CAN2USB protocol is invalid. Inherits - from :class:`USB2CANException`. - """ - - def __init__(self, description, expected, was): - super().__init__(f"{description} flag should be {expected} but was {was}") - - -class InvalidStartFlagException(InvalidFlagException): - """ - Exception thrown when the SOF flag is invalid. Inherits from :class:`InvalidFlagException`. - """ - - def __init__(self, was: int): - super().__init__("SOF", Packet.SOF, was) - - -class InvalidEndFlagException(InvalidFlagException): - """ - Exception thrown when the EOF flag is invalid. Inherits from :class:`InvalidFlagException`. - """ - - def __init__(self, was: int): - super().__init__("EOF", Packet.EOF, was) - - -class Packet: - """ - Represents a packet to or from the CAN to USB board. This class is inherited - by :class:`~mil_usb_to_can.ReceivePacket` (for receiving data from the bus) - and :class:`~mil_usb_to_can.CommandPacket` (for sending commands). Those child - classes should be used over this class whenever possible. - - .. container:: operations - - .. describe:: bytes(x) - - Assembles the packet into a form suitable for sending through a data - stream. For this base packet class, :attr:`~.SOF`, :attr:`~.payload`, - and :attr:`~.EOF` are assembled into one byte string. - - Attributes: - payload (bytes): The payload stored in the packet. - SOF (int): Flag used to mark the beginning of each packet. Equal to `0xC0`. - EOF (int): Flag used to mark the beginning of each packet. Equal to `0xC1`. - """ - - payload: bytes - - # Flag used to mark beginning of each packet - SOF = 0xC0 - # Flag used to mark end of each packet - EOF = 0xC1 - - def __init__(self, payload: bytes): - self.payload = payload - - def __bytes__(self) -> bytes: - """ - Assembles the packet into a form suitable for sending through a data - stream. For this base packet class, :attr:`~.SOF`, :attr:`~.payload`, - and :attr:`~.EOF` are assembled into one byte string. - - Returns: - bytes: The packed bytes. - """ - return struct.pack(f"B{len(self.payload)}sB", self.SOF, self.payload, self.EOF) - - @overload - @classmethod - def _unpack_payload(cls, data: Literal[b""]) -> None: - ... - - @overload - @classmethod - def _unpack_payload(cls, data: bytes) -> bytes: - ... - - @classmethod - def _unpack_payload(cls, data: bytes) -> bytes | None: - """ - Attempts to obtain the raw data from a packed payload. - - Raises: - InvalidStartFlagException: The start flag (first unsigned integer) of - the payload is invalid. - InvalidEndFlagException: The end flag (last unsigned integer) of the payload - is invalid. - - Returns: - Optional[bytes]: The raw data inside the packet payload. If the data - has zero length, then ``None`` is returned. - """ - payload_len = len(data) - 2 - if payload_len < 1: - return None - sof, payload, eof = struct.unpack(f"B{payload_len}sB", data) - if sof != cls.SOF: - raise InvalidStartFlagException(sof) - if eof != cls.EOF: - raise InvalidEndFlagException(eof) - return payload - - @classmethod - def from_bytes(cls: type[T], data: bytes) -> T | None: - """ - Parses a packet from a packed bytes string into a Packet instance. - - Args: - data (bytes): The packed data to construct the Packet instance from. - - Returns: - Optional[Packet]: The packet instance. ``None`` is returned if the packet - contains an empty payload. - """ - payload = cls._unpack_payload(data) - if payload is None: - return None - return cls(payload) - - def __repr__(self): - return f"{self.__class__.__name__}(payload={self.payload})" - - @classmethod - def read_packet( - cls: type[T], stream: serial.Serial | SimulatedUSBtoCAN - ) -> T | None: - """ - Read a packet with a known size from a serial device - - Args: - stream (Union[serial.Serial, SimulatedUSBtoCAN]): A instance of a serial - device to read from. - - Raises: - InvalidStartFlagException: The start flag of the packet read was invalid. - InvalidEndFlagException: The end flag of the packet read was invalid. - - Returns: - Optional[Packet]: The read packet. If a packet was partially transmitted - (ie, starting with a character other than :attr:`~.SOF` or ending with - a character other than :attr:`~.EOF`), then ``None`` is returned. - """ - # Read until SOF is encourntered in case buffer contains the end of a previous packet - sof = None - for _ in range(10): - sof = stream.read(1) - if sof is None or len(sof) == 0: - return None - sof_int = int.from_bytes(sof, byteorder="big") - if sof_int == cls.SOF: - break - assert isinstance(sof, bytes) - sof_int = int.from_bytes(sof, byteorder="big") - if sof_int != cls.SOF: - raise InvalidStartFlagException(sof_int) - data = sof - eof = None - for _ in range(10): - eof = stream.read(1) - if eof is None or len(eof) == 0: - return None - data += eof - eof_int = int.from_bytes(eof, byteorder="big") - if eof_int == cls.EOF: - break - assert isinstance(eof, bytes) - eof_int = int.from_bytes(eof, byteorder="big") - if eof_int != cls.EOF: - raise InvalidEndFlagException(eof_int) - # print hexify(data) - return cls.from_bytes(data) - - -class ReceivePacket(Packet): - """ - Packet used to request data from the USB to CAN board. - - Attributes: - payload (bytes): The payload stored in the packet. - SOF (int): Flag used to mark the beginning of each packet. Equal to `0xC0`. - EOF (int): Flag used to mark the beginning of each packet. Equal to `0xC1`. - """ - - @property - def device(self) -> int: - """ - The device ID associated with the packet. - """ - return struct.unpack("B", self.payload[0:1])[0] - - @property - def data(self) -> bytes: - """ - The data inside the packet. - """ - return self.payload[2:-1] - - @property - def length(self): - """ - The length of the data to receive. - """ - return struct.unpack("B", self.payload[1:2])[0] - - @classmethod - def _calculate_checksum(cls, device_id, payload) -> int: - checksum = device_id + len(payload) + cls.SOF + cls.EOF - for byte in payload: - checksum += byte - return checksum % 16 - - @classmethod - def create_receive_packet(cls, device_id: int, payload: bytes) -> ReceivePacket: - """ - Creates a command packet to request data from a CAN device. - - Args: - device_id (int): The CAN device ID to request data from. - payload (bytes): The data to send in the packet. - - Returns: - ReceivePacket: The packet to request from the CAN device. - """ - if len(payload) > 8: - raise PayloadTooLargeException(len(payload)) - checksum = cls._calculate_checksum(device_id, payload) - data = struct.pack( - f"BB{len(payload)}sB", device_id, len(payload), payload, checksum - ) - return cls(data) - - @classmethod - def from_bytes(cls, data: bytes) -> ReceivePacket: - """ - Creates a receive packet from packed bytes. This strips the checksum from - the bytes and then unpacks the data to gain the raw payload. - - Raises: - ChecksumException: The checksum found in the data differs from that - found in the data. - - Returns: - ReceivePacket: The packet constructed from the packed bytes. - """ - expected_checksum = 0 - for byte in data[:-2]: - expected_checksum += byte - expected_checksum += data[-1] - expected_checksum %= 16 - # expected_checksum = cls._calculate_checksum(data[0], data[:-1]) - real_checksum = data[-2] - if real_checksum != expected_checksum: - raise ChecksumException(expected_checksum, real_checksum) - payload = cls._unpack_payload(data) - return cls(payload) - - -def can_id(task_group, ecu_number): - return (task_group & 240) + (ecu_number & 15) - - -class CommandPacket(Packet): - """ - Represents a packet to the CAN board from the motherboard. This packet can - either request data from a device or send data to a device. - - .. container:: operations - - .. describe:: bytes(x) - - Assembles the packet into a form suitable for sending through a data - stream. - - Attributes: - payload (bytes): The payload stored in the packet. - SOF (int): Flag used to mark the beginning of each packet. Equal to `0xC0`. - EOF (int): Flag used to mark the beginning of each packet. Equal to `0xC1`. - """ - - @property - def length_byte(self) -> int: - """ - The first header byte which encodes the length and the receive flag. - - Returns: - :class:`int` - """ - return struct.unpack("B", self.payload[0:1])[0] - - @property - def is_receive(self) -> bool: - """ - True if this CommandPacket is requesting data. - - Returns: - :class:`bool` - """ - return bool(self.length_byte & 128) - - @property - def length(self) -> int: - """ - The number of bytes of data sent or requested. - - Returns: - :class:`int` - """ - return (self.length_byte & 7) + 1 - - @property - def filter_id(self) -> int: - """ - An integer representing the CAN device ID specified by this packet. - - Returns: - :class:`int` - """ - return struct.unpack("B", self.payload[1 : 1 + 1])[ - 0 - ] # [1:1+1] range used to ensure bytes, not [1] for int - - @property - def data(self) -> bytes: - """ - Returns: - bytes: The data to be sent. - """ - return self.payload[2:] - - @classmethod - def _create_command_packet( - cls, length_byte: int, filter_id: int, data: bytes = b"" - ) -> CommandPacket: - """ - Creates a command packet. - - .. warning:: - - This method should rarely be used. Instead, use :meth:`.create_send_packet` - or :meth:`.create_request_packet` instead. - - Args: - length_byte (int): The first header byte - filter_id (int): The second header byte - data (bytes): Optional data payload when this is a send command. Defaults - to an empty byte string. - - Raises: - PayloadTooLargeException: The payload is larger than 8 bytes. - """ - if len(data) > 8: - raise PayloadTooLargeException(len(data)) - payload = struct.pack(f"BB{len(data)}s", length_byte, filter_id, data) - return cls(payload) - - @classmethod - def create_send_packet(cls, data: bytes, can_id: int = 0) -> CommandPacket: - """ - Creates a command packet to send data to the CAN bus from the motherboard. - - Args: - data (bytes): The data payload. - can_id (int): The ID of the device to send data to. Defaults to 0. - - Raises: - PayloadTooLargeException: The payload is larger than 8 bytes. - - Returns: - CommandPacket: The packet responsible for sending information to the CAN bus - from the motherboard. - """ - length_byte = len(data) - 1 - return cls._create_command_packet(length_byte, can_id, data) - - @classmethod - def create_request_packet( - cls, filter_id: int, receive_length: int - ) -> CommandPacket: - """ - Creates a command packet to request data from a CAN device. - - Args: - filter_id (int): The CAN device ID to request data from. - receive_length (int): The number of bytes to request. - - Returns: - CommandPacket: The command packet responsible for requesting data from - a CAN device. - """ - length_byte = (receive_length - 1) | 128 - return cls._create_command_packet(length_byte, filter_id) - - def calculate_checksum(self, data: bytes) -> int: - checksum = 0 - for byte in data: - checksum += byte - return checksum % 16 - - @overload - @classmethod - def from_bytes(cls, data: Literal[b""]) -> None: - ... - - @overload - @classmethod - def from_bytes(cls: type[T], data: bytes) -> T: - ... - - @classmethod - def from_bytes(cls: type[T], data: bytes) -> T | None: - checksum_expected = 0 - checksum_expected += data[0] - checksum_expected += data[1] & 135 - for byte in data[2:]: - checksum_expected += byte - checksum_expected %= 16 - checksum_real = (data[1] & 120) >> 3 - if checksum_expected != checksum_real: - raise ChecksumException(checksum_expected, checksum_real) - payload = cls._unpack_payload(data) - if payload is None: - return None - return cls(payload) - - def __bytes__(self) -> bytes: - data = super().__bytes__() - checksum = 0 - for byte in data: - checksum += byte - checksum %= 16 - header_byte = (checksum << 3) | data[1] - data = data[:1] + chr(header_byte).encode() + data[2:] - return data - - def __str__(self): - return "CommandPacket(filter_id={}, is_receive={}, receive_length={})".format( - self.filter_id, self.is_receive, self.length - ) diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device.test b/mil_common/drivers/mil_usb_to_can/test/adder_device.test index 54bcafd37..f9eb8170d 100644 --- a/mil_common/drivers/mil_usb_to_can/test/adder_device.test +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device.test @@ -5,9 +5,9 @@ device_handles: - "8": mil_usb_to_can.ExampleAdderDeviceHandle + mil_usb_to_can.example.ExampleAdderDeviceHandle: [mil_usb_to_can.example.ExampleAdderResponsePacket] simulated_devices: - "8": mil_usb_to_can.ExampleSimulatedAdderDevice + mil_usb_to_can.example.ExampleSimulatedAdderDevice: [mil_usb_to_can.example.ExampleAdderRequestPacket] diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device.test b/mil_common/drivers/mil_usb_to_can/test/echo_device.test index ae64c9b54..9bfd44380 100644 --- a/mil_common/drivers/mil_usb_to_can/test/echo_device.test +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device.test @@ -5,9 +5,9 @@ device_handles: - "8": mil_usb_to_can.ExampleEchoDeviceHandle + mil_usb_to_can.example.ExampleEchoDeviceHandle: [mil_usb_to_can.example.ExampleEchoResponsePacket] simulated_devices: - "8": mil_usb_to_can.ExampleSimulatedEchoDevice + mil_usb_to_can.example.ExampleSimulatedEchoDevice: [mil_usb_to_can.example.ExampleEchoRequestPacket] diff --git a/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py b/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py deleted file mode 100755 index b3ee379dc..000000000 --- a/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py +++ /dev/null @@ -1,50 +0,0 @@ -#! /usr/bin/env python3 -import random -import string -import struct -import unittest - -import rostest -from mil_usb_to_can import ApplicationPacket, CommandPacket - - -class BasicApplicationPacketTest(unittest.IsolatedAsyncioTestCase): - """ - Tests basic application packt functionality. - """ - - def test_incorrect_identifiers(self): - packet = ApplicationPacket(256, b"test") - with self.assertRaises(struct.error): - bytes(packet) - packet = ApplicationPacket(-1, b"test") - with self.assertRaises(struct.error): - bytes(packet) - packet = ApplicationPacket("a", b"test") - with self.assertRaises(struct.error): - bytes(packet) - - def test_format(self): - letter = random.choice(string.ascii_letters) - packet = ApplicationPacket(37, letter.encode()) - self.assertEqual(bytes(packet), struct.pack("B1s", 37, letter.encode())) - self.assertEqual( - ApplicationPacket.from_bytes(bytes(packet), expected_identifier=37), packet - ) - - def test_assembled(self): - letter = random.choice(string.ascii_letters) - packet = ApplicationPacket(37, letter.encode()) - command_packet = bytes(CommandPacket(bytes(packet))) - data = struct.pack(f"B{len(bytes(packet))}sB", 0xC0, bytes(packet), 0xC1) - checksum = CommandPacket(b"").calculate_checksum(data) - header_byte = (checksum << 3) | data[1] - data = data[:1] + chr(header_byte).encode() + data[2:] - self.assertEqual(command_packet, data) - - -if __name__ == "__main__": - rostest.rosrun( - "mil_usb_to_can", "test_application_packets", BasicApplicationPacketTest - ) - unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/test_packets.py b/mil_common/drivers/mil_usb_to_can/test/test_packets.py new file mode 100755 index 000000000..e00fed023 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/test_packets.py @@ -0,0 +1,54 @@ +#! /usr/bin/env python3 +import unittest +from dataclasses import dataclass + +import rostest +from mil_usb_to_can import Packet +from mil_usb_to_can.packet import SYNC_CHAR_1, SYNC_CHAR_2 + + +@dataclass +class TestPacket(Packet, msg_id=0x47, subclass_id=0x44, payload_format="BHf"): + example_bool: bool + example_int: int + example_float: float + + +class BasicApplicationPacketTest(unittest.IsolatedAsyncioTestCase): + """ + Tests basic application packt functionality. + """ + + def test_simple_packet(self): + packet = TestPacket(False, 42, 3.14) + self.assertEqual(packet.msg_id, 0x47) + self.assertEqual(packet.subclass_id, 0x44) + self.assertEqual(packet.payload_format, "BHf") + self.assertEqual(packet.example_bool, False) + self.assertEqual(packet.example_int, 42) + self.assertEqual(packet.example_float, 3.14) + + def test_assembled_packet(self): + packet = TestPacket(False, 42, 3.14) + assembled = bytes(TestPacket(False, 42, 3.14)) + self.assertEqual(assembled[0], SYNC_CHAR_1) + self.assertEqual(assembled[1], SYNC_CHAR_2) + self.assertEqual(assembled[2], packet.msg_id) + self.assertEqual(assembled[3], packet.subclass_id) + + def test_comparisons(self): + packet = TestPacket(False, 42, 3.14) + packet_two = TestPacket(False, 42, 3.14) + self.assertEqual(packet, packet_two) + with self.assertRaises(TypeError): + packet < packet_two + with self.assertRaises(TypeError): + packet > packet_two + + +if __name__ == "__main__": + packet = TestPacket(False, 42, 3.14) + rostest.rosrun( + "mil_usb_to_can", "test_application_packets", BasicApplicationPacketTest + ) + unittest.main() From 2df105d5786050ee70e84c0ccee56cd4935d4af0 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sun, 22 Jan 2023 13:49:08 -0500 Subject: [PATCH 03/38] Update sub8_actuator_board package to use new mil_usb_to_can --- .../sub8_actuator_board/handle.py | 48 +++--- .../sub8_actuator_board/packets.py | 142 ++---------------- .../sub8_actuator_board/simulation.py | 36 ++--- 3 files changed, 49 insertions(+), 177 deletions(-) diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py index 7749df29e..c6fe1d41b 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py @@ -3,9 +3,7 @@ from mil_usb_to_can import CANDeviceHandle from sub8_actuator_board.srv import SetValve, SetValveRequest -from .packets import SEND_ID, CommandMessage, FeedbackMessage, InvalidAddressException - -__author__ = "John Morin" +from .packets import ActuatorPollRequest, ActuatorPollResponse, ActuatorSetPacket class ActuatorBoard(CANDeviceHandle): @@ -14,9 +12,12 @@ class ActuatorBoard(CANDeviceHandle): it inherits from the :class:`CANDeviceHandle` class. """ + _recent_response: ActuatorPollResponse | None + def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self._service = rospy.Service("/set_valve", SetValve, self.set_valve) + self._recent_response = None def set_valve(self, req: SetValveRequest) -> dict: """ @@ -30,35 +31,30 @@ def set_valve(self, req: SetValveRequest) -> dict: dict: List of information which is casted into a SetValveResponse. """ # Send board command to open or close specified valve - try: - message = CommandMessage.create_command_message( - address=req.actuator, write=True, on=req.opened - ) - except InvalidAddressException as e: - return {"success": False, "message": str(e)} - self.send_data(bytes(message), can_id=SEND_ID) + message = ActuatorSetPacket(address=req.actuator, open=req.opened) + self.send_data(message) rospy.loginfo( "Set valve {} {}".format(req.actuator, "opened" if req.opened else "closed") ) # Wait some time for board to process command rospy.sleep(0.01) # Request the status of the valve just commanded to ensure it worked - self.send_data( - bytes( - CommandMessage.create_command_message(address=req.actuator, write=False) - ), - can_id=SEND_ID, - ) - return {"success": True} - - def on_data(self, data) -> None: + self.send_data(ActuatorPollRequest()) + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(1): + if self._recent_response is not None: + break + response = { + "success": self._recent_response.values & (1 << req.actuator) + if self._recent_response is not None + else False + } + self._recent_response = None + return response + + def on_data(self, packet) -> None: """ Process data received from board. """ - # Ensure packet contains correct identifier byte - if FeedbackMessage.IDENTIFIER != ord(data[0]): - rospy.logwarn(f"Received packet with wrong identifier byte {ord(data[0])}") - return - # Parse message and (for now) just log it - message = FeedbackMessage.from_bytes(data) - rospy.loginfo(f"ActuatorBoard received {message}") + assert isinstance(packet, ActuatorPollResponse) + self._recent_response = packet diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py index 1cc2016ac..9c29ecfd3 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py @@ -1,137 +1,19 @@ -import struct +from dataclasses import dataclass -from mil_usb_to_can import ApplicationPacket +from mil_usb_to_can import Packet -# CAN ID for the channel from MOBO to actuator board -SEND_ID = 0x51 +@dataclass +class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="BB"): + address: int + open: bool -class CommandMessage(ApplicationPacket): - """ - Represents a packet sent from the motherboard to the actuator board. Being a packet, - inherits from :class:`ApplicationPacket`. - Attributes: - IDENTIFIER (int): The ASCII value of "A". - NUM_VALVES (int): The number of valves. Set to ``12`` currently. - """ +@dataclass +class ActuatorPollRequest(Packet, msg_id=0x03, subclass_id=0x01, payload_format=""): + pass - IDENTIFIER = ord("A") - NUM_VALVES = 12 - @property - def address(self) -> int: - """ - The integer address of this command. - - :rtype: int - """ - return struct.unpack("B", self.payload[0])[0] - - @property - def write(self) -> bool: - """ - If True, this message sets the valve open or closed. If False, this - message requests the status of a valve. - - :rtype: bool - """ - return struct.unpack("B", self.payload[1])[0] == 1 - - @property - def on(self) -> bool: - """ - If True, this message commanded the valve to be open. Else, the valve - is commanded to be closed. - - This parameter only has any effect if :attr:`~.write` is true. - - :rtype: bool - """ - return struct.unpack("B", self.payload[2])[0] == 1 - - @classmethod - def create_command_message(cls, address: int = 0, write: int = 0, on: int = 0): - """ - Create a new command message. - - Args: - address (int): Address of valve, ranging from 0-11. - write (int): If true, set valve open or close. If False, request - the status of the specified valve. - on (int): If true, command valve to open. If write if False, this has no effect - - Raises: - InvalidAddressException: The valve address is not valid, and is outside - of the 0-11 range. - """ - if address < 0 or address >= cls.NUM_VALVES: - raise InvalidAddressException(address) - write_byte = 1 if write else 0 - on_byte = 1 if on else 0 - payload = struct.pack("BBB", address, write_byte, on_byte) - return cls(cls.IDENTIFIER, payload) - - def __str__(self): - return "CommandMessage(address={}, write={}, on={})".format( - self.address, self.write, self.on - ) - - -class InvalidAddressException(RuntimeError): - """ - Exception noting that the requested valve address is outside of the range available - on the sub. Inherits from :class:`RuntimeError`. - """ - - def __init__(self, address: int): - super().__init__( - "Attempted to command valve {}, but valid addresses are only [0,{}]".format( - address, CommandMessage.NUM_VALVES - 1 - ) - ) - - -class FeedbackMessage(ApplicationPacket): - """ - Represents a message received from the actuator board sent to the motherboard - - Attributes: - IDENITIFER (int): The packet identifier. Set to the ASCII value of "A". - """ - - IDENTIFIER = ord("A") - - @property - def address(self) -> int: - """ - The address of valve referenced by this command. Should be between 0-11. - - :rtype: int - """ - return struct.unpack("B", self.payload[0])[0] - - @property - def on(self) -> bool: - """ - If True, valve is currently open. If False, valve is currently closed. - - :rtype: bool - """ - return struct.unpack("B", self.payload[1])[0] == 1 - - @classmethod - def create_feedback_message(cls, address: int = 0, on: bool = False): - """ - Create a feedback message. - - Args: - address (int): valve address this status references, integer 0-11 - on (bool): If True, valve is open - """ - on_byte = 1 if on else 0 - payload = struct.pack("BBB", address, on_byte, 0) - return cls(cls.IDENTIFIER, payload) - - def __str__(self): - return f"FeedbackMessage(address={self.address}, on={self.on})" +@dataclass +class ActuatorPollResponse(Packet, msg_id=0x03, subclass_id=0x02, payload_format=""): + values: int diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py index c57ee9429..c86e3ae06 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py @@ -1,10 +1,10 @@ #!/usr/bin/python3 -from mil_usb_to_can import SimulatedCANDevice +from mil_usb_to_can import AckPacket, SimulatedCANDeviceHandle -from .packets import SEND_ID, CommandMessage, FeedbackMessage +from .packets import ActuatorPollRequest, ActuatorPollResponse, ActuatorSetPacket -class ActuatorBoardSimulation(SimulatedCANDevice): +class ActuatorBoardSimulation(SimulatedCANDeviceHandle): """ Simulator for the communication of the actuator board. @@ -15,32 +15,26 @@ class ActuatorBoardSimulation(SimulatedCANDevice): def __init__(self, *args, **kwargs): # Tracks the status of the 12 valves - self.status = {i: False for i in range(12)} + self.status = {i: False for i in range(4)} super().__init__(*args, **kwargs) - def on_data(self, data, can_id) -> None: + def on_data(self, packet: ActuatorSetPacket | ActuatorPollRequest) -> None: """ Processes data received from motherboard / other devices. For each message received, the class' status attribute is updated if the message is asking to write, otherwise a feedback message is constructed and sent back. """ - # If from wrong ID, ignore - if can_id != SEND_ID: - return - - # First byte should be the identifier char for the command message - assert CommandMessage.IDENTIFIER == ord(data[0]) - - # Parse message - message = CommandMessage.from_bytes(data) - # If message is writing a valve, store this change in the internal dictionary - if message.write: - self.status[message.address] = message.on + if isinstance(packet, ActuatorSetPacket): + self.status[packet.address] = packet.open + self.send_data(bytes(AckPacket())) # If message is a status request, send motherboard the status of the requested valve - else: - response = FeedbackMessage.create_feedback_message( - address=message.address, on=self.status[message.address] + elif isinstance(packet, ActuatorPollRequest): + self.send_data( + bytes( + ActuatorPollResponse( + int("".join(str(x) for x in self.status), base=2) + ) + ) ) - self.send_data(bytes(response)) From feac913331fa2dc635bf397813971e106c875d12 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sun, 22 Jan 2023 14:54:53 -0500 Subject: [PATCH 04/38] Add packet listing and remove old packet classes --- docs/reference/can.rst | 34 ++++++++++++++++++++++++++++++ docs/subjugator/reference.rst | 39 ----------------------------------- 2 files changed, 34 insertions(+), 39 deletions(-) diff --git a/docs/reference/can.rst b/docs/reference/can.rst index d5759225d..05fac26ac 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -51,6 +51,40 @@ packets also have a special byte containing a slightly modified checksum formula The checksum in all packets is found by adding up all bytes in the byte string, including the start/end flags, and then using modulo 16 on this result. +Packet Listing +^^^^^^^^^^^^^^ +Below is a listing of all available packets. + ++------------+--------------+-------------------------------------------------------------------------+ +| Message ID | Subclass ID | Class | ++============+==============+=========================================================================+ +| 0x00 | 0x00 | :class:`mil_usb_to_can.NackPacket` | ++ (Meta) +--------------+-------------------------------------------------------------------------+ +| | 0x01 | :class:`mil_usb_to_can.AckPacket` | ++------------+--------------+-------------------------------------------------------------------------+ +| 0x01 | 0x00 | :class:`sub8_thrust_and_kill_board.HeartbeatPacket` | ++ (Thrust/ +--------------+-------------------------------------------------------------------------+ +| Kill) | 0x01 | :class:`sub8_thrust_and_kill_board.ThrustSetPacket` | ++ +--------------+-------------------------------------------------------------------------+ +| | 0x02 | :class:`sub8_thrust_and_kill_board.KillSetPacket` | ++ +--------------+-------------------------------------------------------------------------+ +| | 0x03 | :class:`sub8_thrust_and_kill_board.KillReceivePacket` | ++------------+--------------+-------------------------------------------------------------------------+ +| 0x02 | 0x00 | :class:`sub8_battery_monitor_board.BatteryPollRequestPacket` | ++ (Battery +--------------+-------------------------------------------------------------------------+ +| Monitor) | 0x01 | :class:`sub8_battery_monitor_board.BatteryPollResponsePacket` | ++------------+--------------+-------------------------------------------------------------------------+ +| 0x03 | 0x00 | :class:`sub8_actuator_board.ActuatorSetPacket` | ++ (Actuator +--------------+-------------------------------------------------------------------------+ +| Board) | 0x01 | :class:`sub8_actuator_board.ActuatorPollRequestPacket` | ++ +--------------+-------------------------------------------------------------------------+ +| | 0x02 | :class:`sub8_actuator_board.ActuatorPollResponsePacket` | ++------------+--------------+-------------------------------------------------------------------------+ +| 0x04 | 0x00 | :class:`sub8_system_status_board.SetLedRequestPacket` | +| (System | | | +| Status) | | | ++------------+--------------+-------------------------------------------------------------------------+ + CANDeviceHandle ^^^^^^^^^^^^^^^ .. attributetable:: mil_usb_to_can.CANDeviceHandle diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index a328730e9..9e677aabb 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -83,10 +83,6 @@ SetValve :type: bool -Exceptions ----------- -.. autoclass:: sub8_actuator_board.InvalidAddressException - Actuator Board -------------- @@ -104,20 +100,6 @@ ActuatorBoardSimulation .. autoclass:: sub8_actuator_board.ActuatorBoardSimulation :members: -CommandMessage -^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.CommandMessage - -.. autoclass:: sub8_actuator_board.CommandMessage - :members: - -FeedbackMessage -^^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.FeedbackMessage - -.. autoclass:: sub8_actuator_board.FeedbackMessage - :members: - Thrust and Kill Board --------------------- @@ -128,27 +110,6 @@ ThrusterAndKillBoard .. autoclass:: sub8_thrust_and_kill_board.ThrusterAndKillBoard :members: -KillMessage -^^^^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.KillMessage - -.. autoclass:: sub8_thrust_and_kill_board.KillMessage - :members: - -HeartbeatMessage -^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.HeartbeatMessage - -.. autoclass:: sub8_thrust_and_kill_board.HeartbeatMessage - :members: - -ThrustPacket -^^^^^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.ThrustPacket - -.. autoclass:: sub8_thrust_and_kill_board.ThrustPacket - :members: - ThrusterAndKillBoardSimulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. attributetable:: sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation From 99d3a9b8623297f9f9745d5c4dfc96bf5ec5b769 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Mon, 23 Jan 2023 11:32:36 -0500 Subject: [PATCH 05/38] Update sub8_actuator_board for new mil_usb_to_can rewrite --- .../sub8_actuator_board/CMakeLists.txt | 1 + .../sub8_actuator_board/__init__.py | 6 +++- .../sub8_actuator_board/handle.py | 33 ++++++++++------- .../sub8_actuator_board/packets.py | 29 +++++++++++++-- .../sub8_actuator_board/simulation.py | 16 ++++++--- .../test/simulated_board.test | 15 ++++++++ .../test/simulated_board_test.py | 35 +++++++++++++++++++ docs/subjugator/reference.rst | 21 +++++++++++ 8 files changed, 136 insertions(+), 20 deletions(-) create mode 100644 SubjuGator/drivers/sub8_actuator_board/test/simulated_board.test create mode 100755 SubjuGator/drivers/sub8_actuator_board/test/simulated_board_test.py diff --git a/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt b/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt index 45856db1c..0db119002 100644 --- a/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt +++ b/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt @@ -9,5 +9,6 @@ add_service_files( FILES SetValve.srv ) +add_rostest(test/simulated_board.test) generate_messages() catkin_package(CATKIN_DEPENDS message_runtime) diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py index 97f6a1868..8d260d68e 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py @@ -1,3 +1,7 @@ from .handle import ActuatorBoard -from .packets import CommandMessage, FeedbackMessage, InvalidAddressException +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) from .simulation import ActuatorBoardSimulation diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py index c6fe1d41b..f004046e1 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py @@ -1,9 +1,15 @@ #!/usr/bin/env python3 +from __future__ import annotations + import rospy -from mil_usb_to_can import CANDeviceHandle +from mil_usb_to_can import AckPacket, CANDeviceHandle from sub8_actuator_board.srv import SetValve, SetValveRequest -from .packets import ActuatorPollRequest, ActuatorPollResponse, ActuatorSetPacket +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) class ActuatorBoard(CANDeviceHandle): @@ -12,7 +18,7 @@ class ActuatorBoard(CANDeviceHandle): it inherits from the :class:`CANDeviceHandle` class. """ - _recent_response: ActuatorPollResponse | None + _recent_response: ActuatorPollResponsePacket | None def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -39,22 +45,25 @@ def set_valve(self, req: SetValveRequest) -> dict: # Wait some time for board to process command rospy.sleep(0.01) # Request the status of the valve just commanded to ensure it worked - self.send_data(ActuatorPollRequest()) + self.send_data(ActuatorPollRequestPacket()) start = rospy.Time.now() while rospy.Time.now() - start < rospy.Duration(1): if self._recent_response is not None: break - response = { - "success": self._recent_response.values & (1 << req.actuator) - if self._recent_response is not None - else False - } + + success = False + if self._recent_response is not None: + if req.opened: + success = not (self._recent_response.values | (1 << req.actuator)) + else: + success = self._recent_response.values & (1 << req.actuator) + response = {"success": success} self._recent_response = None return response - def on_data(self, packet) -> None: + def on_data(self, packet: ActuatorPollResponsePacket | AckPacket) -> None: """ Process data received from board. """ - assert isinstance(packet, ActuatorPollResponse) - self._recent_response = packet + if isinstance(packet, ActuatorPollResponsePacket): + self._recent_response = packet diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py index 9c29ecfd3..ccc8ea4d7 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py @@ -5,15 +5,40 @@ @dataclass class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="BB"): + """ + Packet used by the actuator board to set a specific valve. + + Attributes: + address (int): The actuator ID to set. + open (bool): Whether to open the specified actuator. ``True`` requests opening, + ``False`` requests closing. + """ + address: int open: bool @dataclass -class ActuatorPollRequest(Packet, msg_id=0x03, subclass_id=0x01, payload_format=""): +class ActuatorPollRequestPacket( + Packet, msg_id=0x03, subclass_id=0x01, payload_format="" +): + """ + Packet used by the actuator board to request the status of all valves. + """ + pass @dataclass -class ActuatorPollResponse(Packet, msg_id=0x03, subclass_id=0x02, payload_format=""): +class ActuatorPollResponsePacket( + Packet, msg_id=0x03, subclass_id=0x02, payload_format="B" +): + """ + Packet used by the actuator board to return the status of all valves. + + Attributes: + values (int): The statues of all actuators. Bits 0-3 represent the opened + status of actuators 0-3. + """ + values: int diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py index c86e3ae06..062ff89a9 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py @@ -1,7 +1,13 @@ #!/usr/bin/python3 +from __future__ import annotations + from mil_usb_to_can import AckPacket, SimulatedCANDeviceHandle -from .packets import ActuatorPollRequest, ActuatorPollResponse, ActuatorSetPacket +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) class ActuatorBoardSimulation(SimulatedCANDeviceHandle): @@ -18,7 +24,7 @@ def __init__(self, *args, **kwargs): self.status = {i: False for i in range(4)} super().__init__(*args, **kwargs) - def on_data(self, packet: ActuatorSetPacket | ActuatorPollRequest) -> None: + def on_data(self, packet: ActuatorSetPacket | ActuatorPollRequestPacket) -> None: """ Processes data received from motherboard / other devices. For each message received, the class' status attribute is updated if the message is asking to write, otherwise @@ -30,11 +36,11 @@ def on_data(self, packet: ActuatorSetPacket | ActuatorPollRequest) -> None: self.send_data(bytes(AckPacket())) # If message is a status request, send motherboard the status of the requested valve - elif isinstance(packet, ActuatorPollRequest): + elif isinstance(packet, ActuatorPollRequestPacket): self.send_data( bytes( - ActuatorPollResponse( - int("".join(str(x) for x in self.status), base=2) + ActuatorPollResponsePacket( + int("".join(str(int(x)) for x in self.status.values()), base=2) ) ) ) diff --git a/SubjuGator/drivers/sub8_actuator_board/test/simulated_board.test b/SubjuGator/drivers/sub8_actuator_board/test/simulated_board.test new file mode 100644 index 000000000..1c9d5b55c --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/test/simulated_board.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + sub8_actuator_board.ActuatorBoard: [sub8_actuator_board.ActuatorPollResponsePacket] + simulated_devices: + sub8_actuator_board.ActuatorBoardSimulation: [sub8_actuator_board.ActuatorPollRequestPacket, sub8_actuator_board.ActuatorSetPacket] + + + + + diff --git a/SubjuGator/drivers/sub8_actuator_board/test/simulated_board_test.py b/SubjuGator/drivers/sub8_actuator_board/test/simulated_board_test.py new file mode 100755 index 000000000..24044e7a7 --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/test/simulated_board_test.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from sub8_actuator_board.srv import SetValve, SetValveRequest + + +class SimulatedBoardTest(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.srv = rospy.ServiceProxy("/set_valve", SetValve) + super().__init__(*args) + + def test_correct_response(self): + """ + Test we can get correct data through CAN bus at least ten times. + """ + self.srv.wait_for_service(1) + self.srv(SetValveRequest(0, True)) + self.srv(SetValveRequest(0, False)) + self.srv(SetValveRequest(0, False)) + self.srv(SetValveRequest(3, False)) + self.srv(SetValveRequest(4, False)) + + +if __name__ == "__main__": + rospy.init_node("simulated_board_test", anonymous=True) + import rostest + + rostest.rosrun("sub8_actuator_board", "simulated_board_test", SimulatedBoardTest) + unittest.main() diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index 9e677aabb..c1cd4bcee 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -100,6 +100,27 @@ ActuatorBoardSimulation .. autoclass:: sub8_actuator_board.ActuatorBoardSimulation :members: +ActuatorSetPacket +^^^^^^^^^^^^^^^^^ +.. attributetable:: sub8_actuator_board.ActuatorSetPacket + +.. autoclass:: sub8_actuator_board.ActuatorSetPacket + :members: + +ActuatorPollRequestPacket +^^^^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub8_actuator_board.ActuatorPollRequestPacket + +.. autoclass:: sub8_actuator_board.ActuatorPollRequestPacket + :members: + +ActuatorPollResponsePacket +^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub8_actuator_board.ActuatorPollResponsePacket + +.. autoclass:: sub8_actuator_board.ActuatorPollResponsePacket + :members: + Thrust and Kill Board --------------------- From 000bc5170f948230324922593dd7c64fc1ec0626 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Mon, 23 Jan 2023 11:35:56 -0500 Subject: [PATCH 06/38] Temporary sub8_thrust_and_kill_board fixes for docs build --- .../sub8_thrust_and_kill_board/__init__.py | 3 +- .../sub8_thrust_and_kill_board/handle.py | 18 +- .../sub8_thrust_and_kill_board/packets.py | 295 ++---------------- .../sub8_thrust_and_kill_board/simulation.py | 20 +- 4 files changed, 40 insertions(+), 296 deletions(-) diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py index 88dbf92c1..bb2f57684 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py @@ -1,4 +1,5 @@ from .handle import ThrusterAndKillBoard -from .packets import HeartbeatMessage, KillMessage, StatusMessage, ThrustPacket + +# from .packets import HeartbeatMessage, KillMessage, StatusMessage, ThrustPacket from .simulation import ThrusterAndKillBoardSimulation from .thruster import Thruster diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py index 98fd2ade5..3341f7938 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py @@ -7,14 +7,14 @@ from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse from subjugator_msgs.msg import Thrust -from .packets import ( - KILL_SEND_ID, - THRUST_SEND_ID, - HeartbeatMessage, - KillMessage, - StatusMessage, - ThrustPacket, -) +# from .packets import ( +# KILL_SEND_ID, +# THRUST_SEND_ID, +# HeartbeatMessage, +# KillMessage, +# StatusMessage, +# ThrustPacket, +# ) from .thruster import make_thruster_dictionary @@ -114,7 +114,7 @@ def on_command(self, msg: Thrust) -> None: ) self.send_data(bytes(packet), can_id=THRUST_SEND_ID) - def update_hw_kill(self, status: StatusMessage) -> None: + def update_hw_kill(self, status: "StatusMessage") -> None: """ If needed, update the hw-kill alarm so it reflects the latest status from the board. diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py index 456a56ebf..9bf682b99 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py @@ -1,243 +1,26 @@ -import struct -from collections import namedtuple +from dataclasses import dataclass -from mil_usb_to_can import ApplicationPacket +from mil_usb_to_can import Packet -# CAN channel to send thrust messages to -THRUST_SEND_ID = 0x21 -# CAN channel to send kill messages to -KILL_SEND_ID = 0x11 - -class KillMessage(ApplicationPacket): - """ - Represents a packet sent to kill/thrust board which contains - a command or response related to kill status. - - Inherits from :class:`~mil_usb_to_can.ApplicationPacket`. - - .. container:: operations - - .. describe:: str(x) - - Pretty-prints the class name and each of the three packet attributes. - - Attributes: - IDENTIFIER (int): The packet's identifier. Set equal to the ordinal value - of "K", or 75. - COMMAND (int): Identifier representing the packet as a command packet. - Equal to 67. This identifier should only be found in the first byte - of the payload. - RESPONSE (int): Identifier representing the packet as a response packet. - Equal to 82. This identifier should only be found in the first byte of - the payload. - HARD (int): Identifier representing the type of kill as a hard kill. Equal - to 72. This identifier is designed to be used in the second byte of the - payload. - SOFT (int): Identifier representing the type of kill as a soft kill. Equal - to 83. This identifier is designed to be used in the second byte of the - payload. - ASSERTED (int): Identifier equal to 65. This identifier is meant to be used - in the third byte of the payload. - UNASSERTED (int): Identifier equal to 85. This identifier is meant to be used - in the third byte of the payload. - """ - - IDENTIFIER = ord("K") - COMMAND = 0x43 - RESPONSE = 0x52 - HARD = 0x48 - SOFT = 0x53 - ASSERTED = 0x41 - UNASSERTED = 0x55 - PADDING = 0x00 - - @classmethod - def create_kill_message( - cls, command: bool = False, hard: bool = False, asserted: bool = False - ): - """ - Creates a kill message containing three bytes of information, specified - as parameters. - - Args: - command (bool): Whether to make the kill message a command. If ``False``, - then the packet will represent a response. - hard (bool): Whether to make the packet kill type hard. If ``False``, - then the packet will represent a soft kill. - asserted (bool): Whether to make the packet asserted. If ``False``, then - an unasserted packet is generated. - """ - command_byte = cls.COMMAND if command else cls.RESPONSE - hard_soft_byte = cls.HARD if hard else cls.SOFT - assert_unassert_byte = cls.ASSERTED if asserted else cls.UNASSERTED - payload = struct.pack( - "BBBB", command_byte, hard_soft_byte, assert_unassert_byte, cls.PADDING - ) - return cls(cls.IDENTIFIER, payload) - - @property - def is_command(self) -> bool: - """ - Whether the packet represents a command. - - Returns: - bool: The status of the packet's command/response type. - """ - return self.payload[0] == self.COMMAND - - @property - def is_response(self) -> bool: - """ - Whether the packet represents a response. - - Returns: - bool: The status of the packet's command/response type. - """ - return self.payload[0] == self.RESPONSE - - @property - def is_hard(self) -> bool: - """ - Whether the packet represents a hard kill. - - Returns: - bool: The status of the packet's hard/soft kill type. - """ - return self.payload[1] == self.HARD - - @property - def is_soft(self) -> bool: - """ - Whether the packet represents a soft kill. - - Returns: - bool: The status of the packet's hard/soft kill type. - """ - return self.payload[1] == self.SOFT - - @property - def is_asserted(self): - """ - Whether the packet represents an asserted packet. - - Returns: - bool: The status of the packet's asserteed/unasserted type. - """ - return self.payload[2] == self.ASSERTED - - @property - def is_unasserted(self): - """ - Whether the packet represents an unasserted packet. - - Returns: - bool: The status of the packet's asserteed/unasserted type. - """ - return self.payload[2] == self.UNASSERTED - - def __str__(self): - return "KillMessage(command={}, hard={}, asserted={})".format( - self.is_command, self.is_hard, self.is_asserted - ) - - -KillStatus = namedtuple( - "KillStatus", - [ - "heartbeat_lost", - "mobo_soft_kill", - "switch_soft_kill", - "soft_killed", - "hard_killed", - "thrusters_initializing", - "go_switch", - "soft_kill_switch", - "hard_kill_switch", - ], -) +@dataclass +class HeartbeatPacket(Packet, msg_id=0x01, subclass_id=0x00, payload_format=""): + pass -class StatusMessage(KillStatus): - BIT_MASK = KillStatus( - 1 << 3, 1 << 4, 1 << 5, 1 << 6, 1 << 7, 1 << 11, 1 << 12, 1 << 13, 1 << 14 - ) - STRUCT_FORMAT = "=h" - - def __new__(cls, *args): - """ - Constructs a new namedtuple to derive the class from. This can't be done - in __init__ because namedtuples are immutable. - """ - return super().__new__(cls, *args) - - @classmethod - def from_bytes(cls, data): - unpacked = struct.unpack(cls.STRUCT_FORMAT, data)[0] - args = [] - for field in KillStatus._fields: - args.append(bool(unpacked & getattr(cls.BIT_MASK, field))) - return cls(*args) - - def __bytes__(self): - out = 0 - for field in KillStatus._fields: - if getattr(self, field): - out = out | getattr(self.BIT_MASK, field) - return struct.pack(self.STRUCT_FORMAT, out) - - -class HeartbeatMessage(ApplicationPacket): +@dataclass +class ThrustSetRequestPacket( + Packet, msg_id=0x01, subclass_id=0x01, payload_format="Bf" +): """ - Represents the special heartbeat packet send to kill and thruster board. - - Inherits from :class:`~mil_usb_to_can.ApplicationPacket`. - - .. container:: operations - - .. describe:: str(x) - - Pretty-prints the class name. + Packet to set the speed of a specific thruster. Attributes: - IDENTIFIER (int): The packet identifier. Set equal to the ordinal value of - "H," or 72. - """ - - IDENTIFIER = ord("H") - - @classmethod - def create(cls): - """ - Creates a new heartbeat message to be sent. - """ - return cls(cls.IDENTIFIER, struct.pack("BB", ord("M"), 0)) - - def __str__(self): - return "HeartbeatMessage()" - - -class ThrustPacket(ApplicationPacket): - """ - Represents a command send to the thrust/kill board to set the PWM of a thruster. - - Inherits from :class:`~mil_usb_to_can.ApplicationPacket`. - - .. container:: operations - - .. describe:: str(x) - - Pretty-prints the class name and each of the two packet attributes, - the thruster ID and the command. - - Attributes: - IDENTIFIER (int): The packet identifier, equal to the ordinal value of "T," - or 84. - ID_MAPPING (Dict[str, int]): A dictionary mapping 3-letter thruster codes - to their respective IDs: + thruster_id (int): The ID of the thruster to set the speed of. The ID of + the thruster corresponds to a specific thruster: +--------+------+ - | Name | ID | + | name | id | +========+======+ | FLH | 0 | +--------+------+ @@ -255,53 +38,13 @@ class ThrustPacket(ApplicationPacket): +--------+------+ | BRV | 7 | +--------+------+ + speed (float): The speed to set the thruster to. """ - IDENTIFIER = ord("T") - ID_MAPPING = { - "FLH": 0, - "FRH": 1, - "FLV": 2, - "FRV": 3, - "BLH": 4, - "BRH": 5, - "BLV": 6, - "BRV": 7, - } - - @classmethod - def create_thrust_packet(cls, thruster_id: int, command: float): - """ - Creates a thruster packet given an ID and command. - - Args: - thruster_id (int): The ID of the thruster to create a packet for. - command (float): The command to associate with the packet. - """ - payload = struct.pack("=Bf", thruster_id, command) - return cls(cls.IDENTIFIER, payload) - - @property - def thruster_id(self) -> int: - """ - The thruster ID associated with the packet. - - Returns: - int: The ID. - """ - return struct.unpack("B", self.payload[0])[0] - - @property - def command(self) -> float: - """ - The command associated with the packet. + thruster_id: int + speed: float - Returns: - float: The associated command. - """ - return struct.unpack("f", self.payload[1:])[0] - def __str__(self): - return "ThrustPacket(thruster_id={}, command={})".format( - self.thruster_id, self.command - ) +@dataclass +class KillSetPacket(Packet, msg_id=0x01, subclass_id=0x02, payload_format=""): + set: bool diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py index faedf386c..64ef59930 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py @@ -1,19 +1,19 @@ #!/usr/bin/python3 import rospy -from mil_usb_to_can import SimulatedCANDevice +from mil_usb_to_can import SimulatedCANDeviceHandle from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse -from .packets import ( - KILL_SEND_ID, - THRUST_SEND_ID, - HeartbeatMessage, - KillMessage, - StatusMessage, - ThrustPacket, -) +# from .packets import ( +# KILL_SEND_ID, +# THRUST_SEND_ID, +# HeartbeatMessage, +# KillMessage, +# StatusMessage, +# ThrustPacket, +# ) -class ThrusterAndKillBoardSimulation(SimulatedCANDevice): +class ThrusterAndKillBoardSimulation(SimulatedCANDeviceHandle): """ Serial simulator for the thruster and kill board, providing services to simulate physical plug connections/disconnections. From ca9f6a368e0f445b33c17b44db8fee2e4cb8d51a Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Mon, 23 Jan 2023 12:08:56 -0500 Subject: [PATCH 07/38] Fix name of packet test file --- mil_common/drivers/mil_usb_to_can/test/basic.test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mil_common/drivers/mil_usb_to_can/test/basic.test b/mil_common/drivers/mil_usb_to_can/test/basic.test index 02b7e1969..80384f7f1 100644 --- a/mil_common/drivers/mil_usb_to_can/test/basic.test +++ b/mil_common/drivers/mil_usb_to_can/test/basic.test @@ -1,3 +1,3 @@ - + From 7b9b7241102adefe67b9fa1264e9f145ab18b922 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 24 Jan 2023 13:57:57 -0500 Subject: [PATCH 08/38] Add /get_valve service for sub_actuator_board, move sub8_actuator_board to sub_actuator_board, add processing time in simulated handle --- .../subjugator_launch/launch/can.launch | 4 +- .../subjugator_missions/sub_singleton.py | 2 +- .../sub8_actuator_board/CMakeLists.txt | 14 ---- .../drivers/sub8_actuator_board/package.xml | 15 ---- .../drivers/sub8_actuator_board/setup.py | 9 --- .../sub8_actuator_board/srv/SetValve.srv | 6 -- .../sub8_actuator_board/__init__.py | 7 -- .../sub8_actuator_board/handle.py | 69 ------------------- .../sub8_actuator_board/packets.py | 44 ------------ .../sub8_actuator_board/simulation.py | 46 ------------- .../test/simulated_board.test | 15 ---- .../test/simulated_board_test.py | 35 ---------- docs/reference/can.rst | 6 +- docs/subjugator/reference.rst | 32 ++++----- 14 files changed, 22 insertions(+), 282 deletions(-) delete mode 100644 SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt delete mode 100644 SubjuGator/drivers/sub8_actuator_board/package.xml delete mode 100644 SubjuGator/drivers/sub8_actuator_board/setup.py delete mode 100644 SubjuGator/drivers/sub8_actuator_board/srv/SetValve.srv delete mode 100644 SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py delete mode 100644 SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py delete mode 100644 SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py delete mode 100644 SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py delete mode 100644 SubjuGator/drivers/sub8_actuator_board/test/simulated_board.test delete mode 100755 SubjuGator/drivers/sub8_actuator_board/test/simulated_board_test.py diff --git a/SubjuGator/command/subjugator_launch/launch/can.launch b/SubjuGator/command/subjugator_launch/launch/can.launch index 525671ace..9023b1dfc 100644 --- a/SubjuGator/command/subjugator_launch/launch/can.launch +++ b/SubjuGator/command/subjugator_launch/launch/can.launch @@ -13,10 +13,10 @@ # List of python device handle classes device_handles: "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard - "83": sub8_actuator_board.ActuatorBoard + "83": sub_actuator_board.ActuatorBoard simulated_devices: "18": sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation - "83": sub8_actuator_board.ActuatorBoardSimulation + "83": sub_actuator_board.ActuatorBoardSimulation diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py b/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py index 0ba1868ee..43bfae3af 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py @@ -34,7 +34,7 @@ from std_srvs.srv import SetBool, SetBoolRequest, Trigger, TriggerRequest # from subjugator_msgs.srv import SetValve, SetValveRequest -from sub8_actuator_board.srv import SetValve, SetValveRequest +from sub_actuator_board.srv import SetValve, SetValveRequest from subjugator_msgs.srv import ( VisionRequest, VisionRequest2D, diff --git a/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt b/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt deleted file mode 100644 index 0db119002..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sub8_actuator_board) -find_package(catkin REQUIRED COMPONENTS - message_generation - mil_usb_to_can -) -catkin_python_setup() -add_service_files( - FILES - SetValve.srv -) -add_rostest(test/simulated_board.test) -generate_messages() -catkin_package(CATKIN_DEPENDS message_runtime) diff --git a/SubjuGator/drivers/sub8_actuator_board/package.xml b/SubjuGator/drivers/sub8_actuator_board/package.xml deleted file mode 100644 index 2aec91af8..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - sub8_actuator_board - 0.0.0 - The sub8_actuator_board package - Kevin Allen - MIT - Kevin Allen - catkin - message_generation - mil_usb_to_can - mil_usb_to_can - message_runtime - mil_usb_to_can - diff --git a/SubjuGator/drivers/sub8_actuator_board/setup.py b/SubjuGator/drivers/sub8_actuator_board/setup.py deleted file mode 100644 index afe33361a..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from catkin_pkg.python_setup import generate_distutils_setup -from setuptools import setup - -# Fetch values from package.xml -setup_args = generate_distutils_setup(packages=["sub8_actuator_board"]) - -setup(**setup_args) diff --git a/SubjuGator/drivers/sub8_actuator_board/srv/SetValve.srv b/SubjuGator/drivers/sub8_actuator_board/srv/SetValve.srv deleted file mode 100644 index 837be5cd7..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/srv/SetValve.srv +++ /dev/null @@ -1,6 +0,0 @@ -# Service call to open or close a pneumatic valve -uint8 actuator # ID of valve -bool opened # If True, valve will open; if False, valve will close ---- -bool success # False if there were any problems with the request. -string message # Description of error if success is false diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py deleted file mode 100644 index 8d260d68e..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -from .handle import ActuatorBoard -from .packets import ( - ActuatorPollRequestPacket, - ActuatorPollResponsePacket, - ActuatorSetPacket, -) -from .simulation import ActuatorBoardSimulation diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py deleted file mode 100644 index f004046e1..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python3 -from __future__ import annotations - -import rospy -from mil_usb_to_can import AckPacket, CANDeviceHandle -from sub8_actuator_board.srv import SetValve, SetValveRequest - -from .packets import ( - ActuatorPollRequestPacket, - ActuatorPollResponsePacket, - ActuatorSetPacket, -) - - -class ActuatorBoard(CANDeviceHandle): - """ - Device handle for the actuator board. Because this class implements a CAN device, - it inherits from the :class:`CANDeviceHandle` class. - """ - - _recent_response: ActuatorPollResponsePacket | None - - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self._service = rospy.Service("/set_valve", SetValve, self.set_valve) - self._recent_response = None - - def set_valve(self, req: SetValveRequest) -> dict: - """ - Called when the ``/set_valve`` service is requested. Creates a message to - control the valve and sends it through the inherited device handle. - - Args: - req (SetValveRequest): Request to set the valve. - - Returns: - dict: List of information which is casted into a SetValveResponse. - """ - # Send board command to open or close specified valve - message = ActuatorSetPacket(address=req.actuator, open=req.opened) - self.send_data(message) - rospy.loginfo( - "Set valve {} {}".format(req.actuator, "opened" if req.opened else "closed") - ) - # Wait some time for board to process command - rospy.sleep(0.01) - # Request the status of the valve just commanded to ensure it worked - self.send_data(ActuatorPollRequestPacket()) - start = rospy.Time.now() - while rospy.Time.now() - start < rospy.Duration(1): - if self._recent_response is not None: - break - - success = False - if self._recent_response is not None: - if req.opened: - success = not (self._recent_response.values | (1 << req.actuator)) - else: - success = self._recent_response.values & (1 << req.actuator) - response = {"success": success} - self._recent_response = None - return response - - def on_data(self, packet: ActuatorPollResponsePacket | AckPacket) -> None: - """ - Process data received from board. - """ - if isinstance(packet, ActuatorPollResponsePacket): - self._recent_response = packet diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py deleted file mode 100644 index ccc8ea4d7..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py +++ /dev/null @@ -1,44 +0,0 @@ -from dataclasses import dataclass - -from mil_usb_to_can import Packet - - -@dataclass -class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="BB"): - """ - Packet used by the actuator board to set a specific valve. - - Attributes: - address (int): The actuator ID to set. - open (bool): Whether to open the specified actuator. ``True`` requests opening, - ``False`` requests closing. - """ - - address: int - open: bool - - -@dataclass -class ActuatorPollRequestPacket( - Packet, msg_id=0x03, subclass_id=0x01, payload_format="" -): - """ - Packet used by the actuator board to request the status of all valves. - """ - - pass - - -@dataclass -class ActuatorPollResponsePacket( - Packet, msg_id=0x03, subclass_id=0x02, payload_format="B" -): - """ - Packet used by the actuator board to return the status of all valves. - - Attributes: - values (int): The statues of all actuators. Bits 0-3 represent the opened - status of actuators 0-3. - """ - - values: int diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py deleted file mode 100644 index 062ff89a9..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py +++ /dev/null @@ -1,46 +0,0 @@ -#!/usr/bin/python3 -from __future__ import annotations - -from mil_usb_to_can import AckPacket, SimulatedCANDeviceHandle - -from .packets import ( - ActuatorPollRequestPacket, - ActuatorPollResponsePacket, - ActuatorSetPacket, -) - - -class ActuatorBoardSimulation(SimulatedCANDeviceHandle): - """ - Simulator for the communication of the actuator board. - - Attributes: - status (Dict[int, bool]): The status of the valves. The keys are each of the valve IDs, - and the values are the statues of whether the valves are open. - """ - - def __init__(self, *args, **kwargs): - # Tracks the status of the 12 valves - self.status = {i: False for i in range(4)} - super().__init__(*args, **kwargs) - - def on_data(self, packet: ActuatorSetPacket | ActuatorPollRequestPacket) -> None: - """ - Processes data received from motherboard / other devices. For each message received, - the class' status attribute is updated if the message is asking to write, otherwise - a feedback message is constructed and sent back. - """ - # If message is writing a valve, store this change in the internal dictionary - if isinstance(packet, ActuatorSetPacket): - self.status[packet.address] = packet.open - self.send_data(bytes(AckPacket())) - - # If message is a status request, send motherboard the status of the requested valve - elif isinstance(packet, ActuatorPollRequestPacket): - self.send_data( - bytes( - ActuatorPollResponsePacket( - int("".join(str(int(x)) for x in self.status.values()), base=2) - ) - ) - ) diff --git a/SubjuGator/drivers/sub8_actuator_board/test/simulated_board.test b/SubjuGator/drivers/sub8_actuator_board/test/simulated_board.test deleted file mode 100644 index 1c9d5b55c..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/test/simulated_board.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - device_handles: - sub8_actuator_board.ActuatorBoard: [sub8_actuator_board.ActuatorPollResponsePacket] - simulated_devices: - sub8_actuator_board.ActuatorBoardSimulation: [sub8_actuator_board.ActuatorPollRequestPacket, sub8_actuator_board.ActuatorSetPacket] - - - - - diff --git a/SubjuGator/drivers/sub8_actuator_board/test/simulated_board_test.py b/SubjuGator/drivers/sub8_actuator_board/test/simulated_board_test.py deleted file mode 100755 index 24044e7a7..000000000 --- a/SubjuGator/drivers/sub8_actuator_board/test/simulated_board_test.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python3 -import unittest - -import rospy -from sub8_actuator_board.srv import SetValve, SetValveRequest - - -class SimulatedBoardTest(unittest.TestCase): - """ - Integration test for CAN2USB board driver. Talks - to a simulated CAN device which should add two integers - """ - - def __init__(self, *args): - self.srv = rospy.ServiceProxy("/set_valve", SetValve) - super().__init__(*args) - - def test_correct_response(self): - """ - Test we can get correct data through CAN bus at least ten times. - """ - self.srv.wait_for_service(1) - self.srv(SetValveRequest(0, True)) - self.srv(SetValveRequest(0, False)) - self.srv(SetValveRequest(0, False)) - self.srv(SetValveRequest(3, False)) - self.srv(SetValveRequest(4, False)) - - -if __name__ == "__main__": - rospy.init_node("simulated_board_test", anonymous=True) - import rostest - - rostest.rosrun("sub8_actuator_board", "simulated_board_test", SimulatedBoardTest) - unittest.main() diff --git a/docs/reference/can.rst b/docs/reference/can.rst index 05fac26ac..ac6e174f4 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -74,11 +74,11 @@ Below is a listing of all available packets. + (Battery +--------------+-------------------------------------------------------------------------+ | Monitor) | 0x01 | :class:`sub8_battery_monitor_board.BatteryPollResponsePacket` | +------------+--------------+-------------------------------------------------------------------------+ -| 0x03 | 0x00 | :class:`sub8_actuator_board.ActuatorSetPacket` | +| 0x03 | 0x00 | :class:`sub_actuator_board.ActuatorSetPacket` | + (Actuator +--------------+-------------------------------------------------------------------------+ -| Board) | 0x01 | :class:`sub8_actuator_board.ActuatorPollRequestPacket` | +| Board) | 0x01 | :class:`sub_actuator_board.ActuatorPollRequestPacket` | + +--------------+-------------------------------------------------------------------------+ -| | 0x02 | :class:`sub8_actuator_board.ActuatorPollResponsePacket` | +| | 0x02 | :class:`sub_actuator_board.ActuatorPollResponsePacket` | +------------+--------------+-------------------------------------------------------------------------+ | 0x04 | 0x00 | :class:`sub8_system_status_board.SetLedRequestPacket` | | (System | | | diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index c1cd4bcee..3fa945ac0 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -47,11 +47,11 @@ Services SetValve ^^^^^^^^ -.. attributetable:: sub8_actuator_board.srv.SetValveRequest +.. attributetable:: sub_actuator_board.srv.SetValveRequest -.. class:: sub8_actuator_board.srv.SetValveRequest +.. class:: sub_actuator_board.srv.SetValveRequest - The request class for the ``sub8_actuator_board/SetValve`` service. + The request class for the ``sub_actuator_board/SetValve`` service. .. attribute:: actuator @@ -65,11 +65,11 @@ SetValve :type: bool -.. attributetable:: sub8_actuator_board.srv.SetValveResponse +.. attributetable:: sub_actuator_board.srv.SetValveResponse -.. class:: sub8_actuator_board.srv.SetValveResponse +.. class:: sub_actuator_board.srv.SetValveResponse - The response class for the ``sub8_actuator_board/SetValve`` service. + The response class for the ``sub_actuator_board/SetValve`` service. .. attribute:: success @@ -88,37 +88,37 @@ Actuator Board ActuatorBoard ^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.ActuatorBoard +.. attributetable:: sub_actuator_board.ActuatorBoard -.. autoclass:: sub8_actuator_board.ActuatorBoard +.. autoclass:: sub_actuator_board.ActuatorBoard :members: ActuatorBoardSimulation ^^^^^^^^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.ActuatorBoardSimulation +.. attributetable:: sub_actuator_board.ActuatorBoardSimulation -.. autoclass:: sub8_actuator_board.ActuatorBoardSimulation +.. autoclass:: sub_actuator_board.ActuatorBoardSimulation :members: ActuatorSetPacket ^^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.ActuatorSetPacket +.. attributetable:: sub_actuator_board.ActuatorSetPacket -.. autoclass:: sub8_actuator_board.ActuatorSetPacket +.. autoclass:: sub_actuator_board.ActuatorSetPacket :members: ActuatorPollRequestPacket ^^^^^^^^^^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.ActuatorPollRequestPacket +.. attributetable:: sub_actuator_board.ActuatorPollRequestPacket -.. autoclass:: sub8_actuator_board.ActuatorPollRequestPacket +.. autoclass:: sub_actuator_board.ActuatorPollRequestPacket :members: ActuatorPollResponsePacket ^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.ActuatorPollResponsePacket +.. attributetable:: sub_actuator_board.ActuatorPollResponsePacket -.. autoclass:: sub8_actuator_board.ActuatorPollResponsePacket +.. autoclass:: sub_actuator_board.ActuatorPollResponsePacket :members: Thrust and Kill Board From 3c20a689f36048592bffddd71337efa056d50e5a Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Wed, 25 Jan 2023 11:39:02 -0500 Subject: [PATCH 09/38] Fix broken packet table, update packet listing for thrust_and_kill_board --- .../sub8_thrust_and_kill_board/__init__.py | 1 + .../sub8_thrust_and_kill_board/packets.py | 29 +++++- .../drivers/sub_actuator_board/CMakeLists.txt | 15 +++ .../drivers/sub_actuator_board/package.xml | 15 +++ .../drivers/sub_actuator_board/setup.py | 9 ++ .../sub_actuator_board/srv/GetValve.srv | 4 + .../sub_actuator_board/srv/SetValve.srv | 6 ++ .../sub_actuator_board/__init__.py | 7 ++ .../sub_actuator_board/handle.py | 93 +++++++++++++++++++ .../sub_actuator_board/packets.py | 44 +++++++++ .../sub_actuator_board/simulation.py | 55 +++++++++++ .../test/simulated_board.test | 15 +++ .../test/simulated_board_test.py | 44 +++++++++ docs/reference/can.rst | 6 +- docs/subjugator/reference.rst | 28 ++++++ 15 files changed, 364 insertions(+), 7 deletions(-) create mode 100644 SubjuGator/drivers/sub_actuator_board/CMakeLists.txt create mode 100644 SubjuGator/drivers/sub_actuator_board/package.xml create mode 100644 SubjuGator/drivers/sub_actuator_board/setup.py create mode 100644 SubjuGator/drivers/sub_actuator_board/srv/GetValve.srv create mode 100644 SubjuGator/drivers/sub_actuator_board/srv/SetValve.srv create mode 100644 SubjuGator/drivers/sub_actuator_board/sub_actuator_board/__init__.py create mode 100644 SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py create mode 100644 SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py create mode 100644 SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py create mode 100644 SubjuGator/drivers/sub_actuator_board/test/simulated_board.test create mode 100755 SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py index bb2f57684..52e8a1ed9 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py @@ -1,5 +1,6 @@ from .handle import ThrusterAndKillBoard # from .packets import HeartbeatMessage, KillMessage, StatusMessage, ThrustPacket +from .packets import HeartbeatPacket, KillReceivePacket, KillSetPacket, ThrustSetPacket from .simulation import ThrusterAndKillBoardSimulation from .thruster import Thruster diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py index 9bf682b99..f78fe7f16 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py @@ -5,13 +5,15 @@ @dataclass class HeartbeatPacket(Packet, msg_id=0x01, subclass_id=0x00, payload_format=""): + """ + Packet indicating a heartbeat. Sent both ways. + """ + pass @dataclass -class ThrustSetRequestPacket( - Packet, msg_id=0x01, subclass_id=0x01, payload_format="Bf" -): +class ThrustSetPacket(Packet, msg_id=0x01, subclass_id=0x01, payload_format="Bf"): """ Packet to set the speed of a specific thruster. @@ -46,5 +48,24 @@ class ThrustSetRequestPacket( @dataclass -class KillSetPacket(Packet, msg_id=0x01, subclass_id=0x02, payload_format=""): +class KillSetPacket(Packet, msg_id=0x01, subclass_id=0x02, payload_format="B"): + """ + Packet sent by the motherboard to set/unset the kill. + + Attributes: + set (bool): Whether the kill is set or unset. + """ + + set: bool + + +@dataclass +class KillReceivePacket(Packet, msg_id=0x01, subclass_id=0x03, payload_format="B"): + """ + Packet sent by the motherboard to set/unset the kill. + + Attributes: + set (bool): Whether the kill is set or unset. + """ + set: bool diff --git a/SubjuGator/drivers/sub_actuator_board/CMakeLists.txt b/SubjuGator/drivers/sub_actuator_board/CMakeLists.txt new file mode 100644 index 000000000..365e7d9a1 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sub_actuator_board) +find_package(catkin REQUIRED COMPONENTS + message_generation + mil_usb_to_can +) +catkin_python_setup() +add_service_files( + FILES + SetValve.srv + GetValve.srv +) +add_rostest(test/simulated_board.test) +generate_messages() +catkin_package(CATKIN_DEPENDS message_runtime) diff --git a/SubjuGator/drivers/sub_actuator_board/package.xml b/SubjuGator/drivers/sub_actuator_board/package.xml new file mode 100644 index 000000000..0ab794680 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/package.xml @@ -0,0 +1,15 @@ + + + sub_actuator_board + 0.0.0 + The sub_actuator_board package + Kevin Allen + MIT + Kevin Allen + catkin + message_generation + mil_usb_to_can + mil_usb_to_can + message_runtime + mil_usb_to_can + diff --git a/SubjuGator/drivers/sub_actuator_board/setup.py b/SubjuGator/drivers/sub_actuator_board/setup.py new file mode 100644 index 000000000..42e9f57e8 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/setup.py @@ -0,0 +1,9 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup + +# Fetch values from package.xml +setup_args = generate_distutils_setup(packages=["sub_actuator_board"]) + +setup(**setup_args) diff --git a/SubjuGator/drivers/sub_actuator_board/srv/GetValve.srv b/SubjuGator/drivers/sub_actuator_board/srv/GetValve.srv new file mode 100644 index 000000000..dff0ce39b --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/srv/GetValve.srv @@ -0,0 +1,4 @@ +# Service call to get the status of a pneumatic valve +uint8 actuator # ID of valve +--- +bool opened # Whether the valve has been opened diff --git a/SubjuGator/drivers/sub_actuator_board/srv/SetValve.srv b/SubjuGator/drivers/sub_actuator_board/srv/SetValve.srv new file mode 100644 index 000000000..837be5cd7 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/srv/SetValve.srv @@ -0,0 +1,6 @@ +# Service call to open or close a pneumatic valve +uint8 actuator # ID of valve +bool opened # If True, valve will open; if False, valve will close +--- +bool success # False if there were any problems with the request. +string message # Description of error if success is false diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/__init__.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/__init__.py new file mode 100644 index 000000000..8d260d68e --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/__init__.py @@ -0,0 +1,7 @@ +from .handle import ActuatorBoard +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) +from .simulation import ActuatorBoardSimulation diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py new file mode 100644 index 000000000..39f5665d2 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import rospy +from mil_usb_to_can import AckPacket, CANDeviceHandle +from sub_actuator_board.srv import ( + GetValve, + GetValveRequest, + GetValveResponse, + SetValve, + SetValveRequest, +) + +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) + + +class ActuatorBoard(CANDeviceHandle): + """ + Device handle for the actuator board. Because this class implements a CAN device, + it inherits from the :class:`CANDeviceHandle` class. + """ + + _recent_response: ActuatorPollResponsePacket | None + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._set_service = rospy.Service("/set_valve", SetValve, self.set_valve) + self._get_service = rospy.Service("/get_valve", GetValve, self.get_valve) + self._recent_response = None + + def set_valve(self, req: SetValveRequest) -> dict: + """ + Called when the ``/set_valve`` service is requested. Creates a message to + control the valve and sends it through the inherited device handle. + + Args: + req (SetValveRequest): Request to set the valve. + + Returns: + dict: List of information which is casted into a SetValveResponse. + """ + # Send board command to open or close specified valve + message = ActuatorSetPacket(address=req.actuator, open=req.opened) + self.send_data(message) + rospy.loginfo( + "Set valve {} {}".format(req.actuator, "opened" if req.opened else "closed") + ) + # Wait some time for board to process command + rospy.sleep(0.01) + # Request the status of the valve just commanded to ensure it worked + self.send_data(ActuatorPollRequestPacket()) + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(2): + if self._recent_response is not None: + break + + success = False + if self._recent_response is not None: + if not req.opened: + success = not (self._recent_response.values & (1 << req.actuator)) + else: + success = self._recent_response.values & (1 << req.actuator) + response = {"success": success} + self._recent_response = None + return response + + def get_valve(self, req: GetValveRequest) -> GetValveResponse: + message = ActuatorPollRequestPacket() + self.send_data(message) + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(10): + if self._recent_response is not None: + break + + if not self._recent_response: + raise RuntimeError("No response from the board within 10 seconds.") + + response = GetValveResponse( + opened=self._recent_response.values & (1 << req.actuator) + ) + self._recent_response = None + return response + + def on_data(self, packet: ActuatorPollResponsePacket | AckPacket) -> None: + """ + Process data received from board. + """ + if isinstance(packet, ActuatorPollResponsePacket): + self._recent_response = packet diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py new file mode 100644 index 000000000..ccc8ea4d7 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py @@ -0,0 +1,44 @@ +from dataclasses import dataclass + +from mil_usb_to_can import Packet + + +@dataclass +class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="BB"): + """ + Packet used by the actuator board to set a specific valve. + + Attributes: + address (int): The actuator ID to set. + open (bool): Whether to open the specified actuator. ``True`` requests opening, + ``False`` requests closing. + """ + + address: int + open: bool + + +@dataclass +class ActuatorPollRequestPacket( + Packet, msg_id=0x03, subclass_id=0x01, payload_format="" +): + """ + Packet used by the actuator board to request the status of all valves. + """ + + pass + + +@dataclass +class ActuatorPollResponsePacket( + Packet, msg_id=0x03, subclass_id=0x02, payload_format="B" +): + """ + Packet used by the actuator board to return the status of all valves. + + Attributes: + values (int): The statues of all actuators. Bits 0-3 represent the opened + status of actuators 0-3. + """ + + values: int diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py new file mode 100644 index 000000000..cdc1c5861 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py @@ -0,0 +1,55 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import random + +import rospy +from mil_usb_to_can import AckPacket, SimulatedCANDeviceHandle + +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) + + +class ActuatorBoardSimulation(SimulatedCANDeviceHandle): + """ + Simulator for the communication of the actuator board. + + Attributes: + status (Dict[int, bool]): The status of the valves. The keys are each of the valve IDs, + and the values are the statues of whether the valves are open. + """ + + def __init__(self, *args, **kwargs): + # Tracks the status of the 12 valves + self.status = {i: False for i in range(4)} + super().__init__(*args, **kwargs) + + def on_data(self, packet: ActuatorSetPacket | ActuatorPollRequestPacket) -> None: + """ + Processes data received from motherboard / other devices. For each message received, + the class' status attribute is updated if the message is asking to write, otherwise + a feedback message is constructed and sent back. + """ + # If message is writing a valve, store this change in the internal dictionary + if isinstance(packet, ActuatorSetPacket): + rospy.sleep(random.randrange(0, 1)) # Time to simluate opening of valves + self.status[packet.address] = packet.open + self.send_data(bytes(AckPacket())) + + # If message is a status request, send motherboard the status of the requested valve + elif isinstance(packet, ActuatorPollRequestPacket): + self.send_data( + bytes( + ActuatorPollResponsePacket( + int( + "".join( + str(int(x)) for x in reversed(self.status.values()) + ), + base=2, + ) + ) + ) + ) diff --git a/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test b/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test new file mode 100644 index 000000000..4be33a772 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + sub_actuator_board.ActuatorBoard: [sub_actuator_board.ActuatorPollResponsePacket] + simulated_devices: + sub_actuator_board.ActuatorBoardSimulation: [sub_actuator_board.ActuatorPollRequestPacket, sub_actuator_board.ActuatorSetPacket] + + + + + diff --git a/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py b/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py new file mode 100755 index 000000000..8c4665f09 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from sub_actuator_board.srv import GetValve, GetValveRequest, SetValve, SetValveRequest + + +class SimulatedBoardTest(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.set_srv = rospy.ServiceProxy("/set_valve", SetValve) + self.get_srv = rospy.ServiceProxy("/get_valve", GetValve) + super().__init__(*args) + + def test_correct_response(self): + """ + Test we can get correct data through CAN bus at least ten times. + """ + self.set_srv.wait_for_service(1) + self.get_srv.wait_for_service(1) + self.assertTrue(self.set_srv(SetValveRequest(0, True)).success) + self.assertTrue(self.get_srv(GetValveRequest(0)).opened) + self.assertTrue(self.set_srv(SetValveRequest(0, False)).success) + self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + self.assertTrue(self.set_srv(SetValveRequest(0, False)).success) + self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + self.assertTrue(self.set_srv(SetValveRequest(3, True)).success) + self.assertTrue(self.get_srv(GetValveRequest(3)).opened) + self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + self.assertTrue(self.set_srv(SetValveRequest(4, False)).success) + self.assertFalse(self.get_srv(GetValveRequest(4)).opened) + self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + + +if __name__ == "__main__": + rospy.init_node("simulated_board_test", anonymous=True) + import rostest + + rostest.rosrun("sub_actuator_board", "simulated_board_test", SimulatedBoardTest) + unittest.main() diff --git a/docs/reference/can.rst b/docs/reference/can.rst index ac6e174f4..e3072041a 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -74,11 +74,11 @@ Below is a listing of all available packets. + (Battery +--------------+-------------------------------------------------------------------------+ | Monitor) | 0x01 | :class:`sub8_battery_monitor_board.BatteryPollResponsePacket` | +------------+--------------+-------------------------------------------------------------------------+ -| 0x03 | 0x00 | :class:`sub_actuator_board.ActuatorSetPacket` | +| 0x03 | 0x00 | :class:`sub_actuator_board.ActuatorSetPacket` | + (Actuator +--------------+-------------------------------------------------------------------------+ -| Board) | 0x01 | :class:`sub_actuator_board.ActuatorPollRequestPacket` | +| Board) | 0x01 | :class:`sub_actuator_board.ActuatorPollRequestPacket` | + +--------------+-------------------------------------------------------------------------+ -| | 0x02 | :class:`sub_actuator_board.ActuatorPollResponsePacket` | +| | 0x02 | :class:`sub_actuator_board.ActuatorPollResponsePacket` | +------------+--------------+-------------------------------------------------------------------------+ | 0x04 | 0x00 | :class:`sub8_system_status_board.SetLedRequestPacket` | | (System | | | diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index 3fa945ac0..e92157340 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -145,6 +145,34 @@ Thruster .. autoclass:: sub8_thrust_and_kill_board.Thruster :members: +HeartbeatPacket +^^^^^^^^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.HeartbeatPacket + +.. autoclass:: sub8_thrust_and_kill_board.HeartbeatPacket + :members: + +ThrustSetPacket +^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.ThrustSetPacket + +.. autoclass:: sub8_thrust_and_kill_board.ThrustSetPacket + :members: + +KillSetPacket +^^^^^^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.KillSetPacket + +.. autoclass:: sub8_thrust_and_kill_board.KillSetPacket + :members: + +KillReceivePacket +^^^^^^^^^^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.KillReceivePacket + +.. autoclass:: sub8_thrust_and_kill_board.KillReceivePacket + :members: + Object Detection ---------------- From b17904c8703d1510ead02620cfe5ab53ba583e98 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Wed, 25 Jan 2023 16:32:19 -0500 Subject: [PATCH 10/38] Add documentation on payload format in each packet --- docs/reference/can.rst | 65 ++++++++++++++++++++++-------------------- 1 file changed, 34 insertions(+), 31 deletions(-) diff --git a/docs/reference/can.rst b/docs/reference/can.rst index e3072041a..ce7a59de3 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -53,37 +53,40 @@ including the start/end flags, and then using modulo 16 on this result. Packet Listing ^^^^^^^^^^^^^^ -Below is a listing of all available packets. - -+------------+--------------+-------------------------------------------------------------------------+ -| Message ID | Subclass ID | Class | -+============+==============+=========================================================================+ -| 0x00 | 0x00 | :class:`mil_usb_to_can.NackPacket` | -+ (Meta) +--------------+-------------------------------------------------------------------------+ -| | 0x01 | :class:`mil_usb_to_can.AckPacket` | -+------------+--------------+-------------------------------------------------------------------------+ -| 0x01 | 0x00 | :class:`sub8_thrust_and_kill_board.HeartbeatPacket` | -+ (Thrust/ +--------------+-------------------------------------------------------------------------+ -| Kill) | 0x01 | :class:`sub8_thrust_and_kill_board.ThrustSetPacket` | -+ +--------------+-------------------------------------------------------------------------+ -| | 0x02 | :class:`sub8_thrust_and_kill_board.KillSetPacket` | -+ +--------------+-------------------------------------------------------------------------+ -| | 0x03 | :class:`sub8_thrust_and_kill_board.KillReceivePacket` | -+------------+--------------+-------------------------------------------------------------------------+ -| 0x02 | 0x00 | :class:`sub8_battery_monitor_board.BatteryPollRequestPacket` | -+ (Battery +--------------+-------------------------------------------------------------------------+ -| Monitor) | 0x01 | :class:`sub8_battery_monitor_board.BatteryPollResponsePacket` | -+------------+--------------+-------------------------------------------------------------------------+ -| 0x03 | 0x00 | :class:`sub_actuator_board.ActuatorSetPacket` | -+ (Actuator +--------------+-------------------------------------------------------------------------+ -| Board) | 0x01 | :class:`sub_actuator_board.ActuatorPollRequestPacket` | -+ +--------------+-------------------------------------------------------------------------+ -| | 0x02 | :class:`sub_actuator_board.ActuatorPollResponsePacket` | -+------------+--------------+-------------------------------------------------------------------------+ -| 0x04 | 0x00 | :class:`sub8_system_status_board.SetLedRequestPacket` | -| (System | | | -| Status) | | | -+------------+--------------+-------------------------------------------------------------------------+ +Below is a listing of all available packets. The payload format is the format +used by the :mod:`struct` module. For more information, see the Python documentation +on the :ref:`list of format characters`, and their corresponding +byte length. + ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| Message ID | Subclass ID | Payload Format | Class | ++============+==============+================+=========================================================================+ +| 0x00 | 0x00 | Empty | :class:`mil_usb_to_can.NackPacket` | ++ (Meta) +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x01 | Empty | :class:`mil_usb_to_can.AckPacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x01 | 0x00 | Empty | :class:`sub8_thrust_and_kill_board.HeartbeatPacket` | ++ (Thrust/ +--------------+----------------+-------------------------------------------------------------------------+ +| Kill) | 0x01 | ``Bf`` | :class:`sub8_thrust_and_kill_board.ThrustSetPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x02 | ``B`` | :class:`sub8_thrust_and_kill_board.KillSetPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x03 | ``B`` | :class:`sub8_thrust_and_kill_board.KillReceivePacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x02 | 0x00 | Empty | :class:`sub8_battery_monitor_board.BatteryPollRequestPacket` | ++ (Battery +--------------+----------------+-------------------------------------------------------------------------+ +| Monitor) | 0x01 | ``ffff`` | :class:`sub8_battery_monitor_board.BatteryPollResponsePacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x03 | 0x00 | ``BB`` | :class:`sub_actuator_board.ActuatorSetPacket` | ++ (Actuator +--------------+----------------+-------------------------------------------------------------------------+ +| Board) | 0x01 | Empty | :class:`sub_actuator_board.ActuatorPollRequestPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x02 | ``B`` | :class:`sub_actuator_board.ActuatorPollResponsePacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x04 | 0x00 | Empty | :class:`sub8_system_status_board.SetLedRequestPacket` | +| (System | | | | +| Status) | | | | ++------------+--------------+----------------+-------------------------------------------------------------------------+ CANDeviceHandle ^^^^^^^^^^^^^^^ From 968225f2390f8e47b4538ca88f25e33c0eb3942b Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Fri, 27 Jan 2023 18:43:08 -0500 Subject: [PATCH 11/38] Add support for enums in mil_usb_to_can.Packet --- docs/reference/can.rst | 20 +++++++++----- .../mil_usb_to_can/mil_usb_to_can/packet.py | 27 ++++++++++++++++++- 2 files changed, 40 insertions(+), 7 deletions(-) diff --git a/docs/reference/can.rst b/docs/reference/can.rst index ce7a59de3..a03ebb682 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -66,24 +66,32 @@ byte length. | | 0x01 | Empty | :class:`mil_usb_to_can.AckPacket` | +------------+--------------+----------------+-------------------------------------------------------------------------+ | 0x01 | 0x00 | Empty | :class:`sub8_thrust_and_kill_board.HeartbeatPacket` | -+ (Thrust/ +--------------+----------------+-------------------------------------------------------------------------+ -| Kill) | 0x01 | ``Bf`` | :class:`sub8_thrust_and_kill_board.ThrustSetPacket` | -+ +--------------+----------------+-------------------------------------------------------------------------+ ++ (Sub8 +--------------+----------------+-------------------------------------------------------------------------+ +| Thrust/ | 0x01 | ``Bf`` | :class:`sub8_thrust_and_kill_board.ThrustSetPacket` | ++ Kill) +--------------+----------------+-------------------------------------------------------------------------+ | | 0x02 | ``B`` | :class:`sub8_thrust_and_kill_board.KillSetPacket` | + +--------------+----------------+-------------------------------------------------------------------------+ | | 0x03 | ``B`` | :class:`sub8_thrust_and_kill_board.KillReceivePacket` | +------------+--------------+----------------+-------------------------------------------------------------------------+ -| 0x02 | 0x00 | Empty | :class:`sub8_battery_monitor_board.BatteryPollRequestPacket` | +| 0x02 | 0x00 | Empty | :class:`sub9_thrust_and_kill_board.HeartbeatPacket` | ++ (Sub9 +--------------+----------------+-------------------------------------------------------------------------+ +| Thrust/ | 0x01 | ``Bf`` | :class:`sub9_thrust_and_kill_board.ThrustSetPacket` | ++ Kill) +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x02 | ``B`` | :class:`sub9_thrust_and_kill_board.KillSetPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x03 | ``B`` | :class:`sub9_thrust_and_kill_board.KillReceivePacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x03 | 0x00 | Empty | :class:`sub8_battery_monitor_board.BatteryPollRequestPacket` | + (Battery +--------------+----------------+-------------------------------------------------------------------------+ | Monitor) | 0x01 | ``ffff`` | :class:`sub8_battery_monitor_board.BatteryPollResponsePacket` | +------------+--------------+----------------+-------------------------------------------------------------------------+ -| 0x03 | 0x00 | ``BB`` | :class:`sub_actuator_board.ActuatorSetPacket` | +| 0x04 | 0x00 | ``BB`` | :class:`sub_actuator_board.ActuatorSetPacket` | + (Actuator +--------------+----------------+-------------------------------------------------------------------------+ | Board) | 0x01 | Empty | :class:`sub_actuator_board.ActuatorPollRequestPacket` | + +--------------+----------------+-------------------------------------------------------------------------+ | | 0x02 | ``B`` | :class:`sub_actuator_board.ActuatorPollResponsePacket` | +------------+--------------+----------------+-------------------------------------------------------------------------+ -| 0x04 | 0x00 | Empty | :class:`sub8_system_status_board.SetLedRequestPacket` | +| 0x05 | 0x00 | Empty | :class:`sub9_system_status_board.SetLedRequestPacket` | | (System | | | | | Status) | | | | +------------+--------------+----------------+-------------------------------------------------------------------------+ diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py index d99438baf..fa1da70d2 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py @@ -2,7 +2,9 @@ import struct from dataclasses import dataclass -from typing import ClassVar +from enum import Enum +from functools import lru_cache +from typing import ClassVar, get_type_hints SYNC_CHAR_1 = 0x37 SYNC_CHAR_2 = 0x01 @@ -10,6 +12,11 @@ _packet_registry: dict[int, dict[int, type[Packet]]] = {} +@lru_cache(maxsize=None) +def get_cache_hints(cls): + return get_type_hints(cls) + + @dataclass class Packet: """ @@ -22,6 +29,10 @@ class Packet: subclass IDs, and payload formats. Note that all subclasses must be decorated with :meth:`dataclasses.dataclass`. + If any class members are annotated with a subclass of :class:`enum.Enum`, + the class will always make an attempt to convert the raw data value to an + instance of the enum before constructing the rest of the values in the class. + .. code-block:: python from dataclasses import dataclass @@ -65,6 +76,20 @@ def __init_subclass__(cls, msg_id: int, subclass_id: int, payload_format: str = ) _packet_registry.setdefault(msg_id, {})[subclass_id] = cls + def __post_init__(self): + for (name, field_type) in get_cache_hints(self.__class__).items(): + if ( + name + not in [ + "msg_id", + "subclass_id", + "payload_format", + ] + and not isinstance(self.__dict__[name], field_type) + and issubclass(field_type, Enum) + ): + setattr(self, name, field_type(self.__dict__[name])) + def _calculate_checksum(self): return (0, 0) From 13983dfaa3a95c1d5658d35b30f27293408c0223 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sat, 28 Jan 2023 10:48:13 -0500 Subject: [PATCH 12/38] Add datetime/rospy.Time helpers in mil_common --- .../utils/mil_tools/mil_misc_tools/__init__.py | 1 + mil_common/utils/mil_tools/mil_misc_tools/times.py | 11 +++++++++++ mil_common/utils/mil_tools/test/test_ros_tools.py | 12 ++++++++++++ 3 files changed, 24 insertions(+) create mode 100644 mil_common/utils/mil_tools/mil_misc_tools/times.py diff --git a/mil_common/utils/mil_tools/mil_misc_tools/__init__.py b/mil_common/utils/mil_tools/mil_misc_tools/__init__.py index f2c5ac23f..6af3874c4 100644 --- a/mil_common/utils/mil_tools/mil_misc_tools/__init__.py +++ b/mil_common/utils/mil_tools/mil_misc_tools/__init__.py @@ -4,3 +4,4 @@ from .system_tools import * from .terminal_input import * from .text_effects import * +from .times import datetime_to_rospy, rospy_to_datetime diff --git a/mil_common/utils/mil_tools/mil_misc_tools/times.py b/mil_common/utils/mil_tools/mil_misc_tools/times.py new file mode 100644 index 000000000..554e8fbd8 --- /dev/null +++ b/mil_common/utils/mil_tools/mil_misc_tools/times.py @@ -0,0 +1,11 @@ +import datetime + +import rospy + + +def rospy_to_datetime(time: rospy.Time) -> datetime.datetime: + return datetime.datetime.utcfromtimestamp(time.to_sec()) + + +def datetime_to_rospy(dt: datetime.datetime) -> rospy.Time: + return rospy.Time.from_sec(dt.replace(tzinfo=datetime.timezone.utc).timestamp()) diff --git a/mil_common/utils/mil_tools/test/test_ros_tools.py b/mil_common/utils/mil_tools/test/test_ros_tools.py index f83e2ce10..60b2c7a20 100644 --- a/mil_common/utils/mil_tools/test/test_ros_tools.py +++ b/mil_common/utils/mil_tools/test/test_ros_tools.py @@ -1,8 +1,12 @@ #!/usr/bin/env python3 +import random +import time import unittest import numpy as np +import rospy from geometry_msgs.msg import Pose2D, Quaternion, Vector3 +from mil_misc_tools import datetime_to_rospy, rospy_to_datetime from mil_ros_tools import ( get_image_msg, make_image_msg, @@ -150,6 +154,14 @@ def test_normalize_vector(self): norm, 1.0, err_msg="The normalized vector did not have length 1" ) + def test_datetime_conv(self): + """Test datetime to rospy.Time conversion.""" + for _ in range(10): + unix_stamp = random.randrange(0, int(time.time())) + rp1 = rospy.Time(unix_stamp) + dt1 = rospy_to_datetime(rp1) + self.assertEqual(datetime_to_rospy(dt1), rp1) + if __name__ == "__main__": suite = unittest.TestLoader().loadTestsFromTestCase(TestROSTools) From 2af330ff5cc6b433e8f5799a3a540976f1fd9924 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sat, 28 Jan 2023 12:51:04 -0500 Subject: [PATCH 13/38] First implementation of Sub9 thrust/kill driver --- .../sub9_thrust_and_kill_board/CMakeLists.txt | 8 + .../sub9_thrust_and_kill_board/package.xml | 13 ++ .../sub9_thrust_and_kill_board/setup.py | 11 + .../sub9_thrust_and_kill_board/__init__.py | 11 + .../sub9_thrust_and_kill_board/handle.py | 190 ++++++++++++++++++ .../sub9_thrust_and_kill_board/packets.py | 98 +++++++++ .../sub9_thrust_and_kill_board/simulation.py | 91 +++++++++ .../sub9_thrust_and_kill_board/thruster.py | 66 ++++++ .../test/simulated_board.test | 18 ++ .../test/simulated_board_test.py | 39 ++++ docs/subjugator/reference.rst | 42 +++- .../mil_usb_to_can/mil_usb_to_can/driver.py | 15 +- 12 files changed, 596 insertions(+), 6 deletions(-) create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/CMakeLists.txt create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/package.xml create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/setup.py create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/thruster.py create mode 100644 SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test create mode 100755 SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/CMakeLists.txt b/SubjuGator/drivers/sub9_thrust_and_kill_board/CMakeLists.txt new file mode 100644 index 000000000..19b63afff --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sub9_thrust_and_kill_board) +find_package(catkin REQUIRED COMPONENTS + mil_usb_to_can +) +add_rostest(test/simulated_board.test) +catkin_python_setup() +catkin_package() diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/package.xml b/SubjuGator/drivers/sub9_thrust_and_kill_board/package.xml new file mode 100644 index 000000000..530dcfdf9 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/package.xml @@ -0,0 +1,13 @@ + + + sub9_thrust_and_kill_board + 0.0.0 + The sub9_thrust_and_kill_board package + Cameron Brown + MIT + Cameron Brown + catkin + mil_usb_to_can + mil_usb_to_can + mil_usb_to_can + diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/setup.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/setup.py new file mode 100644 index 000000000..32234daad --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/setup.py @@ -0,0 +1,11 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup + +# Fetch values from package.xml +setup_args = generate_distutils_setup( + packages=["sub9_thrust_and_kill_board"], +) + +setup(**setup_args) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py new file mode 100644 index 000000000..7a9f7f666 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py @@ -0,0 +1,11 @@ +from .handle import ThrusterAndKillBoard +from .packets import ( + HeartbeatReceivePacket, + HeartbeatSetPacket, + KillReceivePacket, + KillSetPacket, + KillStatus, + ThrustSetPacket, +) +from .simulation import ThrusterAndKillBoardSimulation +from .thruster import Thruster diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py new file mode 100644 index 000000000..e3abe1c16 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py @@ -0,0 +1,190 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import rospy +from mil_misc_tools import rospy_to_datetime +from mil_usb_to_can import AckPacket, CANDeviceHandle, NackPacket +from ros_alarms import AlarmBroadcaster, AlarmListener +from ros_alarms.msg import Alarm +from rospy.timer import TimerEvent +from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse +from subjugator_msgs.msg import Thrust + +from .packets import ( + HeartbeatReceivePacket, + HeartbeatSetPacket, + KillReceivePacket, + KillSetPacket, + KillStatus, + ThrustSetPacket, +) +from .thruster import make_thruster_dictionary + + +class ThrusterAndKillBoard(CANDeviceHandle): + """ + Device handle for the thrust and kill board. + """ + + ID_MAPPING = { + "FLH": 0, + "FRH": 1, + "FLV": 2, + "FRV": 3, + "BLH": 4, + "BRH": 5, + "BLV": 6, + "BRV": 7, + } + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + # Initialize thruster mapping from params + self.thrusters = make_thruster_dictionary( + rospy.get_param("/thruster_layout/thrusters") + ) + # Tracks last hw-kill alarm update + self._last_hw_kill = None + # Used to raise/clear hw-kill when board updates + self._kill_broadcaster = AlarmBroadcaster("hw-kill") + # Listens to hw-kill updates to ensure another nodes doesn't manipulate it + self._hw_kill_listener = AlarmListener( + "hw-kill", callback_funct=self.on_hw_kill + ) + # Provide service for alarm handler to set/clear the motherboard kill + self._unkill_service = rospy.Service("/set_mobo_kill", SetBool, self.set_kill) + # Sends heartbeat to board + self._heartbeat_timer = rospy.Timer(rospy.Duration(0.4), self.send_heartbeat) + # Create a subscribe for thruster commands + self._sub = rospy.Subscriber( + "/thrusters/thrust", Thrust, self.on_command, queue_size=10 + ) + self._last_heartbeat = rospy.Time.now() + self._last_packet = None + self._updated_kill = False + + def set_kill(self, req: SetBoolRequest) -> SetBoolResponse: + """ + Called on service calls to ``/set_mobo_kill``, sending the appropriate + packet to the board to unassert or assert to motherboard-origin kill. + + Args: + req (SetBoolRequest): The service request. + + Returns: + SetBoolResponse: The service response. + """ + self.send_data(KillSetPacket(req.data, KillStatus.SOFTWARE_REQUESTED)) + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(1): + if self._last_packet is not None: + break + + if isinstance(self._last_packet, NackPacket): + raise RuntimeError("Request not acknowledged.") + + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(1): + if self._updated_kill: + break + + if self._updated_kill: + self._updated_kill = False + return SetBoolResponse(success=True) + else: + return SetBoolResponse( + success=False, message="No response from board after 1 second." + ) + + def send_heartbeat(self, _: TimerEvent) -> None: + """ + Send a special heartbeat packet. Called by a recurring timer set upon + initialization. + """ + self.send_data(HeartbeatSetPacket()) + + def on_hw_kill(self, alarm: Alarm) -> None: + """ + Update the classes' hw-kill alarm to the latest update. + + Args: + alarm (:class:`~ros_alarms.msg._Alarm.Alarm`): The alarm message to update with. + """ + self._last_hw_kill = alarm + + def on_command(self, msg: Thrust) -> None: + """ + When a thrust command message is received from the Subscriber, send the appropriate packet + to the board. + + Args: + msg (Thrust): The thrust message. + """ + for cmd in msg.thruster_commands: + # If we don't have a mapping for this thruster, ignore it + if cmd.name not in self.thrusters: + rospy.logwarn( + f"Command received for {cmd.name}, but this is not a thruster." + ) + continue + # Map commanded thrust (in newetons) to effort value (-1 to 1) + effort = self.thrusters[cmd.name].effort_from_thrust(cmd.thrust) + # Send packet to command specified thruster the specified force + packet = ThrustSetPacket(self.ID_MAPPING[cmd.name], effort) + self.send_data(packet) + + def update_software_kill(self, raised: bool, message: str): + # If the current status differs from the alarm, update the alarm + severity = 2 if raised else 0 + self._updated_kill = True + self._hw_kill_listener.wait_for_server() + if ( + self._last_hw_kill is None + or self._last_hw_kill.raised != raised + or self._last_hw_kill.problem_description != message + or self._last_hw_kill.severity != severity + ): + if raised: + self._kill_broadcaster.raise_alarm( + severity=severity, problem_description=message + ) + else: + self._kill_broadcaster.clear_alarm(severity=severity) + + def on_data( + self, data: AckPacket | NackPacket | HeartbeatReceivePacket | KillReceivePacket + ) -> None: + """ + Parse the two bytes and raise kills according to a set of specifications + listed below. + """ + if isinstance(data, AckPacket): + self._last_packet = data + elif isinstance(data, NackPacket): + self._last_packet = data + elif isinstance(data, HeartbeatReceivePacket): + self._last_heartbeat = rospy.Time.now() + elif isinstance(data, KillReceivePacket): + if data.set is False: + self.update_software_kill(False, "") + elif data.status is KillStatus.MOBO_HEARTBEAT_LOST: + self.update_software_kill( + True, "Thrust/kill board lost heartbeat from motherboard." + ) + elif data.status is KillStatus.BATTERY_LOW: + self.update_software_kill(True, "Battery too low.") + elif data.status is KillStatus.KILL_SWITCH: + self.update_software_kill(True, "Kill switch was pulled!") + elif data.status is KillStatus.BOARD_HEARTBEAT_LOST: + dt = rospy_to_datetime(self._last_heartbeat) + self.update_software_kill( + True, + f"Motherboard is no longer hearing heartbeat from thrust/kill board. Last heard from board at {dt}.", + ) + elif data.status is KillStatus.SOFTWARE_REQUESTED: + self.update_software_kill( + True, + f"Software requested kill.", + ) + else: + raise ValueError(f"Not expecting packet of type {data.__class__.__name__}!") diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py new file mode 100644 index 000000000..d03850194 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py @@ -0,0 +1,98 @@ +from dataclasses import dataclass +from enum import IntEnum + +from mil_usb_to_can import Packet + + +@dataclass +class HeartbeatSetPacket(Packet, msg_id=0x02, subclass_id=0x00, payload_format=""): + """ + Packet indicating a heartbeat. Sent both ways. + """ + + +@dataclass +class HeartbeatReceivePacket(Packet, msg_id=0x02, subclass_id=0x01, payload_format=""): + """ + Packet indicating a heartbeat. Sent both ways. + """ + + +@dataclass +class ThrustSetPacket(Packet, msg_id=0x02, subclass_id=0x02, payload_format="Bf"): + """ + Packet to set the speed of a specific thruster. + + Attributes: + thruster_id (int): The ID of the thruster to set the speed of. The ID of + the thruster corresponds to a specific thruster: + + +--------+------+ + | name | id | + +========+======+ + | FLH | 0 | + +--------+------+ + | FRH | 1 | + +--------+------+ + | FLV | 2 | + +--------+------+ + | FRV | 3 | + +--------+------+ + | BLH | 4 | + +--------+------+ + | BRH | 5 | + +--------+------+ + | BLV | 6 | + +--------+------+ + | BRV | 7 | + +--------+------+ + speed (float): The speed to set the thruster to. + """ + + thruster_id: int + speed: float + + +class KillStatus(IntEnum): + """ + Enum to represent the purpose behind a kill. + """ + + #: A software user manually requested a kill on the sub. + SOFTWARE_REQUESTED = 0 + #: The board reported that it stopped hearing the heartbeat from the motherboard. + MOBO_HEARTBEAT_LOST = 1 + #: The motherboard reported that it stopped hearing the heartbeat from the board. + BOARD_HEARTBEAT_LOST = 2 + #: The physical kill switch was pulled, requesting an immediate kill. + KILL_SWITCH = 3 + #: The battery is too low to continue operation. + BATTERY_LOW = 4 + + +@dataclass +class KillSetPacket(Packet, msg_id=0x02, subclass_id=0x03, payload_format="BB"): + """ + Packet sent by the motherboard to set/unset the kill. + + Attributes: + set (bool): Whether the kill is set or unset. + status (KillStatus): The reason for the kill. + """ + + set: bool + status: KillStatus + + +@dataclass +class KillReceivePacket(Packet, msg_id=0x02, subclass_id=0x04, payload_format="BB"): + """ + Packet sent by the motherboard to set/unset the kill. + + Attributes: + set (bool): Whether the kill is set or unset. + status (KillStatus): The reason for the kill. + """ + + set: bool + status: KillStatus diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py new file mode 100644 index 000000000..0dd27df80 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py @@ -0,0 +1,91 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import rospy +from mil_usb_to_can import AckPacket, NackPacket, SimulatedCANDeviceHandle +from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse + +from .packets import ( + HeartbeatReceivePacket, + HeartbeatSetPacket, + KillReceivePacket, + KillSetPacket, + KillStatus, + ThrustSetPacket, +) + + +class ThrusterAndKillBoardSimulation(SimulatedCANDeviceHandle): + """ + Serial simulator for the thruster and kill board, + providing services to simulate physical plug connections/disconnections. + + Inherits from :class:`~mil_usb_to_can.SimulatedCANDevice`. + + Attributes: + kill (bool): Whether the hard kill was set. + """ + + HEARTBEAT_TIMEOUT_SECONDS = rospy.Duration(1.0) + + def __init__(self, *args, **kwargs): + self.kill = False + self._last_heartbeat = None + super().__init__(*args, **kwargs) + self._update_timer = rospy.Timer(rospy.Duration(1), self._check_for_timeout) + self._kill_srv = rospy.Service("/simulate_kill", SetBool, self.set_kill) + + def _check_for_timeout(self, _: rospy.timer.TimerEvent): + if self.heartbeat_timedout and not self.kill: + self.send_data(bytes(KillSetPacket(True, KillStatus.BOARD_HEARTBEAT_LOST))) + self.kill = True + + def set_kill(self, req: SetBoolRequest) -> SetBoolResponse: + """ + Called by the `/simulate_kill` service to set the kill state of the + simluated device. + + Args: + req (SetBoolRequest): The request to set the service with. + + Returns: + SetBoolResponse: The response to the service that the operation was successful. + """ + self.kill = req.data + return SetBoolResponse(success=True) + + @property + def heartbeat_timedout(self) -> bool: + """ + Whether the heartbeat timed out. + + Returns: + bool: The status of the heartbeat timing out. + """ + return ( + self._last_heartbeat is None + or (rospy.Time.now() - self._last_heartbeat) + > self.HEARTBEAT_TIMEOUT_SECONDS + ) + + def on_data( + self, packet: HeartbeatSetPacket | ThrustSetPacket | KillSetPacket + ) -> None: + """ + Serves as the data handler for the device. Handles :class:`KillMessage`, + :class:`ThrustPacket`, and :class:`HeartbeatMessage` types. + """ + if isinstance(packet, HeartbeatSetPacket): + self._last_heartbeat = rospy.Time.now() + self.send_data(bytes(HeartbeatReceivePacket())) + + elif isinstance(packet, ThrustSetPacket): + self.send_data(bytes(AckPacket())) + + elif isinstance(packet, KillSetPacket): + self.kill = packet.set + self.send_data(bytes(AckPacket())) + self.send_data(bytes(KillReceivePacket(packet.set, packet.status))) + + else: + self.send_data(bytes(NackPacket())) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/thruster.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/thruster.py new file mode 100644 index 000000000..5a90baac0 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/thruster.py @@ -0,0 +1,66 @@ +from typing import Any, Dict + +from numpy import clip, polyval + + +def make_thruster_dictionary(dictionary): + """ + Make a dictionary mapping thruster names to :class:`Thruster` objects. + """ + ret = {} + for thruster, content in dictionary.items(): + ret[thruster] = Thruster.from_dict(content) + return ret + + +class Thruster: + """ + Models the force (thrust) to PWM (effort) of a thruster. + + Attributes: + forward_calibration (???): ??? + backward_calibration (???): ??? + """ + + def __init__(self, forward_calibration, backward_calibration): + self.forward_calibration = forward_calibration + self.backward_calibration = backward_calibration + + @classmethod + def from_dict(cls, data: Dict[str, Dict[str, Any]]): + """ + Constructs the class from a dictionary. The dictionary should be formatted + as so: + + .. code-block:: python3 + + { + "calib": { + "forward": ..., + "backward": ..., + } + } + + Args: + data (Dict[str, Dict[str, Any]]): The dictionary containing the initialization info. + """ + forward_calibration = data["calib"]["forward"] + backward_calibration = data["calib"]["backward"] + return cls(forward_calibration, backward_calibration) + + def effort_from_thrust_unclipped(self, thrust: Any): + if thrust < 0: + return polyval(self.backward_calibration, thrust) + else: + return polyval(self.forward_calibration, thrust) + + def effort_from_thrust(self, thrust: Any): + """ + Attempts to find the effort from a particular value of thrust. + + Args: + thrust (???): The amount of thrust. + """ + unclipped = self.effort_from_thrust_unclipped(thrust) + # Theoretically can limit to .66 under 16V assumptions or .5 under 12V assumptions... So do both (.5 + 66)/2 + return clip(unclipped, -0.58, 0.58) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test new file mode 100644 index 000000000..5633fa38c --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test @@ -0,0 +1,18 @@ + + + + + + + device_handles: + sub9_thrust_and_kill_board.ThrusterAndKillBoard: [sub9_thrust_and_kill_board.KillReceivePacket] + simulated_devices: + sub9_thrust_and_kill_board.ThrusterAndKillBoardSimulation: [sub9_thrust_and_kill_board.HeartbeatSetPacket, sub9_thrust_and_kill_board.ThrustSetPacket, sub9_thrust_and_kill_board.KillSetPacket] + + + + + + + + diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py new file mode 100755 index 000000000..d2b1f0195 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from ros_alarms import AlarmListener +from std_srvs.srv import SetBool, SetBoolRequest + + +class SimulatedBoardTest(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.kill_srv = rospy.ServiceProxy("/set_mobo_kill", SetBool) + self.hw_alarm_listener = AlarmListener("hw-kill") + super().__init__(*args) + + def test_correct_response(self): + """ + Test we can get correct data through CAN bus at least ten times. + """ + self.kill_srv.wait_for_service(1) + self.hw_alarm_listener.wait_for_server() + self.assertTrue(self.kill_srv(SetBoolRequest(True)).success) + self.assertTrue(self.hw_alarm_listener.is_raised(True)) + self.assertTrue(self.kill_srv(SetBoolRequest(False)).success) + self.assertTrue(self.hw_alarm_listener.is_raised(False)) + + +if __name__ == "__main__": + rospy.init_node("simulated_board_test", anonymous=True) + import rostest + + rostest.rosrun( + "sub9_thrust_and_kill_board", "simulated_board_test", SimulatedBoardTest + ) + unittest.main() diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index e92157340..c3c4c7dba 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -121,8 +121,8 @@ ActuatorPollResponsePacket .. autoclass:: sub_actuator_board.ActuatorPollResponsePacket :members: -Thrust and Kill Board ---------------------- +Sub8 Thrust and Kill Board +-------------------------- ThrusterAndKillBoard ^^^^^^^^^^^^^^^^^^^^ @@ -173,6 +173,44 @@ KillReceivePacket .. autoclass:: sub8_thrust_and_kill_board.KillReceivePacket :members: +Sub9 Thrust and Kill Board +-------------------------- + +HeartbeatPacket +^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.HeartbeatPacket + +.. autoclass:: sub9_thrust_and_kill_board.HeartbeatPacket + :members: + +ThrustSetPacket +^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.ThrustSetPacket + +.. autoclass:: sub9_thrust_and_kill_board.ThrustSetPacket + :members: + +KillStatus +^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.KillStatus + +.. autoclass:: sub9_thrust_and_kill_board.KillStatus + :members: + +KillSetPacket +^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.KillSetPacket + +.. autoclass:: sub9_thrust_and_kill_board.KillSetPacket + :members: + +KillReceivePacket +^^^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.KillReceivePacket + +.. autoclass:: sub9_thrust_and_kill_board.KillReceivePacket + :members: + Object Detection ---------------- diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py index eda88e3b5..1ade01ccb 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py @@ -88,6 +88,7 @@ class USBtoCANDriver: _packet_deque: deque[ SimulatedCANDeviceHandle | CANDeviceHandle ] # Used to keep track of who gets incoming packets + _inbound_listing: dict[type[Packet], CANDeviceHandle] def __init__(self): port = rospy.get_param("~port", "/dev/tty0") @@ -109,9 +110,13 @@ def __init__(self): self.stream.reset_output_buffer() self.stream.reset_input_buffer() - self.handles = [ - device[0](self) for device in self.read_devices(simulated=False) - ] + self.handles = [] + self._inbound_listing = {} + for device in self.read_devices(simulated=False): + handle = device[0](self) + self.handles.append(handle) + for packet in device[1]: + self._inbound_listing[packet] = handle self.timer = rospy.Timer(rospy.Duration(1.0 / 20.0), self.process_in_buffer) @@ -159,7 +164,9 @@ def read_packet(self) -> bool: return False if packet is None: return False - if not self._packet_deque: + if packet.__class__ in self._inbound_listing: + self._inbound_listing[packet.__class__].on_data(packet) + elif not self._packet_deque: rospy.logwarn( f"Message of type {packet.__class__.__qualname__} received, but no device ready to accept" ) From 22e4a38ca1db1b8b9529b865e50320bb12455f3c Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sat, 28 Jan 2023 12:58:34 -0500 Subject: [PATCH 14/38] Differentiate heartbeat packets used in sub9 communication --- .../sub9_thrust_and_kill_board/packets.py | 4 ++-- docs/reference/can.rst | 10 ++++++---- docs/subjugator/reference.rst | 15 +++++++++++---- 3 files changed, 19 insertions(+), 10 deletions(-) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py index d03850194..b50f2bc22 100644 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py @@ -7,14 +7,14 @@ @dataclass class HeartbeatSetPacket(Packet, msg_id=0x02, subclass_id=0x00, payload_format=""): """ - Packet indicating a heartbeat. Sent both ways. + Heartbeat packet sent by the motherboard to the thrust/kill board. """ @dataclass class HeartbeatReceivePacket(Packet, msg_id=0x02, subclass_id=0x01, payload_format=""): """ - Packet indicating a heartbeat. Sent both ways. + Heartbeat packet sent by the thrust/kill board to the motherboard. """ diff --git a/docs/reference/can.rst b/docs/reference/can.rst index a03ebb682..f2c657bf5 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -73,13 +73,15 @@ byte length. + +--------------+----------------+-------------------------------------------------------------------------+ | | 0x03 | ``B`` | :class:`sub8_thrust_and_kill_board.KillReceivePacket` | +------------+--------------+----------------+-------------------------------------------------------------------------+ -| 0x02 | 0x00 | Empty | :class:`sub9_thrust_and_kill_board.HeartbeatPacket` | +| 0x02 | 0x00 | Empty | :class:`sub9_thrust_and_kill_board.HeartbeatSetPacket` | + (Sub9 +--------------+----------------+-------------------------------------------------------------------------+ -| Thrust/ | 0x01 | ``Bf`` | :class:`sub9_thrust_and_kill_board.ThrustSetPacket` | +| Thrust/ | 0x01 | Empty | :class:`sub9_thrust_and_kill_board.HeartbeatReceivePacket` | + Kill) +--------------+----------------+-------------------------------------------------------------------------+ -| | 0x02 | ``B`` | :class:`sub9_thrust_and_kill_board.KillSetPacket` | +| | 0x02 | ``Bf`` | :class:`sub9_thrust_and_kill_board.ThrustSetPacket` | + +--------------+----------------+-------------------------------------------------------------------------+ -| | 0x03 | ``B`` | :class:`sub9_thrust_and_kill_board.KillReceivePacket` | +| | 0x03 | ``B`` | :class:`sub9_thrust_and_kill_board.KillSetPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x04 | ``B`` | :class:`sub9_thrust_and_kill_board.KillReceivePacket` | +------------+--------------+----------------+-------------------------------------------------------------------------+ | 0x03 | 0x00 | Empty | :class:`sub8_battery_monitor_board.BatteryPollRequestPacket` | + (Battery +--------------+----------------+-------------------------------------------------------------------------+ diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index c3c4c7dba..8648685cc 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -176,11 +176,18 @@ KillReceivePacket Sub9 Thrust and Kill Board -------------------------- -HeartbeatPacket -^^^^^^^^^^^^^^^ -.. attributetable:: sub9_thrust_and_kill_board.HeartbeatPacket +HeartbeatSetPacket +^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.HeartbeatSetPacket + +.. autoclass:: sub9_thrust_and_kill_board.HeartbeatSetPacket + :members: + +HeartbeatReceivePacket +^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.HeartbeatReceivePacket -.. autoclass:: sub9_thrust_and_kill_board.HeartbeatPacket +.. autoclass:: sub9_thrust_and_kill_board.HeartbeatReceivePacket :members: ThrustSetPacket From 04fe39088a878fc5ac1f82a880373ea4dbc4c096 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sun, 5 Feb 2023 13:24:07 -0500 Subject: [PATCH 15/38] Add checksum, fix silenced errors --- .../subjugator_launch/launch/can.launch | 2 - .../mil_usb_to_can/mil_usb_to_can/driver.py | 7 ++- .../mil_usb_to_can/mil_usb_to_can/packet.py | 51 +++++++++++++------ 3 files changed, 41 insertions(+), 19 deletions(-) diff --git a/SubjuGator/command/subjugator_launch/launch/can.launch b/SubjuGator/command/subjugator_launch/launch/can.launch index 9023b1dfc..d29f2cf6a 100644 --- a/SubjuGator/command/subjugator_launch/launch/can.launch +++ b/SubjuGator/command/subjugator_launch/launch/can.launch @@ -8,8 +8,6 @@ port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0 # Baudrate of device, should leave as is baudrate: 115200 - # The CAN device id of the usb-can board - can_id: 0 # List of python device handle classes device_handles: "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py index 1ade01ccb..680b36159 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py @@ -178,8 +178,11 @@ def process_in_buffer(self, *args) -> None: """ Read all available packets in the board's in-buffer. """ - while self.read_packet(): - pass + while True: + try: + self.read_packet() + except Exception as e: + rospy.logerr(f"Error when reading packets: {e}") def send_data( self, handle: CANDeviceHandle | SimulatedCANDeviceHandle, packet: Packet diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py index fa1da70d2..5d3e5a2ae 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py @@ -6,17 +6,30 @@ from functools import lru_cache from typing import ClassVar, get_type_hints -SYNC_CHAR_1 = 0x37 -SYNC_CHAR_2 = 0x01 +SYNC_CHAR_1 = ord("3") +SYNC_CHAR_2 = ord("7") _packet_registry: dict[int, dict[int, type[Packet]]] = {} +def hexify(data: bytes) -> str: + return ":".join(f"{c:02x}" for c in data) + + @lru_cache(maxsize=None) def get_cache_hints(cls): return get_type_hints(cls) +class ChecksumException(OSError): + def __init__( + self, packet: type[Packet], received: tuple[int, int], expected: tuple[int, int] + ): + super().__init__( + f"Invalid checksum in packet of type {packet.__qualname__}: received {received}, expected {expected}" + ) + + @dataclass class Packet: """ @@ -90,23 +103,27 @@ def __post_init__(self): ): setattr(self, name, field_type(self.__dict__[name])) - def _calculate_checksum(self): - return (0, 0) + @classmethod + def _calculate_checksum(cls, data: bytes) -> tuple[int, int]: + sum1, sum2 = 0, 0 + for byte in data: + sum1 = (sum1 + byte) % 255 + sum2 = (sum2 + sum1) % 255 + return sum1, sum2 def __bytes__(self): payload = struct.pack(self.payload_format, *self.__dict__.values()) - checksum = self._calculate_checksum() - return struct.pack( - f"BBBBH{len(payload)}sBB", - 0x37, - 0x01, + data = struct.pack( + f"BBBBH{len(payload)}s", + SYNC_CHAR_1, + SYNC_CHAR_2, self.msg_id, self.subclass_id, len(payload), payload, - checksum[0], - checksum[1], ) + checksum = self._calculate_checksum(data[2:]) + return data + struct.pack(f"BB", *checksum) @classmethod def from_bytes(cls, packed: bytes) -> Packet: @@ -118,13 +135,17 @@ def from_bytes(cls, packed: bytes) -> Packet: """ msg_id = packed[2] subclass_id = packed[3] - # for subclass in cls.__subclasses__(): - # if subclass.msg_id == msg_id and subclass.subclass_id == subclass_id: - # payload = packed[6:-2] - # unpacked = struct.unpack(subclass.payload_format, payload) if msg_id in _packet_registry and subclass_id in _packet_registry[msg_id]: subclass = _packet_registry[msg_id][subclass_id] payload = packed[6:-2] + if struct.unpack("BB", packed[-2:]) != cls._calculate_checksum( + packed[2:-2] + ): + raise ChecksumException( + subclass, + struct.unpack("BB", packed[-2:]), + cls._calculate_checksum(packed[2:-2]), + ) unpacked = struct.unpack(subclass.payload_format, payload) return subclass(*unpacked) raise LookupError( From c13f3c0a25b36a0a5e9fbb76cff6f0050d59aa51 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Fri, 10 Feb 2023 12:45:25 -0500 Subject: [PATCH 16/38] most recent for now --- .../drivers/mil_usb_to_can/launch/test.launch | 19 +++++++ .../mil_usb_to_can/mil_usb_to_can/driver.py | 10 ++-- .../mil_usb_to_can/test_mehron.py | 52 +++++++++++++++++++ 3 files changed, 77 insertions(+), 4 deletions(-) create mode 100644 mil_common/drivers/mil_usb_to_can/launch/test.launch create mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/test_mehron.py diff --git a/mil_common/drivers/mil_usb_to_can/launch/test.launch b/mil_common/drivers/mil_usb_to_can/launch/test.launch new file mode 100644 index 000000000..b9f2610dc --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/launch/test.launch @@ -0,0 +1,19 @@ + + + + + + + + + # Path of serial device + port: /dev/serial/by-id/usb-Texas_Instruments_In-Circuit_Debug_Interface_0E237EF9-if00 + # Baudrate of device, should leave as is + baudrate: 115200 + # The CAN device id of the usb-can board + device_handles: + # Device 9 will be handled with ExampleCANDeviceHandle + mil_usb_to_can.test_mehron.ExampleTestDevice: [] + + + diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py index 680b36159..f0a31c534 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py @@ -10,7 +10,7 @@ import serial from mil_misc_tools.serial_tools import SimulatedSerial from mil_usb_to_can.device import CANDeviceHandle, SimulatedCANDeviceHandle -from mil_usb_to_can.packet import SYNC_CHAR_1, Packet +from mil_usb_to_can.packet import SYNC_CHAR_1, Packet, hexify from serial import SerialException if TYPE_CHECKING: @@ -125,12 +125,13 @@ def read_from_stream(self) -> bytes | None: sof = None for _ in range(10): sof = self.stream.read(1) - if sof is None or len(sof) == 0: - return None + if not len(sof): + continue sof_int = int.from_bytes(sof, byteorder="big") if sof_int == SYNC_CHAR_1: break - assert isinstance(sof, bytes) + if not isinstance(sof, bytes): + raise TimeoutError("No SOF received in one second.") sof_int = int.from_bytes(sof, byteorder="big") if sof_int != SYNC_CHAR_1: print("Where da start char at?") @@ -158,6 +159,7 @@ def read_packet(self) -> bool: return False packed_packet = self.read_from_stream() assert isinstance(packed_packet, bytes) + rospy.logerr(f"raw: {hexify(packed_packet)}") packet = Packet.from_bytes(packed_packet) except (SerialException) as e: rospy.logerr(f"Error reading packet: {e}") diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/test_mehron.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/test_mehron.py new file mode 100644 index 000000000..b2b18735c --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/test_mehron.py @@ -0,0 +1,52 @@ +import datetime +from dataclasses import dataclass + +import rich +import rospy +from mil_usb_to_can import CANDeviceHandle, Packet +from sub8_thrust_and_kill_board import HeartbeatPacket, KillSetPacket, ThrustSetPacket +from sub_actuator_board import ActuatorPollRequestPacket, ActuatorSetPacket + +from .packet import hexify + + +@dataclass +class TestPacket(Packet, msg_id=0x02, subclass_id=0x02, payload_format="f"): + msg: float + + +class ExampleTestDevice(CANDeviceHandle): + """ + Example implementation of a SimulatedCANDevice. + On sends, stores the transmitted data in a buffer. + When data is requested, it echos this data back. + """ + + def __init__(self, handle): + # Call parent classes constructor + super().__init__(handle) + self.index = 0 + self.types = [ + HeartbeatPacket(), + ThrustSetPacket(1, 0.5), + KillSetPacket(1), + ActuatorSetPacket(1, 2), + ActuatorPollRequestPacket(), + ] + self._timer = rospy.Timer(rospy.Duration(1), self.send) + + def send(self, _): + # packet = TestPacket(random.random()) + packet = self.types[self.index] + rich.print( + f"[bold chartreuse1]{datetime.datetime.now()}: Sending: {packet}, {hexify(bytes(packet))}" + ) + self.send_data(packet) + self.index += 1 + self.index %= len(self.types) + + def on_data(self, data): + # Echo data received back onto the bus + rich.print( + f"[bold gold1]{datetime.datetime.now()} Received: {data}, {hexify(bytes(data))}" + ) From 06447c537667c5c9713d9054830066562d52fc3e Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Fri, 17 Mar 2023 17:58:09 -0400 Subject: [PATCH 17/38] Begin transition to sub9 package --- .../sub9_thrust_and_kill_board/handle.py | 2 +- .../sub9_thrust_and_kill_board/simulation.py | 2 +- .../sub_actuator_board/handle.py | 2 +- .../sub_actuator_board/packets.py | 2 +- .../sub_actuator_board/simulation.py | 2 +- docs/reference/can.rst | 44 ++++++++-------- .../mil_usb_to_can/mil_usb_to_can/__init__.py | 2 - .../mil_usb_to_can/sub9/__init__.py | 2 + .../mil_usb_to_can/{ => sub9}/device.py | 2 +- .../mil_usb_to_can/{ => sub9}/example.py | 0 .../mil_usb_to_can/{ => sub9}/packet.py | 0 .../{driver.py => sub9/sub9_driver.py} | 0 .../mil_usb_to_can/test_mehron.py | 52 ------------------- 13 files changed, 31 insertions(+), 81 deletions(-) create mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/__init__.py rename mil_common/drivers/mil_usb_to_can/mil_usb_to_can/{ => sub9}/device.py (97%) rename mil_common/drivers/mil_usb_to_can/mil_usb_to_can/{ => sub9}/example.py (100%) rename mil_common/drivers/mil_usb_to_can/mil_usb_to_can/{ => sub9}/packet.py (100%) rename mil_common/drivers/mil_usb_to_can/mil_usb_to_can/{driver.py => sub9/sub9_driver.py} (100%) delete mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/test_mehron.py diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py index e3abe1c16..ef710e264 100644 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py @@ -3,7 +3,7 @@ import rospy from mil_misc_tools import rospy_to_datetime -from mil_usb_to_can import AckPacket, CANDeviceHandle, NackPacket +from mil_usb_to_can.sub9 import AckPacket, CANDeviceHandle, NackPacket from ros_alarms import AlarmBroadcaster, AlarmListener from ros_alarms.msg import Alarm from rospy.timer import TimerEvent diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py index 0dd27df80..25140fff0 100644 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py @@ -2,7 +2,7 @@ from __future__ import annotations import rospy -from mil_usb_to_can import AckPacket, NackPacket, SimulatedCANDeviceHandle +from mil_usb_to_can.sub9 import AckPacket, NackPacket, SimulatedCANDeviceHandle from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse from .packets import ( diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py index 39f5665d2..4893aaef0 100644 --- a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py @@ -2,7 +2,7 @@ from __future__ import annotations import rospy -from mil_usb_to_can import AckPacket, CANDeviceHandle +from mil_usb_to_can.sub9 import AckPacket, CANDeviceHandle from sub_actuator_board.srv import ( GetValve, GetValveRequest, diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py index ccc8ea4d7..17a1d09ca 100644 --- a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py @@ -1,6 +1,6 @@ from dataclasses import dataclass -from mil_usb_to_can import Packet +from mil_usb_to_can.sub9 import Packet @dataclass diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py index cdc1c5861..4ffa1bbb9 100644 --- a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py @@ -4,7 +4,7 @@ import random import rospy -from mil_usb_to_can import AckPacket, SimulatedCANDeviceHandle +from mil_usb_to_can.sub9 import AckPacket, SimulatedCANDeviceHandle from .packets import ( ActuatorPollRequestPacket, diff --git a/docs/reference/can.rst b/docs/reference/can.rst index f2c657bf5..e54e365a0 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -3,8 +3,13 @@ .. automodule:: mil_usb_to_can +SubjuGator 9 +^^^^^^^^^^^^ + +.. automodule:: mil_usb_to_can.sub9 + Packet Format -^^^^^^^^^^^^^ +~~~~~~~~~~~~~ In order to reliably communicate with the USB to CAN board, a consistent packet format must be defined. @@ -44,7 +49,7 @@ sent over the serial connection to the USB to CAN board from ROS. - Second byte of Fletcher's checksum. Checksums -^^^^^^^^^ +~~~~~~~~~ All messages contain a checksum to help verify data integrity. However, receiving packets also have a special byte containing a slightly modified checksum formula. @@ -52,7 +57,7 @@ The checksum in all packets is found by adding up all bytes in the byte string, including the start/end flags, and then using modulo 16 on this result. Packet Listing -^^^^^^^^^^^^^^ +~~~~~~~~~~~~~~ Below is a listing of all available packets. The payload format is the format used by the :mod:`struct` module. For more information, see the Python documentation on the :ref:`list of format characters`, and their corresponding @@ -61,9 +66,9 @@ byte length. +------------+--------------+----------------+-------------------------------------------------------------------------+ | Message ID | Subclass ID | Payload Format | Class | +============+==============+================+=========================================================================+ -| 0x00 | 0x00 | Empty | :class:`mil_usb_to_can.NackPacket` | +| 0x00 | 0x00 | Empty | :class:`mil_usb_to_can.sub9.NackPacket` | + (Meta) +--------------+----------------+-------------------------------------------------------------------------+ -| | 0x01 | Empty | :class:`mil_usb_to_can.AckPacket` | +| | 0x01 | Empty | :class:`mil_usb_to_can.sub9.AckPacket` | +------------+--------------+----------------+-------------------------------------------------------------------------+ | 0x01 | 0x00 | Empty | :class:`sub8_thrust_and_kill_board.HeartbeatPacket` | + (Sub8 +--------------+----------------+-------------------------------------------------------------------------+ @@ -99,39 +104,36 @@ byte length. +------------+--------------+----------------+-------------------------------------------------------------------------+ CANDeviceHandle -^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.CANDeviceHandle +~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub9.CANDeviceHandle -.. autoclass:: mil_usb_to_can.CANDeviceHandle +.. autoclass:: mil_usb_to_can.sub9.CANDeviceHandle :members: SimulatedCANDeviceHandle -^^^^^^^^^^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.SimulatedCANDeviceHandle +~~~~~~~~~~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub9.SimulatedCANDeviceHandle -.. autoclass:: mil_usb_to_can.SimulatedCANDeviceHandle +.. autoclass:: mil_usb_to_can.sub9.SimulatedCANDeviceHandle :members: Packet -^^^^^^ -.. attributetable:: mil_usb_to_can.Packet +~~~~~~ +.. attributetable:: mil_usb_to_can.sub9.Packet -.. autoclass:: mil_usb_to_can.Packet +.. autoclass:: mil_usb_to_can.sub9.Packet :members: -Specific Packets -^^^^^^^^^^^^^^^^ - NackPacket ~~~~~~~~~~ -.. attributetable:: mil_usb_to_can.NackPacket +.. attributetable:: mil_usb_to_can.sub9.NackPacket -.. autoclass:: mil_usb_to_can.NackPacket +.. autoclass:: mil_usb_to_can.sub9.NackPacket :members: AckPacket ~~~~~~~~~ -.. attributetable:: mil_usb_to_can.AckPacket +.. attributetable:: mil_usb_to_can.sub9.AckPacket -.. autoclass:: mil_usb_to_can.AckPacket +.. autoclass:: mil_usb_to_can.sub9.AckPacket :members: diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py index 2501f364e..e69de29bb 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py @@ -1,2 +0,0 @@ -from .device import CANDeviceHandle, SimulatedCANDeviceHandle -from .packet import AckPacket, NackPacket, Packet diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/__init__.py new file mode 100644 index 000000000..2501f364e --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/__init__.py @@ -0,0 +1,2 @@ +from .device import CANDeviceHandle, SimulatedCANDeviceHandle +from .packet import AckPacket, NackPacket, Packet diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py similarity index 97% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py index a2ae1ca0a..6ce618e9e 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py @@ -3,8 +3,8 @@ from typing import TYPE_CHECKING if TYPE_CHECKING: - from .driver import SimulatedUSBtoCANStream, USBtoCANDriver from .packet import Packet + from .sub9_driver import SimulatedUSBtoCANStream, USBtoCANDriver class SimulatedCANDeviceHandle: diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/example.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py similarity index 100% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/example.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py similarity index 100% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/packet.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py similarity index 100% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/test_mehron.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/test_mehron.py deleted file mode 100644 index b2b18735c..000000000 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/test_mehron.py +++ /dev/null @@ -1,52 +0,0 @@ -import datetime -from dataclasses import dataclass - -import rich -import rospy -from mil_usb_to_can import CANDeviceHandle, Packet -from sub8_thrust_and_kill_board import HeartbeatPacket, KillSetPacket, ThrustSetPacket -from sub_actuator_board import ActuatorPollRequestPacket, ActuatorSetPacket - -from .packet import hexify - - -@dataclass -class TestPacket(Packet, msg_id=0x02, subclass_id=0x02, payload_format="f"): - msg: float - - -class ExampleTestDevice(CANDeviceHandle): - """ - Example implementation of a SimulatedCANDevice. - On sends, stores the transmitted data in a buffer. - When data is requested, it echos this data back. - """ - - def __init__(self, handle): - # Call parent classes constructor - super().__init__(handle) - self.index = 0 - self.types = [ - HeartbeatPacket(), - ThrustSetPacket(1, 0.5), - KillSetPacket(1), - ActuatorSetPacket(1, 2), - ActuatorPollRequestPacket(), - ] - self._timer = rospy.Timer(rospy.Duration(1), self.send) - - def send(self, _): - # packet = TestPacket(random.random()) - packet = self.types[self.index] - rich.print( - f"[bold chartreuse1]{datetime.datetime.now()}: Sending: {packet}, {hexify(bytes(packet))}" - ) - self.send_data(packet) - self.index += 1 - self.index %= len(self.types) - - def on_data(self, data): - # Echo data received back onto the bus - rich.print( - f"[bold gold1]{datetime.datetime.now()} Received: {data}, {hexify(bytes(data))}" - ) From cd5906f04781de0482eb981b40f3b938299d0552 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sat, 18 Mar 2023 14:36:29 -0400 Subject: [PATCH 18/38] All sub9-related mil_usb_to_can tests pass --- .../drivers/mil_usb_to_can/CMakeLists.txt | 4 ++-- .../drivers/mil_usb_to_can/launch/test.launch | 19 ------------------- .../mil_usb_to_can/sub9/sub9_driver.py | 6 +++--- .../mil_usb_to_can/test/adder_device.test | 15 --------------- .../test/adder_device_sub9.test | 15 +++++++++++++++ ...vice_test.py => adder_device_test_sub9.py} | 6 +++--- .../drivers/mil_usb_to_can/test/basic.test | 2 +- .../mil_usb_to_can/test/echo_device.test | 15 --------------- .../mil_usb_to_can/test/echo_device_sub9.test | 15 +++++++++++++++ ...evice_test.py => echo_device_test_sub9.py} | 6 +++--- .../{test_packets.py => test_packets_sub9.py} | 6 +++--- 11 files changed, 45 insertions(+), 64 deletions(-) delete mode 100644 mil_common/drivers/mil_usb_to_can/launch/test.launch delete mode 100644 mil_common/drivers/mil_usb_to_can/test/adder_device.test create mode 100644 mil_common/drivers/mil_usb_to_can/test/adder_device_sub9.test rename mil_common/drivers/mil_usb_to_can/test/{adder_device_test.py => adder_device_test_sub9.py} (82%) delete mode 100644 mil_common/drivers/mil_usb_to_can/test/echo_device.test create mode 100644 mil_common/drivers/mil_usb_to_can/test/echo_device_sub9.test rename mil_common/drivers/mil_usb_to_can/test/{echo_device_test.py => echo_device_test_sub9.py} (77%) rename mil_common/drivers/mil_usb_to_can/test/{test_packets.py => test_packets_sub9.py} (89%) diff --git a/mil_common/drivers/mil_usb_to_can/CMakeLists.txt b/mil_common/drivers/mil_usb_to_can/CMakeLists.txt index 8752b24e7..75810fcd7 100644 --- a/mil_common/drivers/mil_usb_to_can/CMakeLists.txt +++ b/mil_common/drivers/mil_usb_to_can/CMakeLists.txt @@ -9,7 +9,7 @@ catkin_package() if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest(test/adder_device.test) - add_rostest(test/echo_device.test) + add_rostest(test/adder_device_sub9.test) + add_rostest(test/echo_device_sub9.test) add_rostest(test/basic.test) endif() diff --git a/mil_common/drivers/mil_usb_to_can/launch/test.launch b/mil_common/drivers/mil_usb_to_can/launch/test.launch deleted file mode 100644 index b9f2610dc..000000000 --- a/mil_common/drivers/mil_usb_to_can/launch/test.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - # Path of serial device - port: /dev/serial/by-id/usb-Texas_Instruments_In-Circuit_Debug_Interface_0E237EF9-if00 - # Baudrate of device, should leave as is - baudrate: 115200 - # The CAN device id of the usb-can board - device_handles: - # Device 9 will be handled with ExampleCANDeviceHandle - mil_usb_to_can.test_mehron.ExampleTestDevice: [] - - - diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py index f0a31c534..707267e20 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py @@ -9,8 +9,8 @@ import rospy import serial from mil_misc_tools.serial_tools import SimulatedSerial -from mil_usb_to_can.device import CANDeviceHandle, SimulatedCANDeviceHandle -from mil_usb_to_can.packet import SYNC_CHAR_1, Packet, hexify +from mil_usb_to_can.sub9.device import CANDeviceHandle, SimulatedCANDeviceHandle +from mil_usb_to_can.sub9.packet import SYNC_CHAR_1, Packet from serial import SerialException if TYPE_CHECKING: @@ -159,7 +159,7 @@ def read_packet(self) -> bool: return False packed_packet = self.read_from_stream() assert isinstance(packed_packet, bytes) - rospy.logerr(f"raw: {hexify(packed_packet)}") + # rospy.logerr(f"raw: {hexify(packed_packet)}") packet = Packet.from_bytes(packed_packet) except (SerialException) as e: rospy.logerr(f"Error reading packet: {e}") diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device.test b/mil_common/drivers/mil_usb_to_can/test/adder_device.test deleted file mode 100644 index f9eb8170d..000000000 --- a/mil_common/drivers/mil_usb_to_can/test/adder_device.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - device_handles: - mil_usb_to_can.example.ExampleAdderDeviceHandle: [mil_usb_to_can.example.ExampleAdderResponsePacket] - simulated_devices: - mil_usb_to_can.example.ExampleSimulatedAdderDevice: [mil_usb_to_can.example.ExampleAdderRequestPacket] - - - - - diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device_sub9.test b/mil_common/drivers/mil_usb_to_can/test/adder_device_sub9.test new file mode 100644 index 000000000..6f8abe6ff --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device_sub9.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + mil_usb_to_can.sub9.example.ExampleAdderDeviceHandle: [mil_usb_to_can.sub9.example.ExampleAdderResponsePacket] + simulated_devices: + mil_usb_to_can.sub9.example.ExampleSimulatedAdderDevice: [mil_usb_to_can.sub9.example.ExampleAdderRequestPacket] + + + + + diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device_test.py b/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub9.py similarity index 82% rename from mil_common/drivers/mil_usb_to_can/test/adder_device_test.py rename to mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub9.py index 798bf84fb..7d55a15b8 100755 --- a/mil_common/drivers/mil_usb_to_can/test/adder_device_test.py +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub9.py @@ -5,7 +5,7 @@ from rospy_tutorials.srv import AddTwoInts, AddTwoIntsRequest -class AdderDeviceTest(unittest.TestCase): +class AdderDeviceTestSub9(unittest.TestCase): """ Integration test for CAN2USB board driver. Talks to a simulated CAN device which should add two integers @@ -33,8 +33,8 @@ def test_2service_works(self): if __name__ == "__main__": - rospy.init_node("adder_device_test", anonymous=True) + rospy.init_node("adder_device_test_sub9", anonymous=True) import rostest - rostest.rosrun("mil_usb_to_can", "adder_device_test", AdderDeviceTest) + rostest.rosrun("mil_usb_to_can", "adder_device_test_sub9", AdderDeviceTestSub9) unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/basic.test b/mil_common/drivers/mil_usb_to_can/test/basic.test index 80384f7f1..bf28ca2aa 100644 --- a/mil_common/drivers/mil_usb_to_can/test/basic.test +++ b/mil_common/drivers/mil_usb_to_can/test/basic.test @@ -1,3 +1,3 @@ - + diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device.test b/mil_common/drivers/mil_usb_to_can/test/echo_device.test deleted file mode 100644 index 9bfd44380..000000000 --- a/mil_common/drivers/mil_usb_to_can/test/echo_device.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - device_handles: - mil_usb_to_can.example.ExampleEchoDeviceHandle: [mil_usb_to_can.example.ExampleEchoResponsePacket] - simulated_devices: - mil_usb_to_can.example.ExampleSimulatedEchoDevice: [mil_usb_to_can.example.ExampleEchoRequestPacket] - - - - - diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device_sub9.test b/mil_common/drivers/mil_usb_to_can/test/echo_device_sub9.test new file mode 100644 index 000000000..64d79fd44 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device_sub9.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + mil_usb_to_can.sub9.example.ExampleEchoDeviceHandle: [mil_usb_to_can.sub9.example.ExampleEchoResponsePacket] + simulated_devices: + mil_usb_to_can.sub9.example.ExampleSimulatedEchoDevice: [mil_usb_to_can.sub9.example.ExampleEchoRequestPacket] + + + + + diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device_test.py b/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub9.py similarity index 77% rename from mil_common/drivers/mil_usb_to_can/test/echo_device_test.py rename to mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub9.py index 48a5c7a31..d99ccd236 100755 --- a/mil_common/drivers/mil_usb_to_can/test/echo_device_test.py +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub9.py @@ -5,7 +5,7 @@ from std_srvs.srv import Trigger, TriggerRequest -class EchoDeviceTest(unittest.TestCase): +class EchoDeviceTestSub9(unittest.TestCase): """ Integration test for CAN2USB board driver. Talks to a simulated CAN device which should add two integers @@ -24,8 +24,8 @@ def test_correct_response(self): if __name__ == "__main__": - rospy.init_node("echo_device_test", anonymous=True) + rospy.init_node("echo_device_test_sub9", anonymous=True) import rostest - rostest.rosrun("mil_usb_to_can", "echo_device_test", EchoDeviceTest) + rostest.rosrun("mil_usb_to_can", "echo_device_test_sub9", EchoDeviceTestSub9) unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/test_packets.py b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py similarity index 89% rename from mil_common/drivers/mil_usb_to_can/test/test_packets.py rename to mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py index e00fed023..4d5a6b8b8 100755 --- a/mil_common/drivers/mil_usb_to_can/test/test_packets.py +++ b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py @@ -3,8 +3,8 @@ from dataclasses import dataclass import rostest -from mil_usb_to_can import Packet -from mil_usb_to_can.packet import SYNC_CHAR_1, SYNC_CHAR_2 +from mil_usb_to_can.sub9 import Packet +from mil_usb_to_can.sub9.packet import SYNC_CHAR_1, SYNC_CHAR_2 @dataclass @@ -49,6 +49,6 @@ def test_comparisons(self): if __name__ == "__main__": packet = TestPacket(False, 42, 3.14) rostest.rosrun( - "mil_usb_to_can", "test_application_packets", BasicApplicationPacketTest + "mil_usb_to_can", "test_application_packets_sub9", BasicApplicationPacketTest ) unittest.main() From 4891f2708f775e449797f9ab0742ab8a566ad798 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Mon, 20 Mar 2023 16:53:21 -0400 Subject: [PATCH 19/38] Resolving resolution errors, docs build is now stable --- .../sub8_thrust_and_kill_board/__init__.py | 4 +- .../sub8_thrust_and_kill_board/handle.py | 20 +- .../sub8_thrust_and_kill_board/packets.py | 300 +++++++++-- .../sub8_thrust_and_kill_board/simulation.py | 20 +- .../sub9_thrust_and_kill_board/packets.py | 2 +- docs/reference/can.rst | 195 ++++++- docs/subjugator/reference.rst | 47 +- .../mil_usb_to_can/sub8/__init__.py | 74 +++ .../{ => sub8}/application_packet.py | 13 +- .../mil_usb_to_can/sub8/board.py | 72 +++ .../mil_usb_to_can/{ => sub8}/device.py | 5 +- .../mil_usb_to_can/sub8/driver.py | 139 +++++ .../mil_usb_to_can/sub8/simulation.py | 131 +++++ .../mil_usb_to_can/sub8/utils.py | 500 ++++++++++++++++++ 14 files changed, 1429 insertions(+), 93 deletions(-) create mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py rename mil_common/drivers/mil_usb_to_can/mil_usb_to_can/{ => sub8}/application_packet.py (93%) create mode 100755 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py rename mil_common/drivers/mil_usb_to_can/mil_usb_to_can/{ => sub8}/device.py (98%) create mode 100755 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/driver.py create mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/simulation.py create mode 100644 mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py index 52e8a1ed9..88dbf92c1 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/__init__.py @@ -1,6 +1,4 @@ from .handle import ThrusterAndKillBoard - -# from .packets import HeartbeatMessage, KillMessage, StatusMessage, ThrustPacket -from .packets import HeartbeatPacket, KillReceivePacket, KillSetPacket, ThrustSetPacket +from .packets import HeartbeatMessage, KillMessage, StatusMessage, ThrustPacket from .simulation import ThrusterAndKillBoardSimulation from .thruster import Thruster diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py index 3341f7938..5931ad039 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py @@ -1,20 +1,20 @@ #!/usr/bin/python3 import rospy -from mil_usb_to_can import CANDeviceHandle +from mil_usb_to_can.sub8 import CANDeviceHandle from ros_alarms import AlarmBroadcaster, AlarmListener from ros_alarms.msg import Alarm from rospy.timer import TimerEvent from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse from subjugator_msgs.msg import Thrust -# from .packets import ( -# KILL_SEND_ID, -# THRUST_SEND_ID, -# HeartbeatMessage, -# KillMessage, -# StatusMessage, -# ThrustPacket, -# ) +from .packets import ( + KILL_SEND_ID, + THRUST_SEND_ID, + HeartbeatMessage, + KillMessage, + StatusMessage, + ThrustPacket, +) from .thruster import make_thruster_dictionary @@ -114,7 +114,7 @@ def on_command(self, msg: Thrust) -> None: ) self.send_data(bytes(packet), can_id=THRUST_SEND_ID) - def update_hw_kill(self, status: "StatusMessage") -> None: + def update_hw_kill(self, status: StatusMessage) -> None: """ If needed, update the hw-kill alarm so it reflects the latest status from the board. diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py index f78fe7f16..93942e603 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py @@ -1,28 +1,243 @@ -from dataclasses import dataclass +import struct +from collections import namedtuple -from mil_usb_to_can import Packet +from mil_usb_to_can.sub8 import ApplicationPacket +# CAN channel to send thrust messages to +THRUST_SEND_ID = 0x21 +# CAN channel to send kill messages to +KILL_SEND_ID = 0x11 -@dataclass -class HeartbeatPacket(Packet, msg_id=0x01, subclass_id=0x00, payload_format=""): + +class KillMessage(ApplicationPacket): """ - Packet indicating a heartbeat. Sent both ways. + Represents a packet sent to kill/thrust board which contains + a command or response related to kill status. + + Inherits from :class:`~mil_usb_to_can.ApplicationPacket`. + + .. container:: operations + + .. describe:: str(x) + + Pretty-prints the class name and each of the three packet attributes. + + Attributes: + IDENTIFIER (int): The packet's identifier. Set equal to the ordinal value + of "K", or 75. + COMMAND (int): Identifier representing the packet as a command packet. + Equal to 67. This identifier should only be found in the first byte + of the payload. + RESPONSE (int): Identifier representing the packet as a response packet. + Equal to 82. This identifier should only be found in the first byte of + the payload. + HARD (int): Identifier representing the type of kill as a hard kill. Equal + to 72. This identifier is designed to be used in the second byte of the + payload. + SOFT (int): Identifier representing the type of kill as a soft kill. Equal + to 83. This identifier is designed to be used in the second byte of the + payload. + ASSERTED (int): Identifier equal to 65. This identifier is meant to be used + in the third byte of the payload. + UNASSERTED (int): Identifier equal to 85. This identifier is meant to be used + in the third byte of the payload. """ - pass + IDENTIFIER = ord("K") + COMMAND = 0x43 + RESPONSE = 0x52 + HARD = 0x48 + SOFT = 0x53 + ASSERTED = 0x41 + UNASSERTED = 0x55 + PADDING = 0x00 + + @classmethod + def create_kill_message( + cls, command: bool = False, hard: bool = False, asserted: bool = False + ): + """ + Creates a kill message containing three bytes of information, specified + as parameters. + + Args: + command (bool): Whether to make the kill message a command. If ``False``, + then the packet will represent a response. + hard (bool): Whether to make the packet kill type hard. If ``False``, + then the packet will represent a soft kill. + asserted (bool): Whether to make the packet asserted. If ``False``, then + an unasserted packet is generated. + """ + command_byte = cls.COMMAND if command else cls.RESPONSE + hard_soft_byte = cls.HARD if hard else cls.SOFT + assert_unassert_byte = cls.ASSERTED if asserted else cls.UNASSERTED + payload = struct.pack( + "BBBB", command_byte, hard_soft_byte, assert_unassert_byte, cls.PADDING + ) + return cls(cls.IDENTIFIER, payload) + + @property + def is_command(self) -> bool: + """ + Whether the packet represents a command. + + Returns: + bool: The status of the packet's command/response type. + """ + return self.payload[0] == self.COMMAND + + @property + def is_response(self) -> bool: + """ + Whether the packet represents a response. + + Returns: + bool: The status of the packet's command/response type. + """ + return self.payload[0] == self.RESPONSE + + @property + def is_hard(self) -> bool: + """ + Whether the packet represents a hard kill. + + Returns: + bool: The status of the packet's hard/soft kill type. + """ + return self.payload[1] == self.HARD + + @property + def is_soft(self) -> bool: + """ + Whether the packet represents a soft kill. + + Returns: + bool: The status of the packet's hard/soft kill type. + """ + return self.payload[1] == self.SOFT + + @property + def is_asserted(self): + """ + Whether the packet represents an asserted packet. + + Returns: + bool: The status of the packet's asserteed/unasserted type. + """ + return self.payload[2] == self.ASSERTED + + @property + def is_unasserted(self): + """ + Whether the packet represents an unasserted packet. + + Returns: + bool: The status of the packet's asserteed/unasserted type. + """ + return self.payload[2] == self.UNASSERTED + + def __str__(self): + return "KillMessage(command={}, hard={}, asserted={})".format( + self.is_command, self.is_hard, self.is_asserted + ) + + +KillStatus = namedtuple( + "KillStatus", + [ + "heartbeat_lost", + "mobo_soft_kill", + "switch_soft_kill", + "soft_killed", + "hard_killed", + "thrusters_initializing", + "go_switch", + "soft_kill_switch", + "hard_kill_switch", + ], +) + + +class StatusMessage(KillStatus): + BIT_MASK = KillStatus( + 1 << 3, 1 << 4, 1 << 5, 1 << 6, 1 << 7, 1 << 11, 1 << 12, 1 << 13, 1 << 14 + ) + STRUCT_FORMAT = "=h" + + def __new__(cls, *args): + """ + Constructs a new namedtuple to derive the class from. This can't be done + in __init__ because namedtuples are immutable. + """ + return super().__new__(cls, *args) + @classmethod + def from_bytes(cls, data): + unpacked = struct.unpack(cls.STRUCT_FORMAT, data)[0] + args = [] + for field in KillStatus._fields: + args.append(bool(unpacked & getattr(cls.BIT_MASK, field))) + return cls(*args) -@dataclass -class ThrustSetPacket(Packet, msg_id=0x01, subclass_id=0x01, payload_format="Bf"): + def __bytes__(self): + out = 0 + for field in KillStatus._fields: + if getattr(self, field): + out = out | getattr(self.BIT_MASK, field) + return struct.pack(self.STRUCT_FORMAT, out) + + +class HeartbeatMessage(ApplicationPacket): + """ + Represents the special heartbeat packet send to kill and thruster board. + + Inherits from :class:`~mil_usb_to_can.ApplicationPacket`. + + .. container:: operations + + .. describe:: str(x) + + Pretty-prints the class name. + + Attributes: + IDENTIFIER (int): The packet identifier. Set equal to the ordinal value of + "H," or 72. + """ + + IDENTIFIER = ord("H") + + @classmethod + def create(cls): + """ + Creates a new heartbeat message to be sent. + """ + return cls(cls.IDENTIFIER, struct.pack("BB", ord("M"), 0)) + + def __str__(self): + return "HeartbeatMessage()" + + +class ThrustPacket(ApplicationPacket): """ - Packet to set the speed of a specific thruster. + Represents a command send to the thrust/kill board to set the PWM of a thruster. + + Inherits from :class:`~mil_usb_to_can.ApplicationPacket`. + + .. container:: operations + + .. describe:: str(x) + + Pretty-prints the class name and each of the two packet attributes, + the thruster ID and the command. Attributes: - thruster_id (int): The ID of the thruster to set the speed of. The ID of - the thruster corresponds to a specific thruster: + IDENTIFIER (int): The packet identifier, equal to the ordinal value of "T," + or 84. + ID_MAPPING (Dict[str, int]): A dictionary mapping 3-letter thruster codes + to their respective IDs: +--------+------+ - | name | id | + | Name | ID | +========+======+ | FLH | 0 | +--------+------+ @@ -40,32 +255,53 @@ class ThrustSetPacket(Packet, msg_id=0x01, subclass_id=0x01, payload_format="Bf" +--------+------+ | BRV | 7 | +--------+------+ - speed (float): The speed to set the thruster to. """ - thruster_id: int - speed: float + IDENTIFIER = ord("T") + ID_MAPPING = { + "FLH": 0, + "FRH": 1, + "FLV": 2, + "FRV": 3, + "BLH": 4, + "BRH": 5, + "BLV": 6, + "BRV": 7, + } + @classmethod + def create_thrust_packet(cls, thruster_id: int, command: float): + """ + Creates a thruster packet given an ID and command. -@dataclass -class KillSetPacket(Packet, msg_id=0x01, subclass_id=0x02, payload_format="B"): - """ - Packet sent by the motherboard to set/unset the kill. + Args: + thruster_id (int): The ID of the thruster to create a packet for. + command (float): The command to associate with the packet. + """ + payload = struct.pack("=Bf", thruster_id, command) + return cls(cls.IDENTIFIER, payload) - Attributes: - set (bool): Whether the kill is set or unset. - """ - - set: bool + @property + def thruster_id(self) -> int: + """ + The thruster ID associated with the packet. + Returns: + int: The ID. + """ + return struct.unpack("B", self.payload[0])[0] -@dataclass -class KillReceivePacket(Packet, msg_id=0x01, subclass_id=0x03, payload_format="B"): - """ - Packet sent by the motherboard to set/unset the kill. + @property + def command(self) -> float: + """ + The command associated with the packet. - Attributes: - set (bool): Whether the kill is set or unset. - """ + Returns: + float: The associated command. + """ + return struct.unpack("f", self.payload[1:])[0] - set: bool + def __str__(self): + return "ThrustPacket(thruster_id={}, command={})".format( + self.thruster_id, self.command + ) diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py index 64ef59930..5a64359ea 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py @@ -1,19 +1,19 @@ #!/usr/bin/python3 import rospy -from mil_usb_to_can import SimulatedCANDeviceHandle +from mil_usb_to_can.sub8 import SimulatedCANDevice from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse -# from .packets import ( -# KILL_SEND_ID, -# THRUST_SEND_ID, -# HeartbeatMessage, -# KillMessage, -# StatusMessage, -# ThrustPacket, -# ) +from .packets import ( + KILL_SEND_ID, + THRUST_SEND_ID, + HeartbeatMessage, + KillMessage, + StatusMessage, + ThrustPacket, +) -class ThrusterAndKillBoardSimulation(SimulatedCANDeviceHandle): +class ThrusterAndKillBoardSimulation(SimulatedCANDevice): """ Serial simulator for the thruster and kill board, providing services to simulate physical plug connections/disconnections. diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py index b50f2bc22..f609c1c42 100644 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py @@ -1,7 +1,7 @@ from dataclasses import dataclass from enum import IntEnum -from mil_usb_to_can import Packet +from mil_usb_to_can.sub9 import Packet @dataclass diff --git a/docs/reference/can.rst b/docs/reference/can.rst index e54e365a0..817163f78 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -3,10 +3,197 @@ .. automodule:: mil_usb_to_can -SubjuGator 9 -^^^^^^^^^^^^ +:mod:`mil_usb_to_can.sub8` - SubjuGator 8 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. automodule:: mil_usb_to_can.sub8 + +Packet Format +~~~~~~~~~~~~~ +In order to reliably communicate with the USB to CAN board, a consistent packet format +must be defined. + +Below is the USBtoCAN Packet Format. This packet format wraps every message being +sent over the serial connection to the USB to CAN board from ROS. + +.. list-table:: USBtoCAN Packet Format + :header-rows: 1 + + * - Name + - Length + - Description + * - Start flag (``0xC0``) + - 1 byte + - Flag indicating the start of a new board message + * - Payload + - 4-11 bytes + - Actual data being transmitted. Often created through application packets. + The two following tables show the payloads of command and receiving packets. + * - End flag (``0xC1``) + - 1 byte + - Flag indicating end of previous board message + +Below are the payload specifications for each type of transmission. Command packets +are packets sent out by the computer to complete an action (sending or requesting +information), whereas receiving packets are packets that listen to data coming from +other devices. + +.. list-table:: Command Packet (:class:`.CommandPacket`) + :header-rows: 1 + + * - Name + - Length + - Description + * - Length + - 1 byte + - Byte indicating the length of the data being sent, or the length of the data + expected to be received, in bytes. Notably, bit 7 of this byte determines whether + the command packet is sending a command, or receiving data through a command. + If bit 7 is 1, then the command packet is receiving. + * - CAN ID + - 1 byte + - ID of the sender, or ID of who to request from + * - Data + - 1-8 bytes + - For sending command packets, the actual data being sent. For requesting command + packets, an empty binary string. + +.. list-table:: Receiving Packet (:class:`.ReceivePacket`) + :header-rows: 1 + + * - Name + - Length + - Description + * - Device ID + - 1 byte + - The CAN ID of the device to receive data from. + * - Payload length + - 1 byte + - The amount of data to listen to. + * - Data + - 1-8 bytes + - The data that was received. + * - Checksum + - 1 byte + - The checksum for the data. + +Checksums +~~~~~~~~~ +All messages contain a checksum to help verify data integrity. However, receiving +packets also have a special byte containing a slightly modified checksum formula. + +The checksum in all packets is found by adding up all bytes in the byte string, +including the start/end flags, and then using modulo 16 on this result. + +Exceptions +~~~~~~~~~~ + +Exception Hierarchy +""""""""""""""""""" +.. currentmodule:: mil_usb_to_can.sub8 + +.. exception_hierarchy:: + + - :exc:`Exception` + - :exc:`ApplicationPacketWrongIdentifierException` + - :exc:`USB2CANException` + - :exc:`ChecksumException` + - :exc:`PayloadTooLargeException` + - :exc:`InvalidFlagException` + - :exc:`InvalidStartFlagException` + - :exc:`InvalidEndFlagException` + +Exception List +""""""""""""""""""" +.. autoclass:: ApplicationPacketWrongIdentifierException + :members: + +.. autoclass:: USB2CANException + :members: + +.. autoclass:: ChecksumException + :members: + +.. autoclass:: PayloadTooLargeException + :members: + +.. autoclass:: InvalidFlagException + :members: + +.. autoclass:: InvalidStartFlagException + :members: + +.. autoclass:: InvalidEndFlagException + :members: + +ApplicationPacket +~~~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.ApplicationPacket + +.. autoclass:: mil_usb_to_can.sub8.ApplicationPacket + :members: + +USBtoCANBoard +~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.USBtoCANBoard + +.. autoclass:: mil_usb_to_can.sub8.USBtoCANBoard + :members: + +CANDeviceHandle +~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.CANDeviceHandle + +.. autoclass:: mil_usb_to_can.sub8.CANDeviceHandle + :members: + +USBtoCANDriver +~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.USBtoCANDriver + +.. autoclass:: mil_usb_to_can.sub8.USBtoCANDriver + :members: + +SimulatedCANDevice +~~~~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.SimulatedCANDevice + +.. autoclass:: mil_usb_to_can.sub8.SimulatedCANDevice + :members: + +SimulatedUSBtoCAN +~~~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.SimulatedUSBtoCAN + +.. autoclass:: mil_usb_to_can.sub8.SimulatedUSBtoCAN + :members: + +Packet +~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.Packet + +.. autoclass:: mil_usb_to_can.sub8.Packet + :members: + +ReceivePacket +~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.ReceivePacket + +.. autoclass:: mil_usb_to_can.sub8.ReceivePacket + :members: + +CommandPacket +~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.CommandPacket + +.. autoclass:: mil_usb_to_can.sub8.CommandPacket + :members: + +:mod:`mil_usb_to_can.sub9` - SubjuGator 9 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. automodule:: mil_usb_to_can.sub9 +.. currentmodule:: mil_usb_to_can.sub9 Packet Format ~~~~~~~~~~~~~ @@ -66,9 +253,9 @@ byte length. +------------+--------------+----------------+-------------------------------------------------------------------------+ | Message ID | Subclass ID | Payload Format | Class | +============+==============+================+=========================================================================+ -| 0x00 | 0x00 | Empty | :class:`mil_usb_to_can.sub9.NackPacket` | +| 0x00 | 0x00 | Empty | :class:`mil_usb_to_can.sub9.NackPacket` | + (Meta) +--------------+----------------+-------------------------------------------------------------------------+ -| | 0x01 | Empty | :class:`mil_usb_to_can.sub9.AckPacket` | +| | 0x01 | Empty | :class:`mil_usb_to_can.sub9.AckPacket` | +------------+--------------+----------------+-------------------------------------------------------------------------+ | 0x01 | 0x00 | Empty | :class:`sub8_thrust_and_kill_board.HeartbeatPacket` | + (Sub8 +--------------+----------------+-------------------------------------------------------------------------+ diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index 8648685cc..2ed8e1296 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -131,46 +131,39 @@ ThrusterAndKillBoard .. autoclass:: sub8_thrust_and_kill_board.ThrusterAndKillBoard :members: -ThrusterAndKillBoardSimulation -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation - -.. autoclass:: sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation - :members: - -Thruster -^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.Thruster +KillMessage +^^^^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.KillMessage -.. autoclass:: sub8_thrust_and_kill_board.Thruster +.. autoclass:: sub8_thrust_and_kill_board.KillMessage :members: -HeartbeatPacket -^^^^^^^^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.HeartbeatPacket +HeartbeatMessage +^^^^^^^^^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.HeartbeatMessage -.. autoclass:: sub8_thrust_and_kill_board.HeartbeatPacket +.. autoclass:: sub8_thrust_and_kill_board.HeartbeatMessage :members: -ThrustSetPacket -^^^^^^^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.ThrustSetPacket +ThrustPacket +^^^^^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.ThrustPacket -.. autoclass:: sub8_thrust_and_kill_board.ThrustSetPacket +.. autoclass:: sub8_thrust_and_kill_board.ThrustPacket :members: -KillSetPacket -^^^^^^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.KillSetPacket +ThrusterAndKillBoardSimulation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation -.. autoclass:: sub8_thrust_and_kill_board.KillSetPacket +.. autoclass:: sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation :members: -KillReceivePacket -^^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_thrust_and_kill_board.KillReceivePacket +Thruster +^^^^^^^^ +.. attributetable:: sub8_thrust_and_kill_board.Thruster -.. autoclass:: sub8_thrust_and_kill_board.KillReceivePacket +.. autoclass:: sub8_thrust_and_kill_board.Thruster :members: Sub9 Thrust and Kill Board diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py new file mode 100644 index 000000000..86cce2fee --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py @@ -0,0 +1,74 @@ +#!/usr/bin/python3 +""" +The ``mil_usb_to_can`` package implements a driver and helper class for the +USB to CAN driver board. The package provides full functionality for communication +with the board, along with helper classes to provide a better, more structured use +of data being sent to and received from the board. + +To launch the driver, start ``driver.py``, an executable Python file. This file +will spin up a driver and interface to the board. If you are starting the driver +from a launch file, you can additionally provide information for the embedded +:class:`USBtoCANBoard` class, which handles connecting to the board. This information +can be provided through local ROS parameters: + +.. code-block:: xml + + + + + + # Path of serial device (default: "/dev/tty0") + port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0 + # Baudrate of device (default: 115200) + baudrate: 115200 + # The CAN device id of the board (default: 0) + can_id: 0 + # List of python device handle classes (list of device id: python class implementation) + device_handles: + "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard + "83": sub8_actuator_board.ActuatorBoard + simulated_devices: + "18": sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation + "83": sub8_actuator_board.ActuatorBoardSimulation + + + + +Many of the parameters shown are used to connect the driver to the physical board. +However, the final two parameters, ``device_handles`` and ``simulated_devices`` +are used to create device handles for specific devices. These device handles can +send and receive data from the board, and many be used to do both, or one or the +other. + +As suggested in the above XML, the package implements drivers for a physical run, +as well as a simulated run. Whether the simulated drivers are used is controlled +by the global ``/is_simulation`` parameter. The physical drivers implement +:class:`CANDeviceHandle`, whereas the simulated drivers implement from +:class:`SimulatedCANDevice`. More information on writing device handles can be +found in the documentation of each type of class. +""" + +from .application_packet import ( + ApplicationPacket, + ApplicationPacketWrongIdentifierException, +) +from .board import USBtoCANBoard +from .device import CANDeviceHandle, ExampleAdderDeviceHandle, ExampleEchoDeviceHandle +from .driver import USBtoCANDriver +from .simulation import ( + ExampleSimulatedAdderDevice, + ExampleSimulatedEchoDevice, + SimulatedCANDevice, + SimulatedUSBtoCAN, +) +from .utils import ( + ChecksumException, + CommandPacket, + InvalidEndFlagException, + InvalidFlagException, + InvalidStartFlagException, + Packet, + PayloadTooLargeException, + ReceivePacket, + USB2CANException, +) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py similarity index 93% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py index ba0051b1d..904867cfa 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py @@ -83,7 +83,9 @@ def __bytes__(self) -> bytes: @classmethod def from_bytes( - cls: type[T], data: bytes, expected_identifier: int | None = None + cls: type[T], + data: bytes, + expected_identifier: int | None = None, ) -> T: """ Unpacks a series of packed bytes representing an application packet using @@ -108,16 +110,19 @@ def from_bytes( packet = cls(*struct.unpack(f"B{payload_len}s", data)) if expected_identifier is not None and expected_identifier != packet.identifier: raise ApplicationPacketWrongIdentifierException( - packet.identifier, expected_identifier + packet.identifier, + expected_identifier, ) return packet def __repr__(self): return "{}(identifier={}, payload={})".format( - self.__class__.__name__, self.identifier, self.payload + self.__class__.__name__, + self.identifier, + self.payload, ) def __eq__(self, other): if not isinstance(other, ApplicationPacket): - raise NotImplementedError() + raise NotImplementedError return self.identifier == other.identifier and self.payload == other.payload diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py new file mode 100755 index 000000000..285332e32 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +from threading import Lock + +# from mil_tools import hexify +from typing import Optional + +import serial + +from .simulation import SimulatedUSBtoCAN +from .utils import ( # causes error if relative import used - GH-731 + CommandPacket, + ReceivePacket, +) + + +class USBtoCANBoard: + """ + ROS-independent wrapper which provides an interface to connect to the USB to CAN board + via a serial (or simulated serial) device. Provides thread-safe functionality. + + Attributes: + lock (threading.Lock): The thread lock. + ser (Union[:class:`SimulatedUSBtoCAN`, :class:`serial.Serial`]): The serial connection. + """ + + def __init__(self, port: str, baud: int = 9600, simulated: bool = False, **kwargs): + """ + Args: + port (str): Path to serial device, such as ``/dev/ttyUSB0``. + baud (int): Baud rate of serial device to connect to. Defaults to 9600. + simulated (bool): If True, use a simulated serial device rather than a real device. Defaults to ``False``. + """ + self.lock = Lock() + if simulated: + self.ser = SimulatedUSBtoCAN(**kwargs) + else: + self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.1, **kwargs) + self.ser.flushOutput() + self.ser.flushInput() + + def read_packet(self) -> Optional[ReceivePacket]: + """ + Read a packet from the board, if available. Returns a :class:`ReceivePacket` + instance if one was succefully read, or ``None`` if the in buffer is empty. + + Returns: + Optional[:class:`ReceivePacket`]: The packet, if found, otherwise ``None``. + """ + # TODO Does this actually return ReceivePacket? Appears it might only be + # able to return Packet. + with self.lock: + if self.ser.in_waiting == 0: + return None + return ReceivePacket.read_packet(self.ser) + + def send_data(self, data: bytes, can_id: int = 0) -> None: + """ + Sends data to a CAN device using the thread lock. Writes using the :meth:`write` + method of the :attr:`.ser` attribute. + + Args: + device_id (int): CAN device ID to send data to. + data (bytes): Data (represented as bytes) to send to the device. + + Raises: + PayloadTooLargeException: The payload is larger than 8 bytes. + """ + p = CommandPacket.create_send_packet(data, can_id=can_id) + with self.lock: + p_bytes = bytes(p) + # print 'SENDING ', hexify(p_bytes) + self.ser.write(p_bytes) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/device.py similarity index 98% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/device.py index f67145180..f9014b2d2 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/device.py @@ -105,7 +105,7 @@ def on_data(self, data): raise RuntimeError(f"Received {data} but have not yet sent anything") elif data.decode() != self.last_sent[0]: raise ValueError( - f"ERROR! Received {data.decode()} but last sent {self.last_sent}" + f"ERROR! Received {data.decode()} but last sent {self.last_sent}", ) else: self.count += 1 @@ -143,7 +143,8 @@ def on_service_req(self, req): if rospy.Time.now() - start > rospy.Duration(1): return -1 res = ApplicationPacket.from_bytes( - self.response_received, expected_identifier=37 + self.response_received, + expected_identifier=37, ) my_sum = struct.unpack("i", res.payload) return my_sum diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/driver.py new file mode 100755 index 000000000..9bc981612 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/driver.py @@ -0,0 +1,139 @@ +#!/usr/bin/python3 +import importlib +from typing import TYPE_CHECKING, Any, Dict, Generator, Optional, Tuple + +import rospy +from serial import SerialException + +from .board import ( + USBtoCANBoard, # relative import causes import error with rosrun - GH-731 +) +from .utils import USB2CANException + +if TYPE_CHECKING: + from .device import CANDeviceHandle + + +class USBtoCANDriver: + """ + ROS Driver which implements the USB to CAN board. Allow users to specify a dictionary of + device handle classes to be loaded at runtime to handle communication with + specific devices. + + Attributes: + board (USBtoCANBoard): The board the driver is implementing. + handles (dict[int, CANDeviceHandle]): The handles served by the driver. Each key represents + a unique device ID, and each corresponding value represents an instance of + a child class inheriting from :class:`CANDeviceHandle`. Upon initialization, + each class is constructed after being parsed from dynamic reconfigure. + timer (rospy.Timer): The timer controlling when buffers are processed. + """ + + def __init__(self): + port = rospy.get_param("~port", "/dev/tty0") + baud = rospy.get_param("~baudrate", 115200) + can_id = rospy.get_param("~can_id", 0) + simulation = rospy.get_param("/is_simulation", False) + # If simulation mode, load simulated devices + if simulation: + rospy.logwarn( + "CAN2USB driver in simulation! Will not talk to real hardware.", + ) + devices = dict( + list( + self.parse_module_dictionary(rospy.get_param("~simulated_devices")), + ), + ) + self.board = USBtoCANBoard( + port=port, + baud=baud, + simulated=simulation, + devices=devices, + can_id=can_id, + ) + else: + self.board = USBtoCANBoard(port=port, baud=baud, simulated=simulation) + + # Add device handles from the modules specified in ROS params + self.handles: Dict[int, CANDeviceHandle] = { + device_id: cls(self, device_id) + for device_id, cls in self.parse_module_dictionary( + rospy.get_param("~device_handles"), + ) + } + + self.timer = rospy.Timer(rospy.Duration(1.0 / 20.0), self.process_in_buffer) + + def read_packet(self) -> bool: + """ + Attempt to read a packet from the board. If the packet has an appropriate device + handler, then the packet is passed to the ``on_data`` method of that handler. + + Returns: + bool: The success in reading a packet. + """ + try: + packet = self.board.read_packet() + except (SerialException, USB2CANException) as e: + rospy.logerr(f"Error reading packet: {e}") + return False + if packet is None: + return False + if packet.device in self.handles: + self.handles[packet.device].on_data(packet.data) + else: + rospy.logwarn( + "Message received for device {}, but no handle registered".format( + packet.device, + ), + ) + return True + + def process_in_buffer(self, *args) -> None: + """ + Read all available packets in the board's in-buffer. + """ + while self.read_packet(): + pass + + def send_data(self, *args, **kwargs) -> Optional[Exception]: + """ + Sends data using the :meth:`USBtoCANBoard.send_data` method. + + Returns: + Optional[Exception]: If data was sent successfully, nothing is returned. + Otherwise, the exception that was raised in sending is returned. + """ + try: + self.board.send_data(*args, **kwargs) + return None + except (SerialException, USB2CANException) as e: + rospy.logerr(f"Error writing packet: {e}") + return e + + @staticmethod + def parse_module_dictionary( + d: Dict[str, Any], + ) -> Generator[Tuple[int, Any], None, None]: + """ + Generator to load classes from module strings specified in a dictionary. + Imports all found classes. + + Yields: + Generator[Tuple[int, Any], None, None]: Yields tuples containing the device + ID and the associated class. + """ + for device_id, module_name in d.items(): + device_id = int(device_id) + # Split module from class name + module_name, cls = module_name.rsplit(".", 1) + # import module + module = importlib.import_module(module_name) + # Yield a tuple (device_id, imported_class) + yield device_id, getattr(module, cls) + + +if __name__ == "__main__": + rospy.init_node("usb_to_can_driver") + driver = USBtoCANDriver() + rospy.spin() diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/simulation.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/simulation.py new file mode 100644 index 000000000..498f72634 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/simulation.py @@ -0,0 +1,131 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import struct + +from mil_misc_tools.serial_tools import SimulatedSerial + +from .application_packet import ApplicationPacket +from .utils import CommandPacket, ReceivePacket + + +class SimulatedCANDevice: + """ + Simulates a CAN device, with functions to be overridden to handle data requests + and sends from motherboard. + + Child classes can inherit from this class to implement a simulated CAN device. + """ + + def __init__(self, sim_board: SimulatedUSBtoCAN, can_id: int): + self._sim_board = sim_board + self._can_id = can_id + + def send_data(self, data: bytes): + """ + Send data onto the bus, delivering it to other simulated devices and to + the driver node. + """ + self._sim_board.send_to_bus(self._can_id, data) + + def on_data(self, data: bytes, can_id: int): + """ + Called when the motherboard or another simulated device sends data onto the bus. + + .. note:: + + Because the CAN bus is shared, you must verify that the received data + is appropriate for your device. + + Args: + data (bytes): The data payload as a string/bytes object. + """ + + +class ExampleSimulatedEchoDevice(SimulatedCANDevice): + """ + Example implementation of a SimulatedCANDevice. + On sends, stores the transmitted data in a buffer. + When data is requested, it echos this data back. + """ + + def __init__(self, *args, **kwargs): + # Call parent classes constructor + super().__init__(*args, **kwargs) + + def on_data(self, data, can_id): + # Echo data received back onto the bus + packet = ApplicationPacket.from_bytes(data, expected_identifier=37) + self.send_data(bytes(ApplicationPacket(37, packet.payload))) + + +class ExampleSimulatedAdderDevice(SimulatedCANDevice): + """ + Example implementation of a SimulatedCANDevice. + On sends, stores the transmitted data in a buffer. + When data is requested, it echos this data back. + """ + + def __init__(self, *args, **kwargs): + # Call parent classes constructor + super().__init__(*args, **kwargs) + + def on_data(self, data, can_id): + packet = ApplicationPacket.from_bytes(data, expected_identifier=37) + a, b = struct.unpack("hh", packet.payload) + c = a + b + res = struct.pack("i", c) + self.send_data(bytes(ApplicationPacket(37, res))) + + +class SimulatedUSBtoCAN(SimulatedSerial): + """ + Simulates the USB to CAN board. Is supplied with a dictionary of simulated + CAN devices to simulate the behavior of the whole CAN network. + """ + + def __init__( + self, + devices: dict[int, type[SimulatedCANDevice]] | None = None, + can_id=-1, + ): + """ + Args: + devices (Dict[:class:`int`, Any]): Dictionary containing CAN IDs and + their associated simulated classes inheriting from :class:`SimulatedCANDevice`. + can_id (int): ID of the CAN2USB device. Defaults to -1. + """ + if devices is None: + devices = {0: SimulatedCANDevice} + + self._my_id = can_id + self._devices = { + can_id: device(self, can_id) for can_id, device in devices.items() + } + super().__init__() + + def send_to_bus(self, can_id: int, data: bytes, from_mobo: bool = False): + """ + Sends data onto the simulated bus from a simulated device. + + Args: + can_id (int): ID of sender. + data (bytes): The payload to send. + from_mobo (bool): Whether the data is from the motherboard. Defaults to + False. + """ + # If not from the motherboard, store this for future requests from motherboard + if not from_mobo: + self.buffer += bytes(ReceivePacket.create_receive_packet(can_id, data)) + # Send data to all simulated devices besides the sender + for device_can_id, device in self._devices.items(): + if device_can_id != can_id: + device.on_data(data, can_id) + + def write(self, data: bytes) -> int: + """ + Parse incoming data as a command packet from the motherboard. + """ + p = CommandPacket.from_bytes(data) + self.send_to_bus(p.filter_id, p.data, from_mobo=True) + return len(data) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py new file mode 100644 index 000000000..2d2558f57 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py @@ -0,0 +1,500 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import struct +from typing import TYPE_CHECKING, Literal, TypeVar, overload + +import serial + +if TYPE_CHECKING: + from .simulation import SimulatedUSBtoCAN + +T = TypeVar("T", bound="Packet") + + +class USB2CANException(Exception): + """ + Base class for exception in USB2CAN board functionality. Inherits from :class:`Exception`. + """ + + +class ChecksumException(USB2CANException): + """ + Exception thrown when the checksum between motherboard and CANtoUSB board is invalid. + Inherits from :class:`USB2CANException`. + """ + + def __init__(self, calculated, expected): + super().__init__( + "Checksum was calculated as {} but reported as {}".format( + calculated, + expected, + ), + ) + + +class PayloadTooLargeException(USB2CANException): + """ + Exception thrown when payload of data sent/received from CAN2USB is too large. + Inherits from :class:`USB2CANException`. + """ + + def __init__(self, length): + super().__init__( + f"Payload is {length} bytes, which is greater than the maximum of 8", + ) + + +class InvalidFlagException(USB2CANException): + """ + Exception thrown when a constant flag in the CAN2USB protocol is invalid. Inherits + from :class:`USB2CANException`. + """ + + def __init__(self, description, expected, was): + super().__init__(f"{description} flag should be {expected} but was {was}") + + +class InvalidStartFlagException(InvalidFlagException): + """ + Exception thrown when the SOF flag is invalid. Inherits from :class:`InvalidFlagException`. + """ + + def __init__(self, was: int): + super().__init__("SOF", Packet.SOF, was) + + +class InvalidEndFlagException(InvalidFlagException): + """ + Exception thrown when the EOF flag is invalid. Inherits from :class:`InvalidFlagException`. + """ + + def __init__(self, was: int): + super().__init__("EOF", Packet.EOF, was) + + +class Packet: + """ + Represents a packet to or from the CAN to USB board. This class is inherited + by :class:`~mil_usb_to_can.ReceivePacket` (for receiving data from the bus) + and :class:`~mil_usb_to_can.CommandPacket` (for sending commands). Those child + classes should be used over this class whenever possible. + + .. container:: operations + + .. describe:: bytes(x) + + Assembles the packet into a form suitable for sending through a data + stream. For this base packet class, :attr:`~.SOF`, :attr:`~.payload`, + and :attr:`~.EOF` are assembled into one byte string. + + Attributes: + payload (bytes): The payload stored in the packet. + SOF (int): Flag used to mark the beginning of each packet. Equal to `0xC0`. + EOF (int): Flag used to mark the beginning of each packet. Equal to `0xC1`. + """ + + payload: bytes + + # Flag used to mark beginning of each packet + SOF = 0xC0 + # Flag used to mark end of each packet + EOF = 0xC1 + + def __init__(self, payload: bytes): + self.payload = payload + + def __bytes__(self) -> bytes: + """ + Assembles the packet into a form suitable for sending through a data + stream. For this base packet class, :attr:`~.SOF`, :attr:`~.payload`, + and :attr:`~.EOF` are assembled into one byte string. + + Returns: + bytes: The packed bytes. + """ + return struct.pack(f"B{len(self.payload)}sB", self.SOF, self.payload, self.EOF) + + @overload + @classmethod + def _unpack_payload(cls, data: Literal[b""]) -> None: + ... + + @overload + @classmethod + def _unpack_payload(cls, data: bytes) -> bytes: + ... + + @classmethod + def _unpack_payload(cls, data: bytes) -> bytes | None: + """ + Attempts to obtain the raw data from a packed payload. + + Raises: + InvalidStartFlagException: The start flag (first unsigned integer) of + the payload is invalid. + InvalidEndFlagException: The end flag (last unsigned integer) of the payload + is invalid. + + Returns: + Optional[bytes]: The raw data inside the packet payload. If the data + has zero length, then ``None`` is returned. + """ + payload_len = len(data) - 2 + if payload_len < 1: + return None + sof, payload, eof = struct.unpack(f"B{payload_len}sB", data) + if sof != cls.SOF: + raise InvalidStartFlagException(sof) + if eof != cls.EOF: + raise InvalidEndFlagException(eof) + return payload + + @classmethod + def from_bytes(cls: type[T], data: bytes) -> T | None: + """ + Parses a packet from a packed bytes string into a Packet instance. + + Args: + data (bytes): The packed data to construct the Packet instance from. + + Returns: + Optional[Packet]: The packet instance. ``None`` is returned if the packet + contains an empty payload. + """ + payload = cls._unpack_payload(data) + if payload is None: + return None + return cls(payload) + + def __repr__(self): + return f"{self.__class__.__name__}(payload={self.payload})" + + @classmethod + def read_packet( + cls: type[T], + stream: serial.Serial | SimulatedUSBtoCAN, + ) -> T | None: + """ + Read a packet with a known size from a serial device + + Args: + stream (Union[serial.Serial, SimulatedUSBtoCAN]): A instance of a serial + device to read from. + + Raises: + InvalidStartFlagException: The start flag of the packet read was invalid. + InvalidEndFlagException: The end flag of the packet read was invalid. + + Returns: + Optional[Packet]: The read packet. If a packet was partially transmitted + (ie, starting with a character other than :attr:`~.SOF` or ending with + a character other than :attr:`~.EOF`), then ``None`` is returned. + """ + # Read until SOF is encourntered in case buffer contains the end of a previous packet + sof = None + for _ in range(10): + sof = stream.read(1) + if sof is None or len(sof) == 0: + return None + sof_int = int.from_bytes(sof, byteorder="big") + if sof_int == cls.SOF: + break + assert isinstance(sof, bytes) + sof_int = int.from_bytes(sof, byteorder="big") + if sof_int != cls.SOF: + raise InvalidStartFlagException(sof_int) + data = sof + eof = None + for _ in range(10): + eof = stream.read(1) + if eof is None or len(eof) == 0: + return None + data += eof + eof_int = int.from_bytes(eof, byteorder="big") + if eof_int == cls.EOF: + break + assert isinstance(eof, bytes) + eof_int = int.from_bytes(eof, byteorder="big") + if eof_int != cls.EOF: + raise InvalidEndFlagException(eof_int) + # print hexify(data) + return cls.from_bytes(data) + + +class ReceivePacket(Packet): + """ + Packet used to request data from the USB to CAN board. + + Attributes: + payload (bytes): The payload stored in the packet. + SOF (int): Flag used to mark the beginning of each packet. Equal to `0xC0`. + EOF (int): Flag used to mark the beginning of each packet. Equal to `0xC1`. + """ + + @property + def device(self) -> int: + """ + The device ID associated with the packet. + """ + return struct.unpack("B", self.payload[0:1])[0] + + @property + def data(self) -> bytes: + """ + The data inside the packet. + """ + return self.payload[2:-1] + + @property + def length(self): + """ + The length of the data to receive. + """ + return struct.unpack("B", self.payload[1:2])[0] + + @classmethod + def _calculate_checksum(cls, device_id, payload) -> int: + checksum = device_id + len(payload) + cls.SOF + cls.EOF + for byte in payload: + checksum += byte + return checksum % 16 + + @classmethod + def create_receive_packet(cls, device_id: int, payload: bytes) -> ReceivePacket: + """ + Creates a command packet to request data from a CAN device. + + Args: + device_id (int): The CAN device ID to request data from. + payload (bytes): The data to send in the packet. + + Returns: + ReceivePacket: The packet to request from the CAN device. + """ + if len(payload) > 8: + raise PayloadTooLargeException(len(payload)) + checksum = cls._calculate_checksum(device_id, payload) + data = struct.pack( + f"BB{len(payload)}sB", + device_id, + len(payload), + payload, + checksum, + ) + return cls(data) + + @classmethod + def from_bytes(cls, data: bytes) -> ReceivePacket: + """ + Creates a receive packet from packed bytes. This strips the checksum from + the bytes and then unpacks the data to gain the raw payload. + + Raises: + ChecksumException: The checksum found in the data differs from that + found in the data. + + Returns: + ReceivePacket: The packet constructed from the packed bytes. + """ + expected_checksum = 0 + for byte in data[:-2]: + expected_checksum += byte + expected_checksum += data[-1] + expected_checksum %= 16 + # expected_checksum = cls._calculate_checksum(data[0], data[:-1]) + real_checksum = data[-2] + if real_checksum != expected_checksum: + raise ChecksumException(expected_checksum, real_checksum) + payload = cls._unpack_payload(data) + return cls(payload) + + +def can_id(task_group, ecu_number): + return (task_group & 240) + (ecu_number & 15) + + +class CommandPacket(Packet): + """ + Represents a packet to the CAN board from the motherboard. This packet can + either request data from a device or send data to a device. + + .. container:: operations + + .. describe:: bytes(x) + + Assembles the packet into a form suitable for sending through a data + stream. + + Attributes: + payload (bytes): The payload stored in the packet. + SOF (int): Flag used to mark the beginning of each packet. Equal to `0xC0`. + EOF (int): Flag used to mark the beginning of each packet. Equal to `0xC1`. + """ + + @property + def length_byte(self) -> int: + """ + The first header byte which encodes the length and the receive flag. + + Returns: + :class:`int` + """ + return struct.unpack("B", self.payload[0:1])[0] + + @property + def is_receive(self) -> bool: + """ + True if this CommandPacket is requesting data. + + Returns: + :class:`bool` + """ + return bool(self.length_byte & 128) + + @property + def length(self) -> int: + """ + The number of bytes of data sent or requested. + + Returns: + :class:`int` + """ + return (self.length_byte & 7) + 1 + + @property + def filter_id(self) -> int: + """ + An integer representing the CAN device ID specified by this packet. + + Returns: + :class:`int` + """ + return struct.unpack("B", self.payload[1 : 1 + 1])[ + 0 + ] # [1:1+1] range used to ensure bytes, not [1] for int + + @property + def data(self) -> bytes: + """ + Returns: + bytes: The data to be sent. + """ + return self.payload[2:] + + @classmethod + def _create_command_packet( + cls, + length_byte: int, + filter_id: int, + data: bytes = b"", + ) -> CommandPacket: + """ + Creates a command packet. + + .. warning:: + + This method should rarely be used. Instead, use :meth:`.create_send_packet` + or :meth:`.create_request_packet` instead. + + Args: + length_byte (int): The first header byte + filter_id (int): The second header byte + data (bytes): Optional data payload when this is a send command. Defaults + to an empty byte string. + + Raises: + PayloadTooLargeException: The payload is larger than 8 bytes. + """ + if len(data) > 8: + raise PayloadTooLargeException(len(data)) + payload = struct.pack(f"BB{len(data)}s", length_byte, filter_id, data) + return cls(payload) + + @classmethod + def create_send_packet(cls, data: bytes, can_id: int = 0) -> CommandPacket: + """ + Creates a command packet to send data to the CAN bus from the motherboard. + + Args: + data (bytes): The data payload. + can_id (int): The ID of the device to send data to. Defaults to 0. + + Raises: + PayloadTooLargeException: The payload is larger than 8 bytes. + + Returns: + CommandPacket: The packet responsible for sending information to the CAN bus + from the motherboard. + """ + length_byte = len(data) - 1 + return cls._create_command_packet(length_byte, can_id, data) + + @classmethod + def create_request_packet( + cls, + filter_id: int, + receive_length: int, + ) -> CommandPacket: + """ + Creates a command packet to request data from a CAN device. + + Args: + filter_id (int): The CAN device ID to request data from. + receive_length (int): The number of bytes to request. + + Returns: + CommandPacket: The command packet responsible for requesting data from + a CAN device. + """ + length_byte = (receive_length - 1) | 128 + return cls._create_command_packet(length_byte, filter_id) + + def calculate_checksum(self, data: bytes) -> int: + checksum = 0 + for byte in data: + checksum += byte + return checksum % 16 + + @overload + @classmethod + def from_bytes(cls, data: Literal[b""]) -> None: + ... + + @overload + @classmethod + def from_bytes(cls: type[T], data: bytes) -> T: + ... + + @classmethod + def from_bytes(cls: type[T], data: bytes) -> T | None: + checksum_expected = 0 + checksum_expected += data[0] + checksum_expected += data[1] & 135 + for byte in data[2:]: + checksum_expected += byte + checksum_expected %= 16 + checksum_real = (data[1] & 120) >> 3 + if checksum_expected != checksum_real: + raise ChecksumException(checksum_expected, checksum_real) + payload = cls._unpack_payload(data) + if payload is None: + return None + return cls(payload) + + def __bytes__(self) -> bytes: + data = super().__bytes__() + checksum = 0 + for byte in data: + checksum += byte + checksum %= 16 + header_byte = (checksum << 3) | data[1] + data = data[:1] + chr(header_byte).encode() + data[2:] + return data + + def __str__(self): + return "CommandPacket(filter_id={}, is_receive={}, receive_length={})".format( + self.filter_id, + self.is_receive, + self.length, + ) From 0d04411f10651e674b14c63ed108fcda151ef72d Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 28 Mar 2023 14:59:28 -0400 Subject: [PATCH 20/38] Adding sub8 tests --- .../mil_usb_to_can/sub8/__init__.py | 2 +- .../sub8/{driver.py => sub8_driver.py} | 8 ++-- .../test/adder_device_sub8.test | 15 +++++++ .../test/adder_device_test_sub8.py | 40 +++++++++++++++++++ .../drivers/mil_usb_to_can/test/basic.test | 1 + .../mil_usb_to_can/test/echo_device_sub8.test | 15 +++++++ .../test/echo_device_test_sub8.py | 31 ++++++++++++++ .../test/test_application_packets.py | 4 +- 8 files changed, 110 insertions(+), 6 deletions(-) rename mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/{driver.py => sub8_driver.py} (96%) create mode 100644 mil_common/drivers/mil_usb_to_can/test/adder_device_sub8.test create mode 100755 mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub8.py create mode 100644 mil_common/drivers/mil_usb_to_can/test/echo_device_sub8.test create mode 100755 mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub8.py diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py index 0d8171b9c..e86b172fa 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py @@ -54,13 +54,13 @@ ) from .board import USBtoCANBoard from .device import CANDeviceHandle, ExampleAdderDeviceHandle, ExampleEchoDeviceHandle -from .driver import USBtoCANDriver from .simulation import ( ExampleSimulatedAdderDevice, ExampleSimulatedEchoDevice, SimulatedCANDevice, SimulatedUSBtoCAN, ) +from .sub8_driver import USBtoCANDriver from .utils import ( ChecksumException, CommandPacket, diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/sub8_driver.py similarity index 96% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/driver.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/sub8_driver.py index 9bc981612..8236940ac 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/driver.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/sub8_driver.py @@ -5,10 +5,12 @@ import rospy from serial import SerialException -from .board import ( - USBtoCANBoard, # relative import causes import error with rosrun - GH-731 +from mil_usb_to_can.sub8.board import ( + USBtoCANBoard, ) -from .utils import USB2CANException + +# relative import causes import error with rosrun - GH-731 +from mil_usb_to_can.sub8.utils import USB2CANException if TYPE_CHECKING: from .device import CANDeviceHandle diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device_sub8.test b/mil_common/drivers/mil_usb_to_can/test/adder_device_sub8.test new file mode 100644 index 000000000..5dc44b94b --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device_sub8.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + "8": mil_usb_to_can.sub8.ExampleAdderDeviceHandle + simulated_devices: + "8": mil_usb_to_can.sub8.ExampleSimulatedAdderDevice + + + + + diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub8.py b/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub8.py new file mode 100755 index 000000000..bc1486086 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub8.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from rospy_tutorials.srv import AddTwoInts, AddTwoIntsRequest + + +class AdderDeviceTest(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.srv = rospy.ServiceProxy("add_two_ints", AddTwoInts) + super().__init__(*args) + + def test_1service_exists(self): + """ + Test raising/clearing kill alarm (user kill) will cause same change in hw-kill + """ + try: + self.srv.wait_for_service(5) + except rospy.ServiceException as e: + self.fail(f"Service error: {e}") + + def test_2service_works(self): + a = 3 + b = 6 + correct_sum = a + b + res = self.srv(AddTwoIntsRequest(a=a, b=b)) + self.assertEqual(res.sum, correct_sum) + + +if __name__ == "__main__": + rospy.init_node("adder_device_test_sub8", anonymous=True) + import rostest + + rostest.rosrun("mil_usb_to_can", "adder_device_test_sub8", AdderDeviceTest) + unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/basic.test b/mil_common/drivers/mil_usb_to_can/test/basic.test index bf28ca2aa..ef4b8100b 100644 --- a/mil_common/drivers/mil_usb_to_can/test/basic.test +++ b/mil_common/drivers/mil_usb_to_can/test/basic.test @@ -1,3 +1,4 @@ + diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device_sub8.test b/mil_common/drivers/mil_usb_to_can/test/echo_device_sub8.test new file mode 100644 index 000000000..f5c2ca2c9 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device_sub8.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + "8": mil_usb_to_can.sub8.ExampleEchoDeviceHandle + simulated_devices: + "8": mil_usb_to_can.sub8.ExampleSimulatedEchoDevice + + + + + diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub8.py b/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub8.py new file mode 100755 index 000000000..cbf698b3a --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub8.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from std_srvs.srv import Trigger, TriggerRequest + + +class EchoDeviceTest(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.srv = rospy.ServiceProxy("start_echo", Trigger) + super().__init__(*args) + + def test_correct_response(self): + """ + Test we can get correct data through CAN bus at least ten times. + """ + self.srv.wait_for_service(1) + self.assertTrue(self.srv(TriggerRequest())) + + +if __name__ == "__main__": + rospy.init_node("echo_device_test_sub9", anonymous=True) + import rostest + + rostest.rosrun("mil_usb_to_can", "echo_device_test_sub9", EchoDeviceTest) + unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py b/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py index 82a428bbf..9aabf0d1d 100755 --- a/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py +++ b/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py @@ -5,7 +5,7 @@ import unittest import rostest -from mil_usb_to_can import ApplicationPacket, CommandPacket +from mil_usb_to_can.sub8 import ApplicationPacket, CommandPacket class BasicApplicationPacketTest(unittest.IsolatedAsyncioTestCase): @@ -20,7 +20,7 @@ def test_incorrect_identifiers(self): packet = ApplicationPacket(-1, b"test") with self.assertRaises(struct.error): bytes(packet) - packet = ApplicationPacket("a", b"test") + packet = ApplicationPacket(ord("a"), b"test") with self.assertRaises(struct.error): bytes(packet) From 702d1385fc833d49738b1a8127f7a622febb46d5 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 28 Mar 2023 15:04:39 -0400 Subject: [PATCH 21/38] Fix failing sub8/sub9 tests --- .../sub9_thrust_and_kill_board/test/simulated_board.test | 2 +- .../sub_actuator_board/sub_actuator_board/handle.py | 8 ++++++-- .../drivers/sub_actuator_board/test/simulated_board.test | 2 +- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test index 5633fa38c..44904f746 100644 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test @@ -2,7 +2,7 @@ - + device_handles: sub9_thrust_and_kill_board.ThrusterAndKillBoard: [sub9_thrust_and_kill_board.KillReceivePacket] diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py index 4893aaef0..0da860670 100644 --- a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py @@ -3,6 +3,7 @@ import rospy from mil_usb_to_can.sub9 import AckPacket, CANDeviceHandle + from sub_actuator_board.srv import ( GetValve, GetValveRequest, @@ -47,7 +48,10 @@ def set_valve(self, req: SetValveRequest) -> dict: message = ActuatorSetPacket(address=req.actuator, open=req.opened) self.send_data(message) rospy.loginfo( - "Set valve {} {}".format(req.actuator, "opened" if req.opened else "closed") + "Set valve {} {}".format( + req.actuator, + "opened" if req.opened else "closed", + ), ) # Wait some time for board to process command rospy.sleep(0.01) @@ -80,7 +84,7 @@ def get_valve(self, req: GetValveRequest) -> GetValveResponse: raise RuntimeError("No response from the board within 10 seconds.") response = GetValveResponse( - opened=self._recent_response.values & (1 << req.actuator) + opened=self._recent_response.values & (1 << req.actuator), ) self._recent_response = None return response diff --git a/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test b/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test index 4be33a772..7257f68ef 100644 --- a/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test +++ b/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test @@ -2,7 +2,7 @@ - + device_handles: sub_actuator_board.ActuatorBoard: [sub_actuator_board.ActuatorPollResponsePacket] From 8fd24bb6287d97c7c090a8b54517b34397b4bce1 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 28 Mar 2023 17:47:06 -0400 Subject: [PATCH 22/38] Fix up driver files to reference sub8 vs sub9 --- SubjuGator/command/subjugator_launch/launch/can.launch | 2 +- mil_common/drivers/mil_usb_to_can/launch/example.launch | 2 +- .../drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py | 4 ++-- .../drivers/mil_usb_to_can/test/test_application_packets.py | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/SubjuGator/command/subjugator_launch/launch/can.launch b/SubjuGator/command/subjugator_launch/launch/can.launch index d29f2cf6a..0ddf124b0 100644 --- a/SubjuGator/command/subjugator_launch/launch/can.launch +++ b/SubjuGator/command/subjugator_launch/launch/can.launch @@ -1,7 +1,7 @@ - + # Path of serial device diff --git a/mil_common/drivers/mil_usb_to_can/launch/example.launch b/mil_common/drivers/mil_usb_to_can/launch/example.launch index 68e69671f..630b8bd95 100644 --- a/mil_common/drivers/mil_usb_to_can/launch/example.launch +++ b/mil_common/drivers/mil_usb_to_can/launch/example.launch @@ -3,7 +3,7 @@ - + # Path of serial device diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py index e86b172fa..668fb7018 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py @@ -5,7 +5,7 @@ with the board, along with helper classes to provide a better, more structured use of data being sent to and received from the board. -To launch the driver, start ``driver.py``, an executable Python file. This file +To launch the driver, start ``sub8_driver.py``, an executable Python file. This file will spin up a driver and interface to the board. If you are starting the driver from a launch file, you can additionally provide information for the embedded :class:`USBtoCANBoard` class, which handles connecting to the board. This information @@ -14,7 +14,7 @@ .. code-block:: xml - + # Path of serial device (default: "/dev/tty0") diff --git a/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py b/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py index 9aabf0d1d..e4e4c90ae 100755 --- a/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py +++ b/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py @@ -20,7 +20,7 @@ def test_incorrect_identifiers(self): packet = ApplicationPacket(-1, b"test") with self.assertRaises(struct.error): bytes(packet) - packet = ApplicationPacket(ord("a"), b"test") + packet = ApplicationPacket("a", b"test") with self.assertRaises(struct.error): bytes(packet) From be5e60fd02fe98729092ed38f0bfb16c08047cae Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 31 May 2023 19:35:44 -0700 Subject: [PATCH 23/38] Update pre-commit hooks to latest versions (#1048) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update pre-commit hooks to latest versions updates: - [github.com/pre-commit/mirrors-clang-format: v14.0.6 → v15.0.7](https://github.com/pre-commit/mirrors-clang-format/compare/v14.0.6...v15.0.7) - [github.com/charliermarsh/ruff-pre-commit: v0.0.247 → v0.0.254](https://github.com/charliermarsh/ruff-pre-commit/compare/v0.0.247...v0.0.254) * Run apt-get update before upgrade (#1001) The package index from the runner image will go out of date and dependencies may be unresolveable. If upgrade is run before update the index can be stale for the upgrade step. * Remove clang-format upgrade because of lack of aarch64 wheels for Linux * Update pre-commit hooks to latest versions updates: - [github.com/adrienverge/yamllint.git: v1.29.0 → v1.30.0](https://github.com/adrienverge/yamllint.git/compare/v1.29.0...v1.30.0) - [github.com/psf/black: 23.1.0 → 23.3.0](https://github.com/psf/black/compare/23.1.0...23.3.0) - [github.com/pre-commit/mirrors-clang-format: v14.0.6 → v16.0.0](https://github.com/pre-commit/mirrors-clang-format/compare/v14.0.6...v16.0.0) - [github.com/PyCQA/autoflake: v2.0.1 → v2.0.2](https://github.com/PyCQA/autoflake/compare/v2.0.1...v2.0.2) - [github.com/scop/pre-commit-shfmt: v3.6.0-1 → v3.6.0-2](https://github.com/scop/pre-commit-shfmt/compare/v3.6.0-1...v3.6.0-2) - [github.com/charliermarsh/ruff-pre-commit: v0.0.254 → v0.0.260](https://github.com/charliermarsh/ruff-pre-commit/compare/v0.0.254...v0.0.260) - [github.com/codespell-project/codespell: v2.2.2 → v2.2.4](https://github.com/codespell-project/codespell/compare/v2.2.2...v2.2.4) * [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci * Fix spelling mistakes for codespell * Update pre-commit hooks to latest versions updates: - [github.com/adrienverge/yamllint.git: v1.30.0 → v1.31.0](https://github.com/adrienverge/yamllint.git/compare/v1.30.0...v1.31.0) - [github.com/pre-commit/mirrors-clang-format: v16.0.0 → v16.0.2](https://github.com/pre-commit/mirrors-clang-format/compare/v16.0.0...v16.0.2) - [github.com/PyCQA/autoflake: v2.0.2 → v2.1.1](https://github.com/PyCQA/autoflake/compare/v2.0.2...v2.1.1) - [github.com/charliermarsh/ruff-pre-commit: v0.0.260 → v0.0.263](https://github.com/charliermarsh/ruff-pre-commit/compare/v0.0.260...v0.0.263) * [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci * Update files to match new pre-commit updates --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Jarrod Sanders <50600614+kawaiiPlat@users.noreply.github.com> Co-authored-by: Cameron Brown --- .pre-commit-config.yaml | 8 ++++---- .../navigator_alarm/navigator_alarm_handlers/kill.py | 2 +- .../navigator_missions/constant_velocity.py | 2 +- .../command/subjugator_alarm/alarm_handlers/kill.py | 4 ++-- .../sub8_adis16400_imu/include/adis16400_imu/driver.h | 10 +++++----- .../sub8_depth_driver/include/depth_driver/driver.h | 2 +- .../subjugator_perception/nodes/torpedo_board.cpp | 2 +- .../include/subjugator_build_tools/backward.hpp | 8 ++++---- docs/extensions/attributetable.py | 2 +- mil_common/gnc/rawgps_common/src/rawgps_common/gps.py | 3 +-- .../mil_vision/src/mil_vision_lib/active_contours.cpp | 2 +- .../mil_vision/src/mil_vision_lib/image_filtering.cpp | 2 +- .../src/ogrid_manager.cpp | 2 +- mil_common/ros_alarms/ros_alarms/handler_template.py | 2 +- .../ros_alarms/test/roscpp/heartbeat_monitor_test.cpp | 4 ++-- mil_common/ros_alarms/test/roscpp/listener_test.cpp | 2 +- 16 files changed, 28 insertions(+), 29 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 93b2c6fc4..3819bdf70 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -11,7 +11,7 @@ ci: repos: - repo: https://github.com/adrienverge/yamllint.git - rev: v1.30.0 + rev: v1.31.0 hooks: - id: yamllint - repo: https://github.com/psf/black @@ -19,11 +19,11 @@ repos: hooks: - id: black - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v16.0.0 + rev: v16.0.2 hooks: - id: clang-format - repo: https://github.com/PyCQA/autoflake - rev: v2.0.2 + rev: v2.1.1 hooks: - id: autoflake args: [--remove-all-unused-imports, --ignore-init-module-imports] @@ -40,7 +40,7 @@ repos: exclude: ^docker|deprecated|NaviGator/simulation/VRX - repo: https://github.com/charliermarsh/ruff-pre-commit # Ruff version. - rev: 'v0.0.260' + rev: 'v0.0.263' hooks: - id: ruff args: [--fix, --exit-non-zero-on-fix] diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py index de42244da..a3f7a612b 100644 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py @@ -54,7 +54,7 @@ def raised(self, alarm): rospy.logwarn("BAG_ALWAYS or BAG_KILL not set. Not making kill bag.") else: goal = BagOnlineGoal(bag_name="kill.bag") - goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["bag_kill"] + goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["BAG_KILL"] self.bag_client.send_goal(goal, done_cb=self._online_bagger_cb) self.task_client.run_mission("Killed", done_cb=self._kill_task_cb) diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py b/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py index c9a5a503b..5ae078c90 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py @@ -61,7 +61,7 @@ def decode_parameters(cls, parameters): if not isinstance(parsed, list) or len(parsed) != 3: raise err for i in range(3): - if not (isinstance(parsed[i], int) or isinstance(parsed[i], float)): + if not (isinstance(parsed[i], (int, float))): raise err return parsed diff --git a/SubjuGator/command/subjugator_alarm/alarm_handlers/kill.py b/SubjuGator/command/subjugator_alarm/alarm_handlers/kill.py index d0d751f0a..4781a760a 100644 --- a/SubjuGator/command/subjugator_alarm/alarm_handlers/kill.py +++ b/SubjuGator/command/subjugator_alarm/alarm_handlers/kill.py @@ -117,7 +117,7 @@ def bagger_dump(self): rospy.logwarn("BAG_ALWAYS or BAG_KILL not set. Not making kill bag.") return goal = BagOnlineGoal(bag_name="kill.bag") - goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["bag_kill"] + goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["BAG_KILL"] self.bag_client.send_goal(goal, done_cb=self._bag_done_cb) def meta_predicate(self, meta_alarm, sub_alarms): @@ -170,5 +170,5 @@ def meta_predicate(self, meta_alarm, sub_alarms): # Raised if any alarms besides the two above are raised return any( - [alarm.raised for name, alarm in sub_alarms.items() if name not in ignore], + alarm.raised for name, alarm in sub_alarms.items() if name not in ignore ) diff --git a/SubjuGator/drivers/sub8_adis16400_imu/include/adis16400_imu/driver.h b/SubjuGator/drivers/sub8_adis16400_imu/include/adis16400_imu/driver.h index b62785f36..c8fd95b78 100644 --- a/SubjuGator/drivers/sub8_adis16400_imu/include/adis16400_imu/driver.h +++ b/SubjuGator/drivers/sub8_adis16400_imu/include/adis16400_imu/driver.h @@ -70,7 +70,7 @@ class Device result.header.frame_id = frame_id; result.header.stamp = ros::Time::now(); - result.orientation_covariance[0] = -1; // indicate no orientation data + result.orientation_covariance[0] = -1; // indicate no orientation data static const double GYRO_CONVERSION = 0.05 * (2 * M_PI / 360); // convert to deg/s and then to rad/s result.angular_velocity.x = get16(data + 4 + 2 * 0) * GYRO_CONVERSION; @@ -78,7 +78,7 @@ class Device result.angular_velocity.z = get16(data + 4 + 2 * 2) * GYRO_CONVERSION; result.angular_velocity_covariance[0] = result.angular_velocity_covariance[4] = result.angular_velocity_covariance[8] = - pow(0.9 * (2 * M_PI / 360), 2); // 0.9 deg/sec rms converted to rad/sec and then squared + pow(0.9 * (2 * M_PI / 360), 2); // 0.9 deg/sec rms converted to rad/sec and then squared static const double ACC_CONVERSION = 3.33e-3 * 9.80665; // convert to g's and then to m/s^2 result.linear_acceleration.x = -get16(data + 10 + 2 * 0) * ACC_CONVERSION; @@ -99,9 +99,9 @@ class Device mag_result.magnetic_field_covariance[8] = pow(1.25e-3 * 0.0001, 2); // 1.25 mgauss converted to tesla and then squared - getu16(data + 0); // flags unused - getu16(data + 2) * 2.418e-3; // supply voltage unused - get16(data + 22) * 0.14 + 25; // temperature unused + getu16(data + 0); // flags unused + getu16(data + 2) * 2.418e-3; // supply voltage unused + get16(data + 22) * 0.14 + 25; // temperature unused return true; } diff --git a/SubjuGator/drivers/sub8_depth_driver/include/depth_driver/driver.h b/SubjuGator/drivers/sub8_depth_driver/include/depth_driver/driver.h index e9c287107..26cca97c3 100644 --- a/SubjuGator/drivers/sub8_depth_driver/include/depth_driver/driver.h +++ b/SubjuGator/drivers/sub8_depth_driver/include/depth_driver/driver.h @@ -166,7 +166,7 @@ class Device void send_heartbeat() { - send_packet(ByteVec()); // heartbeat + send_packet(ByteVec()); // heartbeat uint8_t msg[] = { 4, 1, 20 }; send_packet(ByteVec(msg, msg + sizeof(msg) / sizeof(msg[0]))); // StartPublishing 20hz } diff --git a/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp b/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp index a5f2aa352..51f57e387 100644 --- a/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp +++ b/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp @@ -1494,7 +1494,7 @@ void anisotropic_diffusion(const Mat &src, Mat &dest, int t_max) if (i == 0 || i == x0.rows - 1 || j == 0 || j == x0.cols - 1) // conduction coefficient set to d = 1; // 1 p633 after equation 13 else - d = 1.0 / (1 + (gx * gx + 0 * gy * gy) * K2); // expression of g(gradient(I)) + d = 1.0 / (1 + (gx * gx + 0 * gy * gy) * K2); // expression of g(gradient(I)) // d =-exp(-(gx*gx+gy*gy)*K2); // expression of g(gradient(I)) D.at(i, j) = d; } diff --git a/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp b/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp index 9c6f0316e..35f85d221 100644 --- a/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp +++ b/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp @@ -222,7 +222,7 @@ struct hashtable using std::move; } // namespace details } // namespace backward -#else // NOT BACKWARD_ATLEAST_CXX11 +#else // NOT BACKWARD_ATLEAST_CXX11 #include namespace backward { @@ -752,7 +752,7 @@ class StackTraceImpl : public StackTraceLinuxImplHolder }; }; -#else // BACKWARD_HAS_UNWIND == 0 +#else // BACKWARD_HAS_UNWIND == 0 template <> class StackTraceImpl : public StackTraceLinuxImplHolder @@ -1902,7 +1902,7 @@ class Colorize bool _istty; }; -#else // ndef BACKWARD_SYSTEM_LINUX +#else // ndef BACKWARD_SYSTEM_LINUX namespace Color { @@ -2152,7 +2152,7 @@ class SignalHandling StackTrace st; void* error_addr = 0; -#ifdef REG_RIP // x86_64 +#ifdef REG_RIP // x86_64 error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_RIP]); #elif defined(REG_EIP) // x86_32 error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_EIP]); diff --git a/docs/extensions/attributetable.py b/docs/extensions/attributetable.py index abc041ace..bfee064d7 100644 --- a/docs/extensions/attributetable.py +++ b/docs/extensions/attributetable.py @@ -340,7 +340,7 @@ def process_cppattributetable(app, doctree: Node, fromdocname): ], ) - elif all([c in node.attributes["classes"] for c in ["cpp", "function"]]): + elif all(c in node.attributes["classes"] for c in ["cpp", "function"]): # Get the signature line of the function, where its name is stored try: descriptions = [ diff --git a/mil_common/gnc/rawgps_common/src/rawgps_common/gps.py b/mil_common/gnc/rawgps_common/src/rawgps_common/gps.py index b6acd67db..81393bb3f 100644 --- a/mil_common/gnc/rawgps_common/src/rawgps_common/gps.py +++ b/mil_common/gnc/rawgps_common/src/rawgps_common/gps.py @@ -354,8 +354,7 @@ def predict_pos_deltat_r(self, t): assert abs(t_k) < week_length / 2 if not (abs(t_k) < 6 * 60 * 60): print( - "ERROR: ephemeris predicting more than 6 hours from now (%f hours)" - % (t_k / 60 / 60,), + f"ERROR: ephemeris predicting more than 6 hours from now ({t_k / 60 / 60:f} hours)", ) n = n_0 + self.Deltan M_k = self.M_0 + n * t_k diff --git a/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp b/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp index b6b35bba6..46d902eee 100644 --- a/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp +++ b/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp @@ -135,7 +135,7 @@ void growRoute(const vector& partial, const vector& occupied, auto next_partial = partial; next_partial.push_back(new_elem); - auto next_tails = getHoodIdxs(new_elem, true); // true --> include border + auto next_tails = getHoodIdxs(new_elem, true); // true --> include border auto find_exit_itr = find(next_tails.begin(), next_tails.end(), exit); if (find_exit_itr != next_tails.end() && *find_exit_itr != tail) // add to cache if exit is a possible next tail { diff --git a/mil_common/perception/mil_vision/src/mil_vision_lib/image_filtering.cpp b/mil_common/perception/mil_vision/src/mil_vision_lib/image_filtering.cpp index bbf89ff63..6643b6408 100644 --- a/mil_common/perception/mil_vision/src/mil_vision_lib/image_filtering.cpp +++ b/mil_common/perception/mil_vision/src/mil_vision_lib/image_filtering.cpp @@ -7,7 +7,7 @@ cv::Mat rotateKernel(const cv::Mat &kernel, float theta, bool deg, bool no_expan theta = deg ? theta : theta * mil_tools::PI / 180.0f; cv::Point2f c_org{ kernel.cols * 0.5f, kernel.rows * 0.5f }; // center of original - if (no_expand) // rotates without expanding the canvas + if (no_expand) // rotates without expanding the canvas { cv::Mat result; cv::Mat rot_mat = cv::getRotationMatrix2D(c_org, theta, 1.0f); diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp index 0a8ed2d94..b921ae016 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp @@ -104,7 +104,7 @@ void OgridManager::update_config(Config const& config) ogrid_.info.height = height_meters_ / resolution_meters_per_cell_; ogrid_.info.origin.position.x = -1. * width_meters_ / 2.; //-1. * width_meters_ * resolution_meters_per_cell_ / 2.; ogrid_.info.origin.position.y = - -1. * height_meters_ / 2.; // -1. * height_meters_ * resolution_meters_per_cell_ / 2.; + -1. * height_meters_ / 2.; // -1. * height_meters_ * resolution_meters_per_cell_ / 2.; ogrid_.info.origin.orientation.w = 1; ogrid_.data.resize(ogrid_.info.width * ogrid_.info.height); ogrid_mat_ = cv::Mat(cv::Size(ogrid_.info.width, ogrid_.info.height), CV_8UC1, ogrid_.data.data()); diff --git a/mil_common/ros_alarms/ros_alarms/handler_template.py b/mil_common/ros_alarms/ros_alarms/handler_template.py index 966185a1d..90ae49350 100644 --- a/mil_common/ros_alarms/ros_alarms/handler_template.py +++ b/mil_common/ros_alarms/ros_alarms/handler_template.py @@ -105,4 +105,4 @@ def meta_predicate(self, meta_alarm: Alarm, alarms) -> Union[bool, Alarm]: should be raised, or a new Alarm object which the calling alarm should update itself to. """ - return any([alarm.raised for name, alarm in alarms.items()]) + return any(alarm.raised for name, alarm in alarms.items()) diff --git a/mil_common/ros_alarms/test/roscpp/heartbeat_monitor_test.cpp b/mil_common/ros_alarms/test/roscpp/heartbeat_monitor_test.cpp index 31650c0a7..15e173034 100644 --- a/mil_common/ros_alarms/test/roscpp/heartbeat_monitor_test.cpp +++ b/mil_common/ros_alarms/test/roscpp/heartbeat_monitor_test.cpp @@ -28,7 +28,7 @@ TEST(HeartbeatMonitorTest, heartbeatMonitorTest) // Publish heartbeat ros::Publisher heartbeat_pub = nh.advertise(heartbeat_topic, 1000); - auto pub_valid = [&heartbeat_pub](bool valid) { // Can publish a valid or invalid heartbeat + auto pub_valid = [&heartbeat_pub](bool valid) { // Can publish a valid or invalid heartbeat std_msgs::String msg; msg.data = (valid ? "Will pass the predicate" : ""); // second one won't heartbeat_pub.publish(msg); @@ -97,7 +97,7 @@ TEST(HeartbeatMonitorTest, heartbeatMonitorTest) while (ros::Time::now() - hb_monitor.getLastBeatTime() < time_to_clear) // Shouldn't recover here, { // invalid heartbeat sleep_until(time_to_raise * 0.8, ros::Time::now()); - pub_valid(false); // False --> publish invalid heartbeat + pub_valid(false); // False --> publish invalid heartbeat } EXPECT_TRUE(listener.queryRaised()); diff --git a/mil_common/ros_alarms/test/roscpp/listener_test.cpp b/mil_common/ros_alarms/test/roscpp/listener_test.cpp index a9367fe03..2138546e5 100644 --- a/mil_common/ros_alarms/test/roscpp/listener_test.cpp +++ b/mil_common/ros_alarms/test/roscpp/listener_test.cpp @@ -127,7 +127,7 @@ TEST(ListenerTest, listenerTest) listener.addRaiseCb(raise_cb); // Use this overload for any severity raise listener.addClearCb(clear_cb); // Called for any clear of the alarm - ros::Duration latency(0.01); // Approximate upper bound on publisher latency + ros::Duration latency(0.01); // Approximate upper bound on publisher latency auto update_start = update_count; auto lo_start = lo_priority_raise_count; auto hi_start = hi_priority_raise_count; From c635a958b9bae9ccab5e3cd4240d896caf2494ba Mon Sep 17 00:00:00 2001 From: Andres Pulido <52979429+andrespulido8@users.noreply.github.com> Date: Wed, 31 May 2023 21:38:56 -0500 Subject: [PATCH 24/38] remove hardcoded thruster name id map from script to it's only in yaml (#1050) * remove hardcoded thruster name id map from script to it's only in yaml file * Using variable name without number, add basic typing --------- Co-authored-by: Cameron Brown --- .../sub8_thrust_and_kill_board/handle.py | 4 +-- .../sub8_thrust_and_kill_board/packets.py | 32 ------------------- .../sub8_thrust_and_kill_board/thruster.py | 15 ++++++--- 3 files changed, 12 insertions(+), 39 deletions(-) diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py index 336e0336d..59450765c 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py @@ -26,7 +26,7 @@ class ThrusterAndKillBoard(CANDeviceHandle): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) # Initialize thruster mapping from params - self.thrusters = make_thruster_dictionary( + self.thrusters, self.name_to_id = make_thruster_dictionary( rospy.get_param("/thruster_layout/thrusters"), ) # Tracks last hw-kill alarm update @@ -118,7 +118,7 @@ def on_command(self, msg: Thrust) -> None: effort = self.thrusters[cmd.name].effort_from_thrust(cmd.thrust) # Send packet to command specified thruster the specified force packet = ThrustPacket.create_thrust_packet( - ThrustPacket.ID_MAPPING[cmd.name], + self.name_to_id[cmd.name], effort, ) self.send_data(bytes(packet), can_id=THRUST_SEND_ID) diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py index 519d4e829..f8b2b42b6 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py @@ -250,41 +250,9 @@ class ThrustPacket(ApplicationPacket): Attributes: IDENTIFIER (int): The packet identifier, equal to the ordinal value of "T," or 84. - ID_MAPPING (Dict[str, int]): A dictionary mapping 3-letter thruster codes - to their respective IDs: - - +--------+------+ - | Name | ID | - +========+======+ - | FLH | 0 | - +--------+------+ - | FRH | 1 | - +--------+------+ - | FLV | 2 | - +--------+------+ - | FRV | 3 | - +--------+------+ - | BLH | 4 | - +--------+------+ - | BRH | 5 | - +--------+------+ - | BLV | 6 | - +--------+------+ - | BRV | 7 | - +--------+------+ """ IDENTIFIER = ord("T") - ID_MAPPING = { - "FLH": 0, - "FRH": 1, - "FLV": 2, - "FRV": 3, - "BLH": 4, - "BRH": 5, - "BLV": 6, - "BRV": 7, - } @classmethod def create_thrust_packet(cls, thruster_id: int, command: float): diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py index 5a90baac0..7fe9aef12 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py @@ -1,16 +1,21 @@ -from typing import Any, Dict +from __future__ import annotations + +from typing import Any from numpy import clip, polyval -def make_thruster_dictionary(dictionary): +def make_thruster_dictionary(dictionary) -> tuple[dict[str, Thruster], dict[str, int]]: """ - Make a dictionary mapping thruster names to :class:`Thruster` objects. + Make a dictionary mapping thruster names to :class:`Thruster` objects + and a dictionary mapping thruster names to node IDs. """ ret = {} + name_id_map = {} for thruster, content in dictionary.items(): ret[thruster] = Thruster.from_dict(content) - return ret + name_id_map[thruster] = content["node_id"] + return ret, name_id_map class Thruster: @@ -27,7 +32,7 @@ def __init__(self, forward_calibration, backward_calibration): self.backward_calibration = backward_calibration @classmethod - def from_dict(cls, data: Dict[str, Dict[str, Any]]): + def from_dict(cls, data: dict[str, dict[str, Any]]): """ Constructs the class from a dictionary. The dictionary should be formatted as so: From d266d2908fc6c88b85703f6844ce5136892bc781 Mon Sep 17 00:00:00 2001 From: Andres Pulido <52979429+andrespulido8@users.noreply.github.com> Date: Fri, 2 Jun 2023 22:47:08 -0400 Subject: [PATCH 25/38] Update SubjuGator Testing checklist (#1052) * remove hardcoded thruster name id map from script to it's only in yaml file * expand testing checklist w foam and other items --------- Co-authored-by: Cameron Brown --- docs/testingprocedures.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/docs/testingprocedures.md b/docs/testingprocedures.md index 6b8a1c244..2fcf81852 100644 --- a/docs/testingprocedures.md +++ b/docs/testingprocedures.md @@ -1,4 +1,4 @@ -# Testing Procedures +# SubjuGator Testing Procedures ## Before leaving @@ -14,18 +14,23 @@ * Laptops * 7/16 wrench * 5/32 Allen Key +* Duct tape and scissor +* Buoyancy foams for the sub * Allen Key for paintball tank * Pliers * Flat-head screwdriver * Kill wand +* Multimeter * Goggles / Snorkeling Tube * Pool noddles * Dive fins -* O'ring grease +* O'ring grease (Molykote 55) +* Cable grease (Molykote 44) * Hot glue gun * Zip ties * clippers * Towels +* [Sunscreen](https://www.youtube.com/watch?v=sTJ7AzBIJoI) ### Software/Electrical Checklist From c502a07d8bd13ac944866ca8ed4b281e56864cf0 Mon Sep 17 00:00:00 2001 From: JohnDaBaus <92892515+JohnDaBaus@users.noreply.github.com> Date: Sat, 3 Jun 2023 02:52:38 -0400 Subject: [PATCH 26/38] Update shipping.md (#1021) * Update shipping.md * Update shipping.md I emailed Mr.Schwartz, and he said that having the sender's name in the note would be most useful. * Improve description of shipping note --------- Co-authored-by: Cameron Brown --- docs/infrastructure/shipping.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/docs/infrastructure/shipping.md b/docs/infrastructure/shipping.md index 068280f14..d0636bf21 100644 --- a/docs/infrastructure/shipping.md +++ b/docs/infrastructure/shipping.md @@ -34,3 +34,10 @@ ion base, many shipping carriers won't take batteries without special precaution You will need to review these in order to ship them through standard shipping. For some locations, it may be easier to buy batteries at the competition site rather than to take batteries with you. + +## Note + +Whenever a package is received in MIL, an email needs to be sent to Dr. Schwartz. +The email should contain the sender's name of the package, and a picture of the +packing receipt, if one is included. This should be done by the lab member who +opens up the package. From 1e07dbb9a1492080503cc7f477d9bbc07ff0d3a3 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sat, 3 Jun 2023 10:08:28 -0700 Subject: [PATCH 27/38] Bump requests from 2.22.0 to 2.31.0 (#1051) * Bump requests from 2.22.0 to 2.31.0 Bumps [requests](https://github.com/psf/requests) from 2.22.0 to 2.31.0. - [Release notes](https://github.com/psf/requests/releases) - [Changelog](https://github.com/psf/requests/blob/main/HISTORY.md) - [Commits](https://github.com/psf/requests/compare/v2.22.0...v2.31.0) --- updated-dependencies: - dependency-name: requests dependency-type: direct:production ... Signed-off-by: dependabot[bot] * Bump requests from 2.22.0 to 2.31.0 Bumps [requests](https://github.com/psf/requests) from 2.22.0 to 2.31.0. - [Release notes](https://github.com/psf/requests/releases) - [Changelog](https://github.com/psf/requests/blob/main/HISTORY.md) - [Commits](https://github.com/psf/requests/compare/v2.22.0...v2.31.0) --- updated-dependencies: - dependency-name: requests dependency-type: direct:production ... Signed-off-by: dependabot[bot] --------- Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Cameron Brown --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 7a6d801fe..ae0ac3d2e 100644 --- a/requirements.txt +++ b/requirements.txt @@ -48,4 +48,4 @@ torchvision>=0.13.1 opencv-python==4.6.0.66 seaborn==0.11.2 thop==0.1.1.post2207130030 -requests==2.22.0 +requests==2.31.0 From 2bd5edecec745a82cc89d7b62eef3017874998b4 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sat, 3 Jun 2023 17:40:01 -0700 Subject: [PATCH 28/38] Reformat files with pre-commit --- .../sub9_thrust_and_kill_board/handle.py | 30 ++++++++------ .../sub9_thrust_and_kill_board/simulation.py | 3 +- .../test/simulated_board_test.py | 4 +- .../sub_actuator_board/packets.py | 10 ++++- .../sub_actuator_board/simulation.py | 6 +-- .../mil_usb_to_can/sub9/device.py | 4 +- .../mil_usb_to_can/sub9/example.py | 22 ++++++++--- .../mil_usb_to_can/sub9/packet.py | 17 ++++---- .../mil_usb_to_can/sub9/sub9_driver.py | 39 ++++++++++++------- .../mil_usb_to_can/test/test_packets_sub9.py | 4 +- 10 files changed, 92 insertions(+), 47 deletions(-) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py index ef710e264..5cfee2051 100644 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py @@ -41,7 +41,7 @@ def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) # Initialize thruster mapping from params self.thrusters = make_thruster_dictionary( - rospy.get_param("/thruster_layout/thrusters") + rospy.get_param("/thruster_layout/thrusters"), ) # Tracks last hw-kill alarm update self._last_hw_kill = None @@ -49,7 +49,8 @@ def __init__(self, *args, **kwargs): self._kill_broadcaster = AlarmBroadcaster("hw-kill") # Listens to hw-kill updates to ensure another nodes doesn't manipulate it self._hw_kill_listener = AlarmListener( - "hw-kill", callback_funct=self.on_hw_kill + "hw-kill", + callback_funct=self.on_hw_kill, ) # Provide service for alarm handler to set/clear the motherboard kill self._unkill_service = rospy.Service("/set_mobo_kill", SetBool, self.set_kill) @@ -57,7 +58,10 @@ def __init__(self, *args, **kwargs): self._heartbeat_timer = rospy.Timer(rospy.Duration(0.4), self.send_heartbeat) # Create a subscribe for thruster commands self._sub = rospy.Subscriber( - "/thrusters/thrust", Thrust, self.on_command, queue_size=10 + "/thrusters/thrust", + Thrust, + self.on_command, + queue_size=10, ) self._last_heartbeat = rospy.Time.now() self._last_packet = None @@ -93,7 +97,8 @@ def set_kill(self, req: SetBoolRequest) -> SetBoolResponse: return SetBoolResponse(success=True) else: return SetBoolResponse( - success=False, message="No response from board after 1 second." + success=False, + message="No response from board after 1 second.", ) def send_heartbeat(self, _: TimerEvent) -> None: @@ -124,7 +129,7 @@ def on_command(self, msg: Thrust) -> None: # If we don't have a mapping for this thruster, ignore it if cmd.name not in self.thrusters: rospy.logwarn( - f"Command received for {cmd.name}, but this is not a thruster." + f"Command received for {cmd.name}, but this is not a thruster.", ) continue # Map commanded thrust (in newetons) to effort value (-1 to 1) @@ -146,21 +151,21 @@ def update_software_kill(self, raised: bool, message: str): ): if raised: self._kill_broadcaster.raise_alarm( - severity=severity, problem_description=message + severity=severity, + problem_description=message, ) else: self._kill_broadcaster.clear_alarm(severity=severity) def on_data( - self, data: AckPacket | NackPacket | HeartbeatReceivePacket | KillReceivePacket + self, + data: AckPacket | NackPacket | HeartbeatReceivePacket | KillReceivePacket, ) -> None: """ Parse the two bytes and raise kills according to a set of specifications listed below. """ - if isinstance(data, AckPacket): - self._last_packet = data - elif isinstance(data, NackPacket): + if isinstance(data, (AckPacket, NackPacket)): self._last_packet = data elif isinstance(data, HeartbeatReceivePacket): self._last_heartbeat = rospy.Time.now() @@ -169,7 +174,8 @@ def on_data( self.update_software_kill(False, "") elif data.status is KillStatus.MOBO_HEARTBEAT_LOST: self.update_software_kill( - True, "Thrust/kill board lost heartbeat from motherboard." + True, + "Thrust/kill board lost heartbeat from motherboard.", ) elif data.status is KillStatus.BATTERY_LOW: self.update_software_kill(True, "Battery too low.") @@ -184,7 +190,7 @@ def on_data( elif data.status is KillStatus.SOFTWARE_REQUESTED: self.update_software_kill( True, - f"Software requested kill.", + "Software requested kill.", ) else: raise ValueError(f"Not expecting packet of type {data.__class__.__name__}!") diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py index 25140fff0..52deb3181 100644 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py @@ -69,7 +69,8 @@ def heartbeat_timedout(self) -> bool: ) def on_data( - self, packet: HeartbeatSetPacket | ThrustSetPacket | KillSetPacket + self, + packet: HeartbeatSetPacket | ThrustSetPacket | KillSetPacket, ) -> None: """ Serves as the data handler for the device. Handles :class:`KillMessage`, diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py index d2b1f0195..69ee991be 100755 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py @@ -34,6 +34,8 @@ def test_correct_response(self): import rostest rostest.rosrun( - "sub9_thrust_and_kill_board", "simulated_board_test", SimulatedBoardTest + "sub9_thrust_and_kill_board", + "simulated_board_test", + SimulatedBoardTest, ) unittest.main() diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py index 17a1d09ca..9a146f363 100644 --- a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py @@ -20,7 +20,10 @@ class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="B @dataclass class ActuatorPollRequestPacket( - Packet, msg_id=0x03, subclass_id=0x01, payload_format="" + Packet, + msg_id=0x03, + subclass_id=0x01, + payload_format="", ): """ Packet used by the actuator board to request the status of all valves. @@ -31,7 +34,10 @@ class ActuatorPollRequestPacket( @dataclass class ActuatorPollResponsePacket( - Packet, msg_id=0x03, subclass_id=0x02, payload_format="B" + Packet, + msg_id=0x03, + subclass_id=0x02, + payload_format="B", ): """ Packet used by the actuator board to return the status of all valves. diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py index 4ffa1bbb9..dd77921e1 100644 --- a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py @@ -49,7 +49,7 @@ def on_data(self, packet: ActuatorSetPacket | ActuatorPollRequestPacket) -> None str(int(x)) for x in reversed(self.status.values()) ), base=2, - ) - ) - ) + ), + ), + ), ) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py index 6ce618e9e..1e7638e75 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py @@ -21,7 +21,9 @@ class SimulatedCANDeviceHandle: """ def __init__( - self, sim_board: SimulatedUSBtoCANStream, inbound_packets: list[type[Packet]] + self, + sim_board: SimulatedUSBtoCANStream, + inbound_packets: list[type[Packet]], ): self._sim_board = sim_board self.inbound_packets = inbound_packets diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py index d8bffdb45..d0467b41d 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py @@ -12,21 +12,30 @@ @dataclass class ExampleEchoRequestPacket( - Packet, msg_id=0x99, subclass_id=0x00, payload_format="10s" + Packet, + msg_id=0x99, + subclass_id=0x00, + payload_format="10s", ): my_special_string: bytes @dataclass class ExampleEchoResponsePacket( - Packet, msg_id=0x99, subclass_id=0x01, payload_format="10s" + Packet, + msg_id=0x99, + subclass_id=0x01, + payload_format="10s", ): my_special_string: bytes @dataclass class ExampleAdderRequestPacket( - Packet, msg_id=0x99, subclass_id=0x02, payload_format="BB" + Packet, + msg_id=0x99, + subclass_id=0x02, + payload_format="BB", ): num_one: int num_two: int @@ -34,7 +43,10 @@ class ExampleAdderRequestPacket( @dataclass class ExampleAdderResponsePacket( - Packet, msg_id=0x99, subclass_id=0x03, payload_format="B" + Packet, + msg_id=0x99, + subclass_id=0x03, + payload_format="B", ): response: int @@ -63,7 +75,7 @@ def on_data(self, data: ExampleEchoRequestPacket): raise RuntimeError(f"Received {data} but have not yet sent anything") elif response != self.last_sent[0]: raise ValueError( - f"ERROR! Received {response} but last sent {self.last_sent}" + f"ERROR! Received {response} but last sent {self.last_sent}", ) else: self.count += 1 diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py index 5d3e5a2ae..59ddc3a72 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py @@ -23,10 +23,13 @@ def get_cache_hints(cls): class ChecksumException(OSError): def __init__( - self, packet: type[Packet], received: tuple[int, int], expected: tuple[int, int] + self, + packet: type[Packet], + received: tuple[int, int], + expected: tuple[int, int], ): super().__init__( - f"Invalid checksum in packet of type {packet.__qualname__}: received {received}, expected {expected}" + f"Invalid checksum in packet of type {packet.__qualname__}: received {received}, expected {expected}", ) @@ -85,12 +88,12 @@ def __init_subclass__(cls, msg_id: int, subclass_id: int, payload_format: str = for packet in packets: if packet.msg_id == msg_id and packet.subclass_id == subclass_id: raise ValueError( - f"Cannot reuse msg_id 0x{msg_id:0x} and subclass_id 0x{subclass_id}, already used by {packet.__qualname__}" + f"Cannot reuse msg_id 0x{msg_id:0x} and subclass_id 0x{subclass_id}, already used by {packet.__qualname__}", ) _packet_registry.setdefault(msg_id, {})[subclass_id] = cls def __post_init__(self): - for (name, field_type) in get_cache_hints(self.__class__).items(): + for name, field_type in get_cache_hints(self.__class__).items(): if ( name not in [ @@ -123,7 +126,7 @@ def __bytes__(self): payload, ) checksum = self._calculate_checksum(data[2:]) - return data + struct.pack(f"BB", *checksum) + return data + struct.pack("BB", *checksum) @classmethod def from_bytes(cls, packed: bytes) -> Packet: @@ -139,7 +142,7 @@ def from_bytes(cls, packed: bytes) -> Packet: subclass = _packet_registry[msg_id][subclass_id] payload = packed[6:-2] if struct.unpack("BB", packed[-2:]) != cls._calculate_checksum( - packed[2:-2] + packed[2:-2], ): raise ChecksumException( subclass, @@ -149,7 +152,7 @@ def from_bytes(cls, packed: bytes) -> Packet: unpacked = struct.unpack(subclass.payload_format, payload) return subclass(*unpacked) raise LookupError( - f"Attempted to reconstruct packet with msg_id 0x{msg_id:02x} and subclass_id 0x{subclass_id:02x}, but no packet with IDs was found." + f"Attempted to reconstruct packet with msg_id 0x{msg_id:02x} and subclass_id 0x{subclass_id:02x}, but no packet with IDs was found.", ) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py index 707267e20..59db73c99 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py @@ -9,13 +9,15 @@ import rospy import serial from mil_misc_tools.serial_tools import SimulatedSerial +from serial import SerialException + from mil_usb_to_can.sub9.device import CANDeviceHandle, SimulatedCANDeviceHandle from mil_usb_to_can.sub9.packet import SYNC_CHAR_1, Packet -from serial import SerialException if TYPE_CHECKING: HandlePacketListing = tuple[ - type[SimulatedCANDeviceHandle | CANDeviceHandle], list[Packet] + type[SimulatedCANDeviceHandle | CANDeviceHandle], + list[Packet], ] @@ -68,7 +70,7 @@ def send_to_bus(self, data: bytes, *, from_mobo=False): if not sent and from_mobo: rospy.logerr( - f"{sent}, {from_mobo}: Received packet of type {p.__class__.__qualname__} on simulated bus, but no one is listening for it..." + f"{sent}, {from_mobo}: Received packet of type {p.__class__.__qualname__} on simulated bus, but no one is listening for it...", ) def write(self, data: bytes) -> None: @@ -93,14 +95,14 @@ class USBtoCANDriver: def __init__(self): port = rospy.get_param("~port", "/dev/tty0") baud = rospy.get_param("~baudrate", 115200) - can_id = rospy.get_param("~can_id", 0) + rospy.get_param("~can_id", 0) simulation = rospy.get_param("/is_simulation", False) self.lock = Lock() self._packet_deque = deque() # If simulation mode, load simulated devices if simulation: rospy.logwarn( - "CAN2USB driver in simulation! Will not talk to real hardware." + "CAN2USB driver in simulation! Will not talk to real hardware.", ) devices = self.read_devices(simulated=True) self.stream = SimulatedUSBtoCANStream(devices=devices) @@ -141,7 +143,7 @@ def read_from_stream(self) -> bytes | None: length = self.stream.read(2) # read payload length data += length data += self.stream.read( - int.from_bytes(length, byteorder="little") + 2 + int.from_bytes(length, byteorder="little") + 2, ) # read data and checksum return data @@ -161,7 +163,7 @@ def read_packet(self) -> bool: assert isinstance(packed_packet, bytes) # rospy.logerr(f"raw: {hexify(packed_packet)}") packet = Packet.from_bytes(packed_packet) - except (SerialException) as e: + except SerialException as e: rospy.logerr(f"Error reading packet: {e}") return False if packet is None: @@ -170,7 +172,7 @@ def read_packet(self) -> bool: self._inbound_listing[packet.__class__].on_data(packet) elif not self._packet_deque: rospy.logwarn( - f"Message of type {packet.__class__.__qualname__} received, but no device ready to accept" + f"Message of type {packet.__class__.__qualname__} received, but no device ready to accept", ) else: self._packet_deque.popleft().on_data(packet) @@ -187,7 +189,9 @@ def process_in_buffer(self, *args) -> None: rospy.logerr(f"Error when reading packets: {e}") def send_data( - self, handle: CANDeviceHandle | SimulatedCANDeviceHandle, packet: Packet + self, + handle: CANDeviceHandle | SimulatedCANDeviceHandle, + packet: Packet, ) -> Exception | None: """ Sends data using the :meth:`USBtoCANBoard.send_data` method. @@ -201,27 +205,34 @@ def send_data( self.stream.write(bytes(packet)) self._packet_deque.append(handle) return None - except (SerialException) as e: + except SerialException as e: rospy.logerr(f"Error writing packet: {e}") return e @overload def read_devices( - self, *, simulated: Literal[True] + self, + *, + simulated: Literal[True], ) -> list[tuple[type[SimulatedCANDeviceHandle], list[type[Packet]]]]: ... @overload def read_devices( - self, *, simulated: Literal[False] + self, + *, + simulated: Literal[False], ) -> list[tuple[type[CANDeviceHandle], list[type[Packet]]]]: ... def read_devices( - self, *, simulated: bool + self, + *, + simulated: bool, ) -> list[ tuple[ - type[SimulatedCANDeviceHandle] | type[CANDeviceHandle], list[type[Packet]] + type[SimulatedCANDeviceHandle] | type[CANDeviceHandle], + list[type[Packet]], ], ]: """ diff --git a/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py index 4d5a6b8b8..01c1dc47a 100755 --- a/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py +++ b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py @@ -49,6 +49,8 @@ def test_comparisons(self): if __name__ == "__main__": packet = TestPacket(False, 42, 3.14) rostest.rosrun( - "mil_usb_to_can", "test_application_packets_sub9", BasicApplicationPacketTest + "mil_usb_to_can", + "test_application_packets_sub9", + BasicApplicationPacketTest, ) unittest.main() From d259fb61d90b5f452ada6977d0d41f3389afa16f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 9 Jun 2023 21:52:02 -0700 Subject: [PATCH 29/38] Update pre-commit hooks to latest versions (#1055) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/adrienverge/yamllint.git: v1.31.0 → v1.32.0](https://github.com/adrienverge/yamllint.git/compare/v1.31.0...v1.32.0) - [github.com/pre-commit/mirrors-clang-format: v16.0.2 → v16.0.4](https://github.com/pre-commit/mirrors-clang-format/compare/v16.0.2...v16.0.4) - [github.com/shellcheck-py/shellcheck-py: v0.9.0.2 → v0.9.0.5](https://github.com/shellcheck-py/shellcheck-py/compare/v0.9.0.2...v0.9.0.5) - [github.com/charliermarsh/ruff-pre-commit: v0.0.263 → v0.0.270](https://github.com/charliermarsh/ruff-pre-commit/compare/v0.0.263...v0.0.270) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .pre-commit-config.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 3819bdf70..d2f0e2009 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -11,7 +11,7 @@ ci: repos: - repo: https://github.com/adrienverge/yamllint.git - rev: v1.31.0 + rev: v1.32.0 hooks: - id: yamllint - repo: https://github.com/psf/black @@ -19,7 +19,7 @@ repos: hooks: - id: black - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v16.0.2 + rev: v16.0.4 hooks: - id: clang-format - repo: https://github.com/PyCQA/autoflake @@ -28,7 +28,7 @@ repos: - id: autoflake args: [--remove-all-unused-imports, --ignore-init-module-imports] - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.9.0.2 + rev: v0.9.0.5 hooks: - id: shellcheck exclude: ^docker|deprecated|NaviGator/simulation/VRX @@ -40,7 +40,7 @@ repos: exclude: ^docker|deprecated|NaviGator/simulation/VRX - repo: https://github.com/charliermarsh/ruff-pre-commit # Ruff version. - rev: 'v0.0.263' + rev: 'v0.0.270' hooks: - id: ruff args: [--fix, --exit-non-zero-on-fix] From 5a2bcb17ee4e2bcb0bedf132b2399f037cc0b191 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sun, 11 Jun 2023 13:58:32 -0700 Subject: [PATCH 30/38] Attempting to expand time length for wait_for_service to try to get around timeout errors --- .../sub9_thrust_and_kill_board/test/simulated_board_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py index 69ee991be..4af9813d7 100755 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py @@ -21,7 +21,7 @@ def test_correct_response(self): """ Test we can get correct data through CAN bus at least ten times. """ - self.kill_srv.wait_for_service(1) + self.kill_srv.wait_for_service(5) self.hw_alarm_listener.wait_for_server() self.assertTrue(self.kill_srv(SetBoolRequest(True)).success) self.assertTrue(self.hw_alarm_listener.is_raised(True)) From d2759c9b14f4e63932b56a5774fb93afb5ce98b9 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sun, 11 Jun 2023 14:12:37 -0700 Subject: [PATCH 31/38] temp sleep debuggin --- .github/workflows/ci.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c69ca062a..d4e7b9e66 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -67,6 +67,9 @@ jobs: # catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3 \ # -DPYTHON_INCLUDE_DIR=/usr/include/python3.8 \ # -j6 + - name: temp sleep + run: sleep 15000s + shell: bash - name: Run catkin_make tests run: | export HOME=$GITHUB_WORKSPACE # Temporary fix for setup scripts From e450fe1d7a2ab09d42e8c916aade5c02f974c6be Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Sun, 11 Jun 2023 14:25:39 -0700 Subject: [PATCH 32/38] Update Alarm msg to be from ros_alarms_msgs --- .github/workflows/ci.yaml | 3 --- .../sub8_thrust_and_kill_board/handle.py | 2 +- .../sub9_thrust_and_kill_board/handle.py | 4 ++-- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index d4e7b9e66..c69ca062a 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -67,9 +67,6 @@ jobs: # catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3 \ # -DPYTHON_INCLUDE_DIR=/usr/include/python3.8 \ # -j6 - - name: temp sleep - run: sleep 15000s - shell: bash - name: Run catkin_make tests run: | export HOME=$GITHUB_WORKSPACE # Temporary fix for setup scripts diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py index d0b8c4155..c62f5732b 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py @@ -93,7 +93,7 @@ def on_hw_kill(self, alarm: Alarm) -> None: Update the classes' hw-kill alarm to the latest update. Args: - alarm (:class:`~ros_alarms.msg._Alarm.Alarm`): The alarm message to update with. + alarm (:class:`~ros_alarms_msgs.msg._Alarm.Alarm`): The alarm message to update with. """ self._last_hw_kill = alarm diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py index 5cfee2051..8d103cc78 100644 --- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py @@ -5,7 +5,7 @@ from mil_misc_tools import rospy_to_datetime from mil_usb_to_can.sub9 import AckPacket, CANDeviceHandle, NackPacket from ros_alarms import AlarmBroadcaster, AlarmListener -from ros_alarms.msg import Alarm +from ros_alarms_msgs.msg import Alarm from rospy.timer import TimerEvent from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse from subjugator_msgs.msg import Thrust @@ -113,7 +113,7 @@ def on_hw_kill(self, alarm: Alarm) -> None: Update the classes' hw-kill alarm to the latest update. Args: - alarm (:class:`~ros_alarms.msg._Alarm.Alarm`): The alarm message to update with. + alarm (:class:`~ros_alarms_msgs.msg._Alarm.Alarm`): The alarm message to update with. """ self._last_hw_kill = alarm From 2a3bd5b7f7191e26449ac899ca5a57b4eaca7638 Mon Sep 17 00:00:00 2001 From: cameron brown <52760912+cbrxyz@users.noreply.github.com> Date: Sun, 11 Jun 2023 14:48:28 -0700 Subject: [PATCH 33/38] Update package index before upgrading packages when deploying docs to master (#1053) --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c69ca062a..a510eb91b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -120,7 +120,7 @@ jobs: # We need to install rsync for GitHub Pages deploy action - name: Install rsync run: | - sudo apt-get upgrade && sudo apt-get update && sudo apt-get install -y rsync + sudo apt-get update && sudo apt-get upgrade && sudo apt-get install -y rsync # Publish the artifact to the GitHub Pages branch - name: Push docs to GitHub Pages From 29559541886a83d2198c7c2dac2176e7562d21c8 Mon Sep 17 00:00:00 2001 From: cameron brown <52760912+cbrxyz@users.noreply.github.com> Date: Sun, 25 Jun 2023 18:06:10 -0700 Subject: [PATCH 34/38] Fix mil_usb_to_can driver by updating sub_actuator_board (#1059) --- .../subjugator_launch/launch/can.launch | 4 +- .../subjugator_missions/sub_singleton.py | 2 +- .../sub8_actuator_board/CMakeLists.txt | 13 ++ .../drivers/sub8_actuator_board/package.xml | 15 ++ .../drivers/sub8_actuator_board/setup.py | 9 ++ .../sub8_actuator_board/srv/SetValve.srv | 6 + .../sub8_actuator_board/__init__.py | 3 + .../sub8_actuator_board/handle.py | 73 +++++++++ .../sub8_actuator_board/packets.py | 140 ++++++++++++++++++ .../sub8_actuator_board/simulation.py | 47 ++++++ .../mil_usb_to_can/sub8/__init__.py | 4 +- 11 files changed, 311 insertions(+), 5 deletions(-) create mode 100644 SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt create mode 100644 SubjuGator/drivers/sub8_actuator_board/package.xml create mode 100644 SubjuGator/drivers/sub8_actuator_board/setup.py create mode 100644 SubjuGator/drivers/sub8_actuator_board/srv/SetValve.srv create mode 100644 SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py create mode 100644 SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py create mode 100644 SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py create mode 100644 SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py diff --git a/SubjuGator/command/subjugator_launch/launch/can.launch b/SubjuGator/command/subjugator_launch/launch/can.launch index 0ddf124b0..813b94625 100644 --- a/SubjuGator/command/subjugator_launch/launch/can.launch +++ b/SubjuGator/command/subjugator_launch/launch/can.launch @@ -11,10 +11,10 @@ # List of python device handle classes device_handles: "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard - "83": sub_actuator_board.ActuatorBoard + "83": sub8_actuator_board.ActuatorBoard simulated_devices: "18": sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation - "83": sub_actuator_board.ActuatorBoardSimulation + "83": sub8_actuator_board.ActuatorBoardSimulation diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py b/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py index 360e38023..693fc3bd7 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py @@ -34,7 +34,7 @@ from std_srvs.srv import SetBool, SetBoolRequest, Trigger, TriggerRequest # from subjugator_msgs.srv import SetValve, SetValveRequest -from sub_actuator_board.srv import SetValve, SetValveRequest +from sub8_actuator_board.srv import SetValve, SetValveRequest from subjugator_msgs.srv import ( VisionRequest, VisionRequest2D, diff --git a/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt b/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt new file mode 100644 index 000000000..82e50480e --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.0.2) +project(sub8_actuator_board) +find_package(catkin REQUIRED COMPONENTS + message_generation + mil_usb_to_can +) +catkin_python_setup() +add_service_files( + FILES + SetValve.srv +) +generate_messages() +catkin_package(CATKIN_DEPENDS message_runtime) diff --git a/SubjuGator/drivers/sub8_actuator_board/package.xml b/SubjuGator/drivers/sub8_actuator_board/package.xml new file mode 100644 index 000000000..2aec91af8 --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/package.xml @@ -0,0 +1,15 @@ + + + sub8_actuator_board + 0.0.0 + The sub8_actuator_board package + Kevin Allen + MIT + Kevin Allen + catkin + message_generation + mil_usb_to_can + mil_usb_to_can + message_runtime + mil_usb_to_can + diff --git a/SubjuGator/drivers/sub8_actuator_board/setup.py b/SubjuGator/drivers/sub8_actuator_board/setup.py new file mode 100644 index 000000000..afe33361a --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/setup.py @@ -0,0 +1,9 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup + +# Fetch values from package.xml +setup_args = generate_distutils_setup(packages=["sub8_actuator_board"]) + +setup(**setup_args) diff --git a/SubjuGator/drivers/sub8_actuator_board/srv/SetValve.srv b/SubjuGator/drivers/sub8_actuator_board/srv/SetValve.srv new file mode 100644 index 000000000..837be5cd7 --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/srv/SetValve.srv @@ -0,0 +1,6 @@ +# Service call to open or close a pneumatic valve +uint8 actuator # ID of valve +bool opened # If True, valve will open; if False, valve will close +--- +bool success # False if there were any problems with the request. +string message # Description of error if success is false diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py new file mode 100644 index 000000000..97f6a1868 --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/__init__.py @@ -0,0 +1,3 @@ +from .handle import ActuatorBoard +from .packets import CommandMessage, FeedbackMessage, InvalidAddressException +from .simulation import ActuatorBoardSimulation diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py new file mode 100644 index 000000000..cee7efb30 --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +import rospy +from mil_usb_to_can.sub8 import CANDeviceHandle + +from sub8_actuator_board.srv import SetValve, SetValveRequest + +from .packets import SEND_ID, CommandMessage, FeedbackMessage, InvalidAddressException + +__author__ = "John Morin" + + +class ActuatorBoard(CANDeviceHandle): + """ + Device handle for the actuator board. Because this class implements a CAN device, + it inherits from the :class:`CANDeviceHandle` class. + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._service = rospy.Service("/set_valve", SetValve, self.set_valve) + + def set_valve(self, req: SetValveRequest) -> dict: + """ + Called when the ``/set_valve`` service is requested. Creates a message to + control the valve and sends it through the inherited device handle. + + Args: + req (SetValveRequest): Request to set the valve. + + Returns: + dict: List of information which is casted into a SetValveResponse. + """ + # Send board command to open or close specified valve + try: + message = CommandMessage.create_command_message( + address=req.actuator, + write=True, + on=req.opened, + ) + except InvalidAddressException as e: + return {"success": False, "message": str(e)} + self.send_data(bytes(message), can_id=SEND_ID) + rospy.loginfo( + "Set valve {} {}".format( + req.actuator, + "opened" if req.opened else "closed", + ), + ) + # Wait some time for board to process command + rospy.sleep(0.01) + # Request the status of the valve just commanded to ensure it worked + self.send_data( + bytes( + CommandMessage.create_command_message( + address=req.actuator, + write=False, + ), + ), + can_id=SEND_ID, + ) + return {"success": True} + + def on_data(self, data) -> None: + """ + Process data received from board. + """ + # Ensure packet contains correct identifier byte + if ord(data[0]) != FeedbackMessage.IDENTIFIER: + rospy.logwarn(f"Received packet with wrong identifier byte {ord(data[0])}") + return + # Parse message and (for now) just log it + message = FeedbackMessage.from_bytes(data) + rospy.loginfo(f"ActuatorBoard received {message}") diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py new file mode 100644 index 000000000..f287e09fa --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py @@ -0,0 +1,140 @@ +import struct + +from mil_usb_to_can.sub8 import ApplicationPacket + +# CAN ID for the channel from MOBO to actuator board +SEND_ID = 0x51 + + +class CommandMessage(ApplicationPacket): + """ + Represents a packet sent from the motherboard to the actuator board. Being a packet, + inherits from :class:`ApplicationPacket`. + + Attributes: + IDENTIFIER (int): The ASCII value of "A". + NUM_VALVES (int): The number of valves. Set to ``12`` currently. + """ + + IDENTIFIER = ord("A") + NUM_VALVES = 12 + + @property + def address(self) -> int: + """ + The integer address of this command. + + :rtype: int + """ + return struct.unpack("B", self.payload[0])[0] + + @property + def write(self) -> bool: + """ + If True, this message sets the valve open or closed. If False, this + message requests the status of a valve. + + :rtype: bool + """ + return struct.unpack("B", self.payload[1])[0] == 1 + + @property + def on(self) -> bool: + """ + If True, this message commanded the valve to be open. Else, the valve + is commanded to be closed. + + This parameter only has any effect if :attr:`~.write` is true. + + :rtype: bool + """ + return struct.unpack("B", self.payload[2])[0] == 1 + + @classmethod + def create_command_message(cls, address: int = 0, write: int = 0, on: int = 0): + """ + Create a new command message. + + Args: + address (int): Address of valve, ranging from 0-11. + write (int): If true, set valve open or close. If False, request + the status of the specified valve. + on (int): If true, command valve to open. If write if False, this has no effect + + Raises: + InvalidAddressException: The valve address is not valid, and is outside + of the 0-11 range. + """ + if address < 0 or address >= cls.NUM_VALVES: + raise InvalidAddressException(address) + write_byte = 1 if write else 0 + on_byte = 1 if on else 0 + payload = struct.pack("BBB", address, write_byte, on_byte) + return cls(cls.IDENTIFIER, payload) + + def __str__(self): + return "CommandMessage(address={}, write={}, on={})".format( + self.address, + self.write, + self.on, + ) + + +class InvalidAddressException(RuntimeError): + """ + Exception noting that the requested valve address is outside of the range available + on the sub. Inherits from :class:`RuntimeError`. + """ + + def __init__(self, address: int): + super().__init__( + "Attempted to command valve {}, but valid addresses are only [0,{}]".format( + address, + CommandMessage.NUM_VALVES - 1, + ), + ) + + +class FeedbackMessage(ApplicationPacket): + """ + Represents a message received from the actuator board sent to the motherboard + + Attributes: + IDENITIFER (int): The packet identifier. Set to the ASCII value of "A". + """ + + IDENTIFIER = ord("A") + + @property + def address(self) -> int: + """ + The address of valve referenced by this command. Should be between 0-11. + + :rtype: int + """ + return struct.unpack("B", self.payload[0])[0] + + @property + def on(self) -> bool: + """ + If True, valve is currently open. If False, valve is currently closed. + + :rtype: bool + """ + return struct.unpack("B", self.payload[1])[0] == 1 + + @classmethod + def create_feedback_message(cls, address: int = 0, on: bool = False): + """ + Create a feedback message. + + Args: + address (int): valve address this status references, integer 0-11 + on (bool): If True, valve is open + """ + on_byte = 1 if on else 0 + payload = struct.pack("BBB", address, on_byte, 0) + return cls(cls.IDENTIFIER, payload) + + def __str__(self): + return f"FeedbackMessage(address={self.address}, on={self.on})" diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py new file mode 100644 index 000000000..f776274d2 --- /dev/null +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py @@ -0,0 +1,47 @@ +#!/usr/bin/python3 +from mil_usb_to_can.sub8 import SimulatedCANDevice + +from .packets import SEND_ID, CommandMessage, FeedbackMessage + + +class ActuatorBoardSimulation(SimulatedCANDevice): + """ + Simulator for the communication of the actuator board. + + Attributes: + status (Dict[int, bool]): The status of the valves. The keys are each of the valve IDs, + and the values are the statues of whether the valves are open. + """ + + def __init__(self, *args, **kwargs): + # Tracks the status of the 12 valves + self.status = {i: False for i in range(12)} + super().__init__(*args, **kwargs) + + def on_data(self, data, can_id) -> None: + """ + Processes data received from motherboard / other devices. For each message received, + the class' status attribute is updated if the message is asking to write, otherwise + a feedback message is constructed and sent back. + """ + # If from wrong ID, ignore + if can_id != SEND_ID: + return + + # First byte should be the identifier char for the command message + assert ord(data[0]) == CommandMessage.IDENTIFIER + + # Parse message + message = CommandMessage.from_bytes(data) + + # If message is writing a valve, store this change in the internal dictionary + if message.write: + self.status[message.address] = message.on + + # If message is a status request, send motherboard the status of the requested valve + else: + response = FeedbackMessage.create_feedback_message( + address=message.address, + on=self.status[message.address], + ) + self.send_data(bytes(response)) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py index 668fb7018..30d817879 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py @@ -26,10 +26,10 @@ # List of python device handle classes (list of device id: python class implementation) device_handles: "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard - "83": sub_actuator_board.ActuatorBoard + "83": sub8_actuator_board.ActuatorBoard simulated_devices: "18": sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation - "83": sub_actuator_board.ActuatorBoardSimulation + "83": sub8_actuator_board.ActuatorBoardSimulation From 8dbecd9f44e8163dde95dff49bcc75150c8672c3 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sun, 23 Jul 2023 17:17:37 -0700 Subject: [PATCH 35/38] Bump aiohttp from 3.8.1 to 3.8.5 (#1064) Bumps [aiohttp](https://github.com/aio-libs/aiohttp) from 3.8.1 to 3.8.5. - [Release notes](https://github.com/aio-libs/aiohttp/releases) - [Changelog](https://github.com/aio-libs/aiohttp/blob/v3.8.5/CHANGES.rst) - [Commits](https://github.com/aio-libs/aiohttp/compare/v3.8.1...v3.8.5) --- updated-dependencies: - dependency-name: aiohttp dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index ae0ac3d2e..e7207f72d 100644 --- a/requirements.txt +++ b/requirements.txt @@ -22,7 +22,7 @@ PyYAML==6.0 urdf-parser-py==0.0.4 # Internet -aiohttp==3.8.1 +aiohttp==3.8.5 aiohttp-xmlrpc==1.5.0 # Documentation From 64a7f8a218ace88996d967fe0fdd99b3a2e12742 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sun, 23 Jul 2023 17:51:40 -0700 Subject: [PATCH 36/38] Bump scipy from 1.7.3 to 1.10.0 (#1061) Bumps [scipy](https://github.com/scipy/scipy) from 1.7.3 to 1.10.0. - [Release notes](https://github.com/scipy/scipy/releases) - [Commits](https://github.com/scipy/scipy/compare/v1.7.3...v1.10.0) --- updated-dependencies: - dependency-name: scipy dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: cameron brown <52760912+cbrxyz@users.noreply.github.com> --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index e7207f72d..90237fb22 100644 --- a/requirements.txt +++ b/requirements.txt @@ -11,7 +11,7 @@ pandas==1.4.1 numpy==1.22.3 # Machine Learning -scipy==1.7.3 +scipy==1.10.0 scikit-learn==1.1.1 # Computer Vision From 9bcfdcb2a83c01cae80829afb61cf16560fba6c5 Mon Sep 17 00:00:00 2001 From: Yovany Molina <80288489+yomole@users.noreply.github.com> Date: Sun, 23 Jul 2023 21:05:46 -0400 Subject: [PATCH 37/38] Testing changes (#1062) * Added changes from recent testings - Changed the thruster ID values to correspond to the current thruster connection setup. - Changed FRV thruster direction from -1Z to +1Z. - Changed PID values for the sub. Values are still not ideal and will be adjusted over the next few testing sessions. - Changed the identifier for the USB to CAN board's USB to serial converter. - Changed tcp address for the depth sensor, imu, and dvl connection scripts to their ip addresses instead of their DNS names (there is an issue with the DNS server in the network box, or in the lab, or both). - Made changes to the sub8.urdf.xacro for gazebo to avoid URDF inertia warning. - Added bag_debugging_controller.launch file which is a launch file to record bags for debugging the PID controller on the sub. * Add documentation for bag_debugging_controller.launch * pre-commit and CI fixes...probably * Added documentation about PID controller - Added documentation page about PID controller containing information about the bag_debugging_controller.launch file and some tips for how to tune the PID controller. - Reverted subjugator_launch/readme.md changes to reflect moving to a new documentation page. - Modified adaptive controller values to remove commented out kd values. * Modify URDF fix to fix /mission_server crash - Another URDF xacro fix has been applied to the URDF xacro file using these ROS forum posts as reference: https://answers.ros.org/question/393006/urdf-link-not-properly-fixed-to-world/, https://answers.ros.org/question/192817/error-msg-the-root-link_base-has-an-inertia-specified-in-the-urdf-but-kdl/. NOTE: This is not a complete fix yet. I'm getting a tf republisher error that 'base_link' and 'map' do not have a connection with TF having 2+ unconnected trees. Additionally, gazebogui does not work since the /gzclient/set_physics_properties service is not advertised (even though /gazebo/set_physics_properties is). * Add support for simulation vs physical PID gains --------- Co-authored-by: Cameron Brown Co-authored-by: cameron brown <52760912+cbrxyz@users.noreply.github.com> --- .../config/adaptive_controller.yaml | 10 +++---- .../config/adaptive_controller_gazebo.yaml | 6 +++++ .../launch/bag_debugging_controller.launch | 7 +++++ .../subjugator_launch/launch/can.launch | 2 +- .../subjugator_launch/launch/sub8.launch | 4 ++- .../subsystems/adaptive_controller.launch | 8 +++--- .../command/subjugator_launch/package.xml | 2 ++ .../subjugator_launch/scripts/depth_conn | 2 +- .../subjugator_launch/scripts/dvl_conn | 2 +- .../subjugator_launch/scripts/imu_conn | 2 +- .../config/thruster_layout.yaml | 18 ++++++------- .../subjugator_gazebo/urdf/sub8.urdf.xacro | 7 +++++ docs/subjugator/index.rst | 1 + docs/subjugator/pid.rst | 26 +++++++++++++++++++ 14 files changed, 75 insertions(+), 22 deletions(-) create mode 100644 SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml create mode 100644 SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch create mode 100644 docs/subjugator/pid.rst diff --git a/SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml b/SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml index c6406c44d..aefd3cb36 100644 --- a/SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml +++ b/SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml @@ -1,6 +1,6 @@ --- -kp: 1000, 1000, 1000, 5000, 5000, 5000 -kd: 150, 150, 150, 50, 100, 25 -kg: 5,5,5,5,5,5 -ki: 5,5,5,5,10,5 -use_learned: false +kp: 120, 150, 200, 100, 50, 50 +kd: 25, 25, 25, 25, 25, 25 +kg: 2.5,2.5,2.5,2.5,2.5,2.5 +ki: 2.5,2.5,2.5,2.5,2.5,2.5 +use_learned: true diff --git a/SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml b/SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml new file mode 100644 index 000000000..c6406c44d --- /dev/null +++ b/SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml @@ -0,0 +1,6 @@ +--- +kp: 1000, 1000, 1000, 5000, 5000, 5000 +kd: 150, 150, 150, 50, 100, 25 +kg: 5,5,5,5,5,5 +ki: 5,5,5,5,10,5 +use_learned: false diff --git a/SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch b/SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch new file mode 100644 index 000000000..4c6024669 --- /dev/null +++ b/SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/SubjuGator/command/subjugator_launch/launch/can.launch b/SubjuGator/command/subjugator_launch/launch/can.launch index 813b94625..b46fbd8ba 100644 --- a/SubjuGator/command/subjugator_launch/launch/can.launch +++ b/SubjuGator/command/subjugator_launch/launch/can.launch @@ -5,7 +5,7 @@ # Path of serial device - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0 + port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0 # Baudrate of device, should leave as is baudrate: 115200 # List of python device handle classes diff --git a/SubjuGator/command/subjugator_launch/launch/sub8.launch b/SubjuGator/command/subjugator_launch/launch/sub8.launch index 178efc6bb..247fe1e62 100644 --- a/SubjuGator/command/subjugator_launch/launch/sub8.launch +++ b/SubjuGator/command/subjugator_launch/launch/sub8.launch @@ -39,7 +39,9 @@ - + + + diff --git a/SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch b/SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch index 17bd1d3ac..a758c4278 100644 --- a/SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch +++ b/SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch @@ -1,8 +1,10 @@ - + + - - + + + diff --git a/SubjuGator/command/subjugator_launch/package.xml b/SubjuGator/command/subjugator_launch/package.xml index 6a6beeeb6..e3f6ef4ba 100644 --- a/SubjuGator/command/subjugator_launch/package.xml +++ b/SubjuGator/command/subjugator_launch/package.xml @@ -5,8 +5,10 @@ The subjugator_launch package Jacob Panikulam MIT + rosbag roslaunch roslaunch + rosbag robot_state_publisher nodelet c3_trajectory_generator diff --git a/SubjuGator/command/subjugator_launch/scripts/depth_conn b/SubjuGator/command/subjugator_launch/scripts/depth_conn index 13bf8a480..c63922eb9 100755 --- a/SubjuGator/command/subjugator_launch/scripts/depth_conn +++ b/SubjuGator/command/subjugator_launch/scripts/depth_conn @@ -1,2 +1,2 @@ #!/bin/sh -exec socat -d -d pty,link=/tmp/depth,raw,echo=0 tcp:mil-sub-gumstix.ad.mil.ufl.edu:33056 +exec socat -d -d pty,link=/tmp/depth,raw,echo=0 tcp:192.168.37.61:33056 diff --git a/SubjuGator/command/subjugator_launch/scripts/dvl_conn b/SubjuGator/command/subjugator_launch/scripts/dvl_conn index f0be15464..745a45097 100755 --- a/SubjuGator/command/subjugator_launch/scripts/dvl_conn +++ b/SubjuGator/command/subjugator_launch/scripts/dvl_conn @@ -1,2 +1,2 @@ #!/bin/sh -exec socat -d -d pty,link=/tmp/dvl,raw,echo=0 tcp:mil-sub-gumstix.ad.mil.ufl.edu:349 +exec socat -d -d pty,link=/tmp/dvl,raw,echo=0 tcp:192.168.37.61:349 diff --git a/SubjuGator/command/subjugator_launch/scripts/imu_conn b/SubjuGator/command/subjugator_launch/scripts/imu_conn index 59275c393..bc93e1eb1 100755 --- a/SubjuGator/command/subjugator_launch/scripts/imu_conn +++ b/SubjuGator/command/subjugator_launch/scripts/imu_conn @@ -1,2 +1,2 @@ #!/bin/sh -exec socat -d -d pipe:/tmp/imu,wronly=1 tcp:mil-sub-gumstix.ad.mil.ufl.edu:1382 +exec socat -d -d pipe:/tmp/imu,wronly=1 tcp:192.168.37.61:1382 diff --git a/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml b/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml index 2d3de8ac9..48992559a 100644 --- a/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml +++ b/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml @@ -30,7 +30,7 @@ thruster_ports: # mapping from thrust in Newtons to effort (e = t[0]x^4 + t[1]x^3 + t[2]x^2 + t[3]x) thrusters: FLH: { - node_id: 3, + node_id: 4, position: [0.2678, 0.2795, 0.0], direction: [0.866, -0.5, 0.0], thrust_bounds: [-81.85, 99.7], @@ -42,7 +42,7 @@ thrusters: } } FLV: { - node_id: 1, + node_id: 5, position: [0.1583, 0.169, 0.0142], direction: [0.0, 0.0, 1.0], thrust_bounds: [-81.85, 99.7], @@ -54,7 +54,7 @@ thrusters: } } FRH: { - node_id: 2, + node_id: 0, position: [0.2678, -0.2795, 0.0], direction: [0.866, 0.5, 0.0], thrust_bounds: [-81.85, 99.7], @@ -66,9 +66,9 @@ thrusters: } } FRV: { - node_id: 0, + node_id: 1, position: [0.1583, -0.169, 0.0142], - direction: [0.0, 0.0, -1.0], + direction: [0.0, 0.0, 1.0], thrust_bounds: [-81.85, 99.7], calib: { # yamllint disable-line rule:line-length @@ -78,7 +78,7 @@ thrusters: } } BLH: { - node_id: 4, + node_id: 3, position: [-0.2678, 0.2795, 0.0], direction: [0.866, 0.5, 0.0], thrust_bounds: [-81.85, 99.7], @@ -90,7 +90,7 @@ thrusters: } } BLV: { - node_id: 5, + node_id: 6, position: [-0.1583, 0.169, 0.0142], direction: [0.0, 0.0, 1.0], thrust_bounds: [-81.85, 99.7], @@ -102,7 +102,7 @@ thrusters: } } BRH: { - node_id: 6, + node_id: 7, position: [-0.2678, -0.2795, 0.0], direction: [0.866, -0.5, 0.0], thrust_bounds: [-81.85, 99.7], @@ -114,7 +114,7 @@ thrusters: } } BRV: { - node_id: 7, + node_id: 2, position: [-0.1583, -0.169, 0.0142], direction: [0.0, 0.0, 1.0], thrust_bounds: [-81.85, 99.7], diff --git a/SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro b/SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro index 3f4517335..9290884dd 100644 --- a/SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro +++ b/SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro @@ -11,6 +11,8 @@ + + @@ -31,6 +33,11 @@ + + + + + diff --git a/docs/subjugator/index.rst b/docs/subjugator/index.rst index c34f4e7fd..28566c2ac 100644 --- a/docs/subjugator/index.rst +++ b/docs/subjugator/index.rst @@ -15,6 +15,7 @@ SubjuGator 8 Simulating Enabling Cameras + PID Controller electrical diff --git a/docs/subjugator/pid.rst b/docs/subjugator/pid.rst new file mode 100644 index 000000000..c2feeae29 --- /dev/null +++ b/docs/subjugator/pid.rst @@ -0,0 +1,26 @@ +PID Controller +-------------- + +SubjuGator 8 uses 6 PID controllers for its 6 degrees of freedom (x, y, z, roll, pitch, yaw). The gain for these PID controllers can be adjusted in ``adaptive_controller.yaml`` found in the ``subjugator_launch/config`` directory. Since the weight of the sub shifts with each component modified, the PID controller values have to be adjusted from time to time. There are two approaches to adjust PID values in the water: + +1. Have someone with experience in tuning PID controllers swim with the sub and use the sub's response to movement commands to adjust the gains. +2. Eliminate the error in each axis by adjusting the gains and evaluating the RMS error in each axis after sending the same movement commands. + +PID Controller Tuning Tips +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +* The learning gains (``ki`` and ``kd``) provide very little input to the wrenches applied to the sub, so it is better to treat it as a PD controller to start with. You can disable the learning gains using ``use_learned: false``. +* While you are tuning the PID values, keep a history of the values you have tried for further analysis during and after testing. You can use ``scp`` for this, but it may just be easier to take a screenshot or a photo. You can also take videos of the sub from the side of the pool to reference later. + +PID Debugging Data Collection +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: bash + + $ roslaunch subjugator_launch bag_debugging_controller.launch prefix_name:="my_prefix" + +This launch file records a bag file containing adaptive controller ``pose_error``, ``twist_error``, ``adaptation``, ``dist``, and ``drag`` data along with ``wrench``, ``trajectory``, ``odom``, ``imu/data_raw``, and ``dvl/range`` data. This data is useful in determining if the PID values are getting better or worse using simulations. The resulting bag file will be located in ``gnc/subjugator_controller/debug_bags/``. This launch file was written by Andres Pulido. + +.. warning:: + + This launch file needs the sub launch file (``sub8.launch``) running in order to function properly. From bb6d831273faad6d46100f90b33a82f7a8bd27df Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 7 Aug 2023 19:45:20 +0000 Subject: [PATCH 38/38] Update pre-commit hooks to latest versions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/psf/black: 23.3.0 → 23.7.0](https://github.com/psf/black/compare/23.3.0...23.7.0) - [github.com/pre-commit/mirrors-clang-format: v16.0.4 → v16.0.6](https://github.com/pre-commit/mirrors-clang-format/compare/v16.0.4...v16.0.6) - [github.com/PyCQA/autoflake: v2.1.1 → v2.2.0](https://github.com/PyCQA/autoflake/compare/v2.1.1...v2.2.0) - [github.com/scop/pre-commit-shfmt: v3.6.0-2 → v3.7.0-1](https://github.com/scop/pre-commit-shfmt/compare/v3.6.0-2...v3.7.0-1) - https://github.com/charliermarsh/ruff-pre-commit → https://github.com/astral-sh/ruff-pre-commit - [github.com/astral-sh/ruff-pre-commit: v0.0.270 → v0.0.282](https://github.com/astral-sh/ruff-pre-commit/compare/v0.0.270...v0.0.282) - [github.com/codespell-project/codespell: v2.2.4 → v2.2.5](https://github.com/codespell-project/codespell/compare/v2.2.4...v2.2.5) --- .pre-commit-config.yaml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d2f0e2009..cb5d978ed 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,15 +15,15 @@ repos: hooks: - id: yamllint - repo: https://github.com/psf/black - rev: 23.3.0 + rev: 23.7.0 hooks: - id: black - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v16.0.4 + rev: v16.0.6 hooks: - id: clang-format - repo: https://github.com/PyCQA/autoflake - rev: v2.1.1 + rev: v2.2.0 hooks: - id: autoflake args: [--remove-all-unused-imports, --ignore-init-module-imports] @@ -34,18 +34,18 @@ repos: exclude: ^docker|deprecated|NaviGator/simulation/VRX args: [--severity=warning, --exclude=SC1090] - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.6.0-2 + rev: v3.7.0-1 hooks: - id: shfmt exclude: ^docker|deprecated|NaviGator/simulation/VRX - - repo: https://github.com/charliermarsh/ruff-pre-commit + - repo: https://github.com/astral-sh/ruff-pre-commit # Ruff version. - rev: 'v0.0.270' + rev: 'v0.0.282' hooks: - id: ruff args: [--fix, --exit-non-zero-on-fix] - repo: https://github.com/codespell-project/codespell - rev: v2.2.4 + rev: v2.2.5 hooks: - id: codespell args: