mil_usb_to_can - USB to CAN Communication

mil_usb_to_can.sub8 - SubjuGator 8

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 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 USBtoCANBoard class, which handles connecting to the board. This information can be provided through local ROS parameters:

<launch>
  <node pkg="mil_usb_to_can" type="sub8_driver.py" name="usb_to_can_driver">
    <rosparam command="delete" />
    <rosparam>
    # 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
    </rosparam>
  </node>
</launch>

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 CANDeviceHandle, whereas the simulated drivers implement from SimulatedCANDevice. More information on writing device handles can be found in the documentation of each type of class.

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.

USBtoCAN Packet Format

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.

Command Packet (CommandPacket)

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.

Receiving Packet (ReceivePacket)

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

Exception List

class mil_usb_to_can.sub8.ApplicationPacketWrongIdentifierException(received: int, expected: int)[source]

Exception thrown when the identifier for a MIL application level CAN packet had a different identifier from what was expected.

Inherits from Exception.

received

A value representing what received found.

Type

int

expected

What the found value should have been.

Type

int

class mil_usb_to_can.sub8.USB2CANException[source]

Base class for exception in USB2CAN board functionality. Inherits from Exception.

class mil_usb_to_can.sub8.ChecksumException(calculated, expected)[source]

Exception thrown when the checksum between motherboard and CANtoUSB board is invalid. Inherits from USB2CANException.

class mil_usb_to_can.sub8.PayloadTooLargeException(length)[source]

Exception thrown when payload of data sent/received from CAN2USB is too large. Inherits from USB2CANException.

class mil_usb_to_can.sub8.InvalidFlagException(description, expected, was)[source]

Exception thrown when a constant flag in the CAN2USB protocol is invalid. Inherits from USB2CANException.

class mil_usb_to_can.sub8.InvalidStartFlagException(was: int)[source]

Exception thrown when the SOF flag is invalid. Inherits from InvalidFlagException.

class mil_usb_to_can.sub8.InvalidEndFlagException(was: int)[source]

Exception thrown when the EOF flag is invalid. Inherits from InvalidFlagException.

ApplicationPacket

class mil_usb_to_can.sub8.ApplicationPacket(identifier: int, payload: bytes)[source]

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.

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

Assembles the packet into a form suitable for sending through a data stream. Packs identifier and payload into a single bytes object.

identifier

The identifier for the packet. Allowed range is between 0 and 255.

Type

int

payload

The payload of bytes to be sent in the packet.

Type

bytes

classmethod from_bytes(data: bytes, expected_identifier: int | None = None) T[source]

Unpacks a series of packed bytes representing an application packet using 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.

Parameters
  • 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

The data represented as an application packet.

Return type

ApplicationPacket

USBtoCANBoard

Attributes
Methods
class mil_usb_to_can.sub8.USBtoCANBoard(port: str, baud: int = 9600, simulated: bool = False, **kwargs)[source]

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.

lock

The thread lock.

Type

threading.Lock

ser

The serial connection.

Type

Union[SimulatedUSBtoCAN, serial.Serial]

Parameters
  • 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.

read_packet() Optional[ReceivePacket][source]

Read a packet from the board, if available. Returns a ReceivePacket instance if one was successfully read, or None if the in buffer is empty.

Returns

The packet, if found, otherwise None.

Return type

Optional[ReceivePacket]

send_data(data: bytes, can_id: int = 0) None[source]

Sends data to a CAN device using the thread lock. Writes using the write() method of the ser attribute.

Parameters
  • 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.

CANDeviceHandle

Methods
class mil_usb_to_can.sub8.CANDeviceHandle(driver: USBtoCANBoard, device_id: int)[source]

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.

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())
Parameters
  • 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.

on_data(data: bytes)[source]

Called when data is received from the device this handle is registered for.

Parameters

data (bytes) – The data received.

send_data(data: bytes, can_id: int = 0)[source]

Sends data to the device.

Parameters
  • 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.

USBtoCANDriver

class mil_usb_to_can.sub8.USBtoCANDriver[source]

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.

board

The board the driver is implementing.

Type

USBtoCANBoard

handles

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 CANDeviceHandle. Upon initialization, each class is constructed after being parsed from dynamic reconfigure.

Type

dict[int, CANDeviceHandle]

timer

The timer controlling when buffers are processed.

Type

rospy.Timer

static parse_module_dictionary(d: Dict[str, Any]) Generator[Tuple[int, Any], None, None][source]

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.

process_in_buffer(*args) None[source]

Read all available packets in the board’s in-buffer.

read_packet() bool[source]

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

The success in reading a packet.

Return type

bool

send_data(*args, **kwargs) Optional[Exception][source]

Sends data using the USBtoCANBoard.send_data() method.

Returns

If data was sent successfully, nothing is returned. Otherwise, the exception that was raised in sending is returned.

Return type

Optional[Exception]

SimulatedCANDevice

Methods
class mil_usb_to_can.sub8.SimulatedCANDevice(sim_board: SimulatedUSBtoCAN, can_id: int)[source]

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.

on_data(data: bytes, can_id: int)[source]

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.

Parameters

data (bytes) – The data payload as a string/bytes object.

send_data(data: bytes)[source]

Send data onto the bus, delivering it to other simulated devices and to the driver node.

SimulatedUSBtoCAN

Methods
class mil_usb_to_can.sub8.SimulatedUSBtoCAN(devices: dict[int, type[SimulatedCANDevice]] | None = None, can_id=- 1)[source]

Simulates the USB to CAN board. Is supplied with a dictionary of simulated CAN devices to simulate the behavior of the whole CAN network.

Parameters
  • devices (Dict[int, Any]) – Dictionary containing CAN IDs and their associated simulated classes inheriting from SimulatedCANDevice.

  • can_id (int) – ID of the CAN2USB device. Defaults to -1.

send_to_bus(can_id: int, data: bytes, from_mobo: bool = False)[source]

Sends data onto the simulated bus from a simulated device.

Parameters
  • 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.

write(data: bytes) int[source]

Parse incoming data as a command packet from the motherboard.

Packet

class mil_usb_to_can.sub8.Packet(payload: bytes)[source]

Represents a packet to or from the CAN to USB board. This class is inherited by ReceivePacket (for receiving data from the bus) and CommandPacket (for sending commands). Those child classes should be used over this class whenever possible.

bytes(x)

Assembles the packet into a form suitable for sending through a data stream. For this base packet class, SOF, payload, and EOF are assembled into one byte string.

payload

The payload stored in the packet.

Type

bytes

SOF

Flag used to mark the beginning of each packet. Equal to 0xC0.

Type

int

EOF

Flag used to mark the beginning of each packet. Equal to 0xC1.

Type

int

classmethod from_bytes(data: bytes) T | None[source]

Parses a packet from a packed bytes string into a Packet instance.

Parameters

data (bytes) – The packed data to construct the Packet instance from.

Returns

The packet instance. None is returned if the packet contains an empty payload.

Return type

Optional[Packet]

classmethod read_packet(stream: serial.Serial | SimulatedUSBtoCAN) T | None[source]

Read a packet with a known size from a serial device

Parameters

stream (Union[serial.Serial, SimulatedUSBtoCAN]) – A instance of a serial device to read from.

Raises
Returns

The read packet. If a packet was partially transmitted (ie, starting with a character other than SOF or ending with a character other than EOF), then None is returned.

Return type

Optional[Packet]

ReceivePacket

class mil_usb_to_can.sub8.ReceivePacket(payload: bytes)[source]

Packet used to request data from the USB to CAN board.

payload

The payload stored in the packet.

Type

bytes

SOF

Flag used to mark the beginning of each packet. Equal to 0xC0.

Type

int

EOF

Flag used to mark the beginning of each packet. Equal to 0xC1.

Type

int

classmethod create_receive_packet(device_id: int, payload: bytes) ReceivePacket[source]

Creates a command packet to request data from a CAN device.

Parameters
  • device_id (int) – The CAN device ID to request data from.

  • payload (bytes) – The data to send in the packet.

Returns

The packet to request from the CAN device.

Return type

ReceivePacket

property data: bytes

The data inside the packet.

property device: int

The device ID associated with the packet.

classmethod from_bytes(data: bytes) ReceivePacket[source]

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

The packet constructed from the packed bytes.

Return type

ReceivePacket

property length

The length of the data to receive.

CommandPacket

class mil_usb_to_can.sub8.CommandPacket(payload: bytes)[source]

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.

bytes(x)

Assembles the packet into a form suitable for sending through a data stream.

payload

The payload stored in the packet.

Type

bytes

SOF

Flag used to mark the beginning of each packet. Equal to 0xC0.

Type

int

EOF

Flag used to mark the beginning of each packet. Equal to 0xC1.

Type

int

classmethod create_request_packet(filter_id: int, receive_length: int) CommandPacket[source]

Creates a command packet to request data from a CAN device.

Parameters
  • filter_id (int) – The CAN device ID to request data from.

  • receive_length (int) – The number of bytes to request.

Returns

The command packet responsible for requesting data from a CAN device.

Return type

CommandPacket

classmethod create_send_packet(data: bytes, can_id: int = 0) CommandPacket[source]

Creates a command packet to send data to the CAN bus from the motherboard.

Parameters
  • 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

The packet responsible for sending information to the CAN bus from the motherboard.

Return type

CommandPacket

property data: bytes

Returns: bytes: The data to be sent.

property filter_id: int

An integer representing the CAN device ID specified by this packet.

Returns

int

property is_receive: bool

True if this CommandPacket is requesting data.

Returns

bool

property length: int

The number of bytes of data sent or requested.

Returns

int

property length_byte: int

The first header byte which encodes the length and the receive flag.

Returns

int

mil_usb_to_can.sub9 - SubjuGator 9

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.

USBtoCAN Packet Format

Name

Length

Description

Sync character 1 (0x37)

1 byte

First sync character indicating the start of packets.

Sync character 2 (0x01)

1 byte

Second sync character indicating the start of packets.

Class ID

1 byte

Message class. Determines the family of messages the packet belongs to.

Subclass ID

1 byte

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

First byte of Fletcher’s checksum.

Checksum B

1 byte

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.

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 struct module. For more information, see the Python documentation on the list of format characters, and their corresponding byte length.

Message ID

Subclass ID

Payload Format

Class

0x00 (Meta)

0x00

Empty

mil_usb_to_can.sub9.NackPacket

0x01

Empty

mil_usb_to_can.sub9.AckPacket

0x01 (Sub8 Thrust/ Kill)

0x00

Empty

sub8_thrust_and_kill_board.HeartbeatPacket

0x01

Bf

sub8_thrust_and_kill_board.ThrustSetPacket

0x02

B

sub8_thrust_and_kill_board.KillSetPacket

0x03

B

sub8_thrust_and_kill_board.KillReceivePacket

0x02 (Sub9 Thrust/ Kill)

0x00

Empty

sub9_thrust_and_kill_board.HeartbeatSetPacket

0x01

Empty

sub9_thrust_and_kill_board.HeartbeatReceivePacket

0x02

Bf

sub9_thrust_and_kill_board.ThrustSetPacket

0x03

B

sub9_thrust_and_kill_board.KillSetPacket

0x04

B

sub9_thrust_and_kill_board.KillReceivePacket

0x03 (Battery Monitor)

0x00

Empty

sub8_battery_monitor_board.BatteryPollRequestPacket

0x01

ffff

sub8_battery_monitor_board.BatteryPollResponsePacket

0x04 (Actuator Board)

0x00

BB

sub_actuator_board.ActuatorSetPacket

0x01

Empty

sub_actuator_board.ActuatorPollRequestPacket

0x02

B

sub_actuator_board.ActuatorPollResponsePacket

0x05 (System Status)

0x00

Empty

sub9_system_status_board.SetLedRequestPacket

Exceptions

Exception Hierarchy

Exception List

class mil_usb_to_can.sub9.ChecksumException(packet: type[Packet], received: tuple[int, int], expected: tuple[int, int])[source]

An invalid checksum appeared.

packet

The packet with the invalid checksum.

Type

Type[Packet]

received

The received Fletcher’s checksum.

Type

Tuple[int, int]

expected

The expected Fletcher’s checksum.

Type

Tuple[int, int]

CANDeviceHandle

Methods
class mil_usb_to_can.sub9.CANDeviceHandle(driver: USBtoCANDriver)[source]

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.

Parameters
  • 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.

on_data(data: Packet)[source]

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.

Parameters

packet (Packet) – The incoming packet.

send_data(data: Packet)[source]

Sends a packet over the serial connection.

Parameters

data (Packet) – The packet to send.

SimulatedCANDeviceHandle

Attributes
Methods
class mil_usb_to_can.sub9.SimulatedCANDeviceHandle(sim_board: SimulatedUSBtoCANStream, inbound_packets: list[type[Packet]])[source]

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.

inbound_packets

The types of packets listened to by this device. Packets of this type will be routed to the on_data() method of the device handle.

Type

type[Packet]

on_data(packet: Packet)[source]

Called when an relevant incoming packet is received over the serial connection. Relevant packets are those listed in inbound_packets.

Partial data (ie, incomplete packets) are not sent through this event.

Parameters

packet (Packet) – The incoming packet.

send_data(data: bytes)[source]

Sends data over the serial connection.

Packet

Methods
class mil_usb_to_can.sub9.Packet[source]

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 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. Note that all subclasses must be decorated with dataclasses.dataclass().

If any class members are annotated with a subclass of 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.

from dataclasses import dataclass

@dataclass
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
bytes(x)

Returns a bytes object representing the data of the packet in the specified packet format.

Parameters
  • 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.

classmethod from_bytes(packed: bytes) PacketSelf[source]

Constructs a packet from a packed packet in a bytes object. If a packet is found with the corresponding message and subclass ID, then an instance (or subclass) of that packet class will be returned.

Raises
Returns

An instance of the appropriate packet subclass.

NackPacket

class mil_usb_to_can.sub9.NackPacket[source]

Common not-acknowledged packet. Should only be found in response operations.

AckPacket

class mil_usb_to_can.sub9.AckPacket[source]

Common acknowledgment packet. Should only be found in response operations.