diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index bdccf44..a0a8a7e 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -6,6 +6,7 @@ from .convert import string_to_viccan # CAN import can import can.interfaces.socketcan +import struct # Serial import serial @@ -177,6 +178,235 @@ class CANConnector(Connector): def __init__(self, logger: RcutilsLogger): super().__init__(logger) + self.can_channel: str | None = None + self.can_bus: can.BusABC | None = None + + if self.can_channel is None: + avail = can.interfaces.socketcan.SocketcanBus._detect_available_configs() + + if len(avail) == 0: + raise NoValidDeviceException("no CAN interfaces found") + if (l := len(avail)) > 1: + channels = ", ".join(str(b.get("channel")) for b in avail) + raise MultipleValidDevicesException( + f"too many ({l}) CAN interfaces found: [{channels}]" + ) + + bus = avail[0] + self.can_channel = str(bus.get("channel")) + self.logger.info(f"found CAN interface '{self.can_channel}'") + + try: + self.can_bus = can.Bus( + interface="socketcan", + channel=self.can_channel, + bitrate=1_000_000, + ) + except can.CanError as e: + raise NoWorkingDeviceException( + f"could not open CAN channel '{self.can_channel}': {e}" + ) + + if self.can_channel and self.can_channel.startswith("v"): + self.logger.warn("using virtual CAN interface; this is likely vcan*") + + def read(self) -> VicCAN | None: + if not self.can_bus: + raise DeviceClosedException("CAN bus not initialized") + + try: + message = self.can_bus.recv(timeout=0.0) + except can.CanError as e: + self.logger.error(f"CAN error while receiving: {e}") + raise DeviceClosedException("CAN bus closed unexpectedly") + + if message is None: + return None + + arbitration_id = message.arbitration_id & 0x7FF + data_bytes = bytes(message.data) + + mcu_key = (arbitration_id >> 8) & 0b111 + data_type_key = (arbitration_id >> 6) & 0b11 + command = arbitration_id & 0x3F + + key_to_mcu: dict[int, str] = { + 1: "broadcast", + 2: "core", + 3: "arm", + 4: "digit", + 5: "faerie", + 6: "citadel", + } + + mcu_name = key_to_mcu.get(mcu_key) + if mcu_name is None: + self.logger.warn( + f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}" + ) + return None + + data: list[float] = [] + + try: + if data_type_key == 3: + data = [] + elif data_type_key == 0: + if len(data_bytes) < 8: + self.logger.warn( + f"received double payload with insufficient length {len(data_bytes)}; dropping frame" + ) + return None + (value,) = struct.unpack(" 0x3F: + self.logger.warn( + f"command_id 0x{command:X} exceeds 6-bit range; truncating to lower 6 bits" + ) + command &= 0x3F + + arbitration_id = ((mcu_key & 0b111) << 8) | ((data_type_key & 0b11) << 6) | (command & 0x3F) + + # Map VicCAN.data (floats) to up to 8 CAN data bytes. + raw_bytes: list[int] = [] + for value in msg.data: + try: + b = int(round(value)) + except (TypeError, ValueError): + self.logger.error( + f"non-numeric VicCAN data value: {value}; dropping message" + ) + return + + if b < 0 or b > 255: + self.logger.warn( + f"VicCAN data value {value} out of byte range; clamping into [0, 255]" + ) + b = max(0, min(255, b)) + + raw_bytes.append(b) + + if len(raw_bytes) > 8: + self.logger.warn( + f"VicCAN data too long for single CAN frame ({len(raw_bytes)} > 8); truncating" + ) + raw_bytes = raw_bytes[:8] + + try: + can_message = can.Message( + arbitration_id=arbitration_id, + data=raw_bytes, + is_extended_id=False, + ) + except Exception as e: + self.logger.error(f"failed to construct CAN message: {e}") + return + + try: + self.can_bus.send(can_message) + self.logger.debug( + f"sent CAN frame id=0x{can_message.arbitration_id:X}, " + f"data={list(can_message.data)}" + ) + except can.CanError as e: + self.logger.error(f"CAN error while sending: {e}") + raise DeviceClosedException("CAN bus closed unexpectedly") + + def cleanup(self): + try: + if self.can_bus is not None: + self.logger.info("shutting down CAN bus") + self.can_bus.shutdown() + except Exception as e: + self.logger.error(e) + class MockConnector(Connector): def __init__(self, logger: RcutilsLogger): diff --git a/src/anchor_pkg/launch/rover.launch.py b/src/anchor_pkg/launch/rover.launch.py index 2a949ba..d2b16a8 100644 --- a/src/anchor_pkg/launch/rover.launch.py +++ b/src/anchor_pkg/launch/rover.launch.py @@ -19,6 +19,7 @@ dont_write_bytecode = True def launch_setup(context, *args, **kwargs): # Retrieve the resolved value of the launch argument 'mode' mode = LaunchConfiguration("mode").perform(context) + connector = LaunchConfiguration("connector").perform(context) nodes = [] if mode == "anchor": @@ -70,7 +71,7 @@ def launch_setup(context, *args, **kwargs): executable="anchor", # change as needed name="anchor", output="both", - parameters=[{"launch_mode": mode}], + parameters=[{"launch_mode": mode, "connector": connector}], on_exit=Shutdown(), ) )