From 18fce2c19bbce0a2d71677532754c799bf071a6a Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Sat, 14 Feb 2026 21:39:32 -0600 Subject: [PATCH 01/25] worth a shot to see if it works --- src/anchor_pkg/anchor_pkg/anchor_node.py | 283 ++--------------------- src/anchor_pkg/anchor_pkg/connector.py | 194 ++++++++++++++++ 2 files changed, 214 insertions(+), 263 deletions(-) create mode 100644 src/anchor_pkg/anchor_pkg/connector.py diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index a2374fa..59198f6 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -1,18 +1,13 @@ import rclpy from rclpy.node import Node from rclpy.executors import ExternalShutdownException -from std_srvs.srv import Empty import signal -import time import atexit -import serial -import serial.tools.list_ports -import os +from connector import Connector, SerialConnector, CANConnector import sys import threading -import glob from std_msgs.msg import String, Header from astra_msgs.msg import VicCAN @@ -46,120 +41,13 @@ class Anchor(Node): - Publish raw strings to this topic to send directly to the MCU for debugging """ + connector: Connector + def __init__(self): # Initalize node with name super().__init__("anchor_node") # previously 'serial_publisher' - self.serial_port: str | None = None # e.g., "/dev/ttyUSB0" - - # Serial port override - if port_override := os.getenv("PORT_OVERRIDE"): - self.serial_port = port_override - - ################################################## - # Serial MCU Discovery - - # If there was not a port override, look for a MCU over USB for Serial. - if self.serial_port is None: - comports = serial.tools.list_ports.comports() - real_ports = list( - filter( - lambda p: p.vid is not None - and p.pid is not None - and p.device is not None, - comports, - ) - ) - recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports)) - - if len(recog_ports) == 1: # Found singular recognized MCU - found_port = recog_ports[0] - self.get_logger().info( - f"Selecting MCU '{found_port.description}' at {found_port.device}." - ) - self.serial_port = found_port.device # String, location of device file; e.g., '/dev/ttyACM0' - elif len(recog_ports) > 1: # Found multiple recognized MCUs - # Kinda jank log message - self.get_logger().error( - f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}" - ) - # Don't set self.serial_port; later if-statement will exit() - elif ( - len(recog_ports) == 0 and len(real_ports) > 0 - ): # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU - self.get_logger().error( - f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}." - ) - # Don't set self.serial_port; later if-statement will exit() - else: # Found jack shit - self.get_logger().error("No valid Serial ports specified or found.") - # Don't set self.serial_port; later if-statement will exit() - - # We still don't have a serial port; fall back to legacy discovery (Areeb's code) - # Loop through all serial devices on the computer to check for the MCU - if self.serial_port is None: - self.get_logger().warning("Falling back to legacy MCU discovery...") - ports = Anchor.list_serial_ports() - for _ in range(4): - if self.serial_port is not None: - break - for port in ports: - try: - # connect and send a ping command - ser = serial.Serial(port, 115200, timeout=1) - # (f"Checking port {port}...") - ser.write(b"ping\n") - response = ser.read_until(bytes("\n", "utf8")) - - # if pong is in response, then we are talking with the MCU - if b"pong" in response: - self.serial_port = port - self.get_logger().info(f"Found MCU at {self.serial_port}!") - break - except: - pass - - # If port is still None then we ain't finding no mcu - if self.serial_port is None: - self.get_logger().error("Unable to find MCU. Exiting...") - time.sleep(1) - sys.exit(1) - # Found a Serial port, try to open it; above code has not officially opened a Serial port - else: - self.get_logger().debug( - f"Attempting to open Serial port '{self.serial_port}'..." - ) - try: - self.serial_interface = serial.Serial( - self.serial_port, 115200, timeout=1 - ) - - # Attempt to get name of connected MCU - self.serial_interface.write( - b"can_relay_mode,on\n" - ) # can_relay_ready,[mcu] - mcu_name: str = "" - for _ in range(4): - response = self.serial_interface.read_until(bytes("\n", "utf8")) - try: - if b"can_relay_ready" in response: - args: list[str] = response.decode("utf8").strip().split(",") - if len(args) == 2: - mcu_name = args[1] - break - except UnicodeDecodeError: - pass # ignore malformed responses - self.get_logger().info( - f"MCU '{mcu_name}' is ready at '{self.serial_port}'." - ) - - except serial.SerialException as e: - self.get_logger().error( - f"Could not open Serial port '{self.serial_port}' for reason:" - ) - self.get_logger().error(e.strerror) - time.sleep(1) - sys.exit(1) + self.connector = SerialConnector(self.get_logger()) # Close serial port on exit atexit.register(self.cleanup) @@ -167,7 +55,7 @@ class Anchor(Node): ################################################## # ROS2 Topic Setup - # New pub/sub with VicCAN + # Pub/sub with VicCAN self.fromvic_debug_pub_ = self.create_publisher( String, "/anchor/from_vic/debug", 20 ) @@ -181,163 +69,32 @@ class Anchor(Node): VicCAN, "/anchor/from_vic/bio", 20 ) - self.mock_mcu_sub_ = self.create_subscription( - String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20 - ) self.tovic_sub_ = self.create_subscription( - VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20 - ) - self.tovic_debug_sub_ = self.create_subscription( - String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20 + VicCAN, "/anchor/to_vic/relay", self.connector.write, 20 ) - # Create publishers - self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10) - self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10) - self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10) - - self.debug_pub = self.create_publisher(String, "/anchor/debug", 10) - - # Create a subscriber - self.relay_sub = self.create_subscription( - String, "/anchor/relay", self.on_relay_tovic_string, 10 - ) + def cleanup(self): + self.connector.cleanup() def read_MCU(self): """Check the USB serial port for new data from the MCU, and publish string to appropriate topics""" - try: - output = str(self.serial_interface.readline(), "utf8") + output = self.connector.read() - if output: - self.relay_fromvic(output) - # All output over debug temporarily - # self.get_logger().info(f"[MCU] {output}") - msg = String() - msg.data = output - self.debug_pub.publish(msg) - if output.startswith("can_relay_fromvic,core"): - self.core_pub.publish(msg) - elif output.startswith("can_relay_fromvic,arm") or output.startswith( - "can_relay_fromvic,digit" - ): # digit for voltage readings - self.arm_pub.publish(msg) - if output.startswith("can_relay_fromvic,citadel") or output.startswith( - "can_relay_fromvic,digit" - ): # digit for SHT sensor - self.bio_pub.publish(msg) - # msg = String() - # msg.data = output - # self.debug_pub.publish(msg) - return - except serial.SerialException as e: - print(f"SerialException: {e}") - print("Closing serial port.") - try: - if self.serial_interface.is_open: - self.serial_interface.close() - except: - pass - exit(1) - except TypeError as e: - print(f"TypeError: {e}") - print("Closing serial port.") - try: - if self.serial_interface.is_open: - self.serial_interface.close() - except: - pass - exit(1) - except Exception as e: - print(f"Exception: {e}") - # print("Closing serial port.") - # if self.ser.is_open: - # self.ser.close() - # exit(1) + if not output: + return - def on_mock_fromvic(self, msg: String): - """For testing without an actual MCU, publish strings here as if they came from an MCU""" - # self.get_logger().info(f"Got command from mock MCU: {msg}") - self.relay_fromvic(msg.data) + self.relay_fromvic(output) - def on_relay_tovic_viccan(self, msg: VicCAN): - """Relay a VicCAN message to the MCU""" - output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}" - for num in msg.data: - output += f",{round(num, 7)}" # limit to 7 decimal places - output += "\n" - # self.get_logger().info(f"VicCAN relay to MCU: {output}") - self.serial_interface.write(bytes(output, "utf8")) - - def relay_fromvic(self, msg: str): + def relay_fromvic(self, msg: VicCAN): """Relay a string message from the MCU to the appropriate VicCAN topic""" - self.fromvic_debug_pub_.publish(String(data=msg)) - parts = msg.strip().split(",") - if len(parts) > 0 and parts[0] != "can_relay_fromvic": - self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'") - return + msg.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic") - # String validation - malformed: bool = False - malformed_reason: str = "" - if len(parts) < 3 or len(parts) > 7: - malformed = True - malformed_reason = ( - f"invalid argument count (expected [3,7], got {len(parts)})" - ) - elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]: - malformed = True - malformed_reason = f"invalid mcu_name '{parts[1]}'" - elif not (parts[2].isnumeric()) or int(parts[2]) < 0: - malformed = True - malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer" - else: - for x in parts[3:]: - try: - float(x) - except ValueError: - malformed = True - malformed_reason = f"data '{x}' is not a float" - break - - if malformed: - self.get_logger().warning( - f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}" - ) - return - - # Have valid VicCAN message - - output = VicCAN() - output.mcu_name = parts[1] - output.command_id = int(parts[2]) - if len(parts) > 3: - output.data = [float(x) for x in parts[3:]] - output.header = Header( - stamp=self.get_clock().now().to_msg(), frame_id="from_vic" - ) - - # self.get_logger().info(f"Relaying from MCU: {output}") - if output.mcu_name == "core": - self.fromvic_core_pub_.publish(output) - elif output.mcu_name == "arm" or output.mcu_name == "digit": - self.fromvic_arm_pub_.publish(output) - elif output.mcu_name == "citadel" or output.mcu_name == "digit": - self.fromvic_bio_pub_.publish(output) - - def on_relay_tovic_string(self, msg: String): - """Relay a raw string message to the MCU for debugging""" - message = msg.data - # self.get_logger().info(f"Sending command to MCU: {msg}") - self.serial_interface.write(bytes(message, "utf8")) - - @staticmethod - def list_serial_ports(): - return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*") - - def cleanup(self): - print("Cleaning up before terminating...") - if self.serial_interface.is_open: - self.serial_interface.close() + if msg.mcu_name == "core": + self.fromvic_core_pub_.publish(msg) + elif msg.mcu_name == "arm" or msg.mcu_name == "digit": + self.fromvic_arm_pub_.publish(msg) + elif msg.mcu_name == "citadel" or msg.mcu_name == "digit": + self.fromvic_bio_pub_.publish(msg) def main(args=None): diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py new file mode 100644 index 0000000..a19f30e --- /dev/null +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -0,0 +1,194 @@ +from abc import ABC, abstractmethod +import serial +import serial.tools.list_ports +from astra_msgs.msg import VicCAN +from rclpy.impl.rcutils_logger import RcutilsLogger + +KNOWN_USBS = [ + (0x2E8A, 0x00C0), # Raspberry Pi Pico + (0x1A86, 0x55D4), # Adafruit Feather ESP32 V2 + (0x10C4, 0xEA60), # DOIT ESP32 Devkit V1 + (0x1A86, 0x55D3), # ESP32 S3 Development Board +] +BAUD_RATE = 115200 + + +class NoValidDeviceException(Exception): + pass + + +class NoWorkingDeviceException(Exception): + pass + + +class MultipleValidDevicesException(Exception): + pass + + +class DeviceClosedException(Exception): + pass + + +class Connector(ABC): + logger: RcutilsLogger + + @abstractmethod + def read(self) -> VicCAN | None: + pass + + @abstractmethod + def write(self, msg: VicCAN): + pass + + def cleanup(self): + pass + + +class SerialConnector(Connector): + port: str + mcu_name: str + serial_interface: serial.Serial + override: bool + + def _get_name(self, port: str) -> str | None: + """ + Get the name of the MCU (if it works) + + returns: str name of the MCU, None if it doesn't work + """ + # attempt to open the serial port + serial_interface: serial.Serial + try: + self.logger.info(f"asking {port} for its name") + serial_interface = serial.Serial(port, BAUD_RATE, timeout=1) + + for i in range(4): + self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name") + response = serial_interface.read_until(bytes("\n", "utf8")) + try: + if b"can_relay_ready" in response: + args = response.decode("utf8").strip().split(",") + if len(args) == 2: + return args[1] + break + except UnicodeDecodeError as e: + self.logger.debug( + f"ignoring UnicodeDecodeError when asking for MCU name: {e}" + ) + + if serial_interface.is_open: + serial_interface.close() + except serial.SerialException as e: + self.logger.error(f"SerialException when asking for MCU name: {e}") + + return None + + def _find_ports(self) -> list[str]: + """ + Finds all valid ports but does not test them + + returns: all valid ports + """ + comports = serial.tools.list_ports.comports() + valid_ports = list( + map( # get just device strings + lambda p: p.device, + filter( # make sure we have a known device + lambda p: (p.vid, p.pid, p.device) in KNOWN_USBS + and p.device is not None, + comports, + ), + ) + ) + self.logger.debug(f"found valid MCU ports: [ {', '.join(valid_ports)} ]") + return valid_ports + + def cleanup(self): + self.logger.info(f"closing serial port if open {self.port}") + try: + if self.serial_interface.is_open: + self.serial_interface.close() + except Exception as e: + self.logger.error(e) + + def __init__(self, logger: RcutilsLogger, port_override: str | None = None): + self.logger = logger + self.override = bool(port_override) + + ports: list[str] + + if port_override: + ports = [port_override] + else: + ports = self._find_ports() + + if len(ports) <= 0: + raise NoValidDeviceException("no valid serial device found") + if (l := len(ports)) > 1: + raise MultipleValidDevicesException( + f"too many ({l}) valid serial devices found" + ) + + # check each of our ports to make sure one of them is responding + port = ports[0] + mcu_name = "mock" if self.override else self._get_name(port) + if not mcu_name: + raise NoWorkingDeviceException( + f"found {port}, but it did not respond with its name" + ) + + self.port = port + self.mcu_name = mcu_name + + # if we fail at this point, it should crash because we've already tested the port + self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=1) + + def read(self) -> VicCAN | None: + try: + raw = str(self.serial_interface.readline(), "utf8") + + if not raw: + return None + + parts = raw.split(",") + + # don't need an extra check because len of .split output is always >= 1 + if parts[0] != "can_relay_fromvic": + self.logger.debug(f"got non-CAN data from {self.mcu_name}: {raw}") + return None + elif len(parts) < 3: + self.logger.debug( + f"got garbage (not enough parts) CAN data from {self.mcu_name}: {raw}" + ) + return None + elif len(parts) > 7: + self.logger.debug( + f"got garbage (too many parts) CAN data from {self.mcu_name}: {raw}" + ) + return None + + return VicCAN( + mcu_name=parts[1], + command_id=parts[2], + data=[float(x) for x in parts[3:]], + ) + except serial.SerialException as e: + self.logger.error(f"SerialException: {e}") + self._close_port() + raise DeviceClosedException(f"serial port {self.port} closed unexpectedly") + except TypeError as e: + self.logger.error(f"TypeError: {e}") + self._close_port() + raise DeviceClosedException(f"serial port {self.port} closed unexpectedly") + except Exception: + pass # pretty much no other error matters + + def write(self, msg: VicCAN): + # go from [ w, x, y, z ] -> "w,x,y,z" & round to 7 digits max + data = ",".join([str(round(x, 7)) for x in msg.data]) + output = f"can_relay_tovic,{msg.mcu_name},{msg.command_id},{data}\n" + self.serial_interface.write(bytes(output, "utf8")) + + +class CANConnector(Connector): + pass From 4459886fc108f3eb5d0c2c6795dd40af480d1cd9 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Sat, 14 Feb 2026 23:16:34 -0600 Subject: [PATCH 02/25] add a mock mode and fix a logic error --- src/anchor_pkg/anchor_pkg/anchor_node.py | 76 +++++++++++++++++++++--- src/anchor_pkg/anchor_pkg/connector.py | 50 ++++++---------- src/anchor_pkg/anchor_pkg/convert.py | 24 ++++++++ 3 files changed, 108 insertions(+), 42 deletions(-) create mode 100644 src/anchor_pkg/anchor_pkg/convert.py diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 59198f6..d29bca1 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -1,11 +1,20 @@ import rclpy from rclpy.node import Node from rclpy.executors import ExternalShutdownException +from rcl_interfaces.msg import ParameterDescriptor, ParameterType import signal import atexit -from connector import Connector, SerialConnector, CANConnector +from .connector import ( + Connector, + MockConnector, + SerialConnector, + CANConnector, + NoValidDeviceException, + NoWorkingDeviceException, +) +from .convert import string_to_viccan import sys import threading @@ -37,25 +46,61 @@ class Anchor(Node): - For testing without an actual MCU, publish strings here as if they came from an MCU * /anchor/to_vic/relay - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU - * /anchor/to_vic/relay_string - - Publish raw strings to this topic to send directly to the MCU for debugging """ connector: Connector def __init__(self): - # Initalize node with name - super().__init__("anchor_node") # previously 'serial_publisher' + super().__init__("anchor_node") - self.connector = SerialConnector(self.get_logger()) + logger = self.get_logger() - # Close serial port on exit + self.declare_parameter( + "connector", + "auto", + ParameterDescriptor( + name="connector", + description="Declares which MCU connector should be used. Defaults to 'auto'.", + type=ParameterType.PARAMETER_STRING, + additional_constraints="Must be 'serial', 'can', 'mock', or 'auto'.", + ), + ) + + # Determine which connector to use. Options are Mock, Serial, and CAN + connector_select = ( + self.get_parameter("connector").get_parameter_value().string_value + ) + + match connector_select: + case "serial": + logger.info("using serial connector") + self.connector = SerialConnector(self.get_logger()) + case "can": + logger.info("using CAN connector") + self.connector = CANConnector(self.get_logger()) + case "mock": + logger.info("using mock connector") + self.connector = MockConnector(self.get_logger()) + case "auto": + logger.info("automatically determining connector") + try: + logger.info("trying CAN connector") + self.connector = CANConnector(self.get_logger()) + except (NoValidDeviceException, NoWorkingDeviceException): + logger.info("CAN connector failed, trying serial connector") + self.connector = SerialConnector(self.get_logger()) + case _: + self.get_logger().fatal( + f"invalid value for connector parameter: {connector_select}" + ) + exit(1) + + # Close devices on exit atexit.register(self.cleanup) - ################################################## # ROS2 Topic Setup - # Pub/sub with VicCAN + # Publishers self.fromvic_debug_pub_ = self.create_publisher( String, "/anchor/from_vic/debug", 20 ) @@ -69,9 +114,13 @@ class Anchor(Node): VicCAN, "/anchor/from_vic/bio", 20 ) + # Subscribers self.tovic_sub_ = self.create_subscription( VicCAN, "/anchor/to_vic/relay", self.connector.write, 20 ) + self.mock_mcu_sub_ = self.create_subscription( + String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20 + ) def cleanup(self): self.connector.cleanup() @@ -96,6 +145,15 @@ class Anchor(Node): elif msg.mcu_name == "citadel" or msg.mcu_name == "digit": self.fromvic_bio_pub_.publish(msg) + def on_mock_fromvic(self, msg: String): + viccan = string_to_viccan( + msg.data, + "mock", + self.get_logger(), + ) + if viccan: + self.relay_fromvic(viccan) + def main(args=None): try: diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index a19f30e..c70e0c5 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -3,6 +3,7 @@ import serial import serial.tools.list_ports from astra_msgs.msg import VicCAN from rclpy.impl.rcutils_logger import RcutilsLogger +from .convert import string_to_viccan KNOWN_USBS = [ (0x2E8A, 0x00C0), # Raspberry Pi Pico @@ -111,16 +112,10 @@ class SerialConnector(Connector): except Exception as e: self.logger.error(e) - def __init__(self, logger: RcutilsLogger, port_override: str | None = None): + def __init__(self, logger: RcutilsLogger): self.logger = logger - self.override = bool(port_override) - ports: list[str] - - if port_override: - ports = [port_override] - else: - ports = self._find_ports() + ports = self._find_ports() if len(ports) <= 0: raise NoValidDeviceException("no valid serial device found") @@ -150,35 +145,12 @@ class SerialConnector(Connector): if not raw: return None - parts = raw.split(",") - - # don't need an extra check because len of .split output is always >= 1 - if parts[0] != "can_relay_fromvic": - self.logger.debug(f"got non-CAN data from {self.mcu_name}: {raw}") - return None - elif len(parts) < 3: - self.logger.debug( - f"got garbage (not enough parts) CAN data from {self.mcu_name}: {raw}" - ) - return None - elif len(parts) > 7: - self.logger.debug( - f"got garbage (too many parts) CAN data from {self.mcu_name}: {raw}" - ) - return None - - return VicCAN( - mcu_name=parts[1], - command_id=parts[2], - data=[float(x) for x in parts[3:]], - ) + return string_to_viccan(raw, self.mcu_name, self.logger) except serial.SerialException as e: self.logger.error(f"SerialException: {e}") - self._close_port() raise DeviceClosedException(f"serial port {self.port} closed unexpectedly") except TypeError as e: self.logger.error(f"TypeError: {e}") - self._close_port() raise DeviceClosedException(f"serial port {self.port} closed unexpectedly") except Exception: pass # pretty much no other error matters @@ -191,4 +163,16 @@ class SerialConnector(Connector): class CANConnector(Connector): - pass + def __init__(self, logger: RcutilsLogger): + pass + + +class MockConnector(Connector): + def __init__(self, _: RcutilsLogger): + pass + + def read(self) -> VicCAN | None: + return None + + def write(self, msg: VicCAN): + print(msg) diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py new file mode 100644 index 0000000..cd13178 --- /dev/null +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -0,0 +1,24 @@ +from astra_msgs.msg import VicCAN +from rclpy.impl.rcutils_logger import RcutilsLogger + +def string_to_viccan(msg: str, mcu_name, logger: RcutilsLogger): + parts: list[str] = msg.split(",") + + # don't need an extra check because len of .split output is always >= 1 + if parts[0] != "can_relay_fromvic": + logger.info(f"got non-CAN data from {mcu_name}: {msg}") + return None + elif len(parts) < 3: + logger.info( + f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}" + ) + return None + elif len(parts) > 7: + logger.info(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}") + return None + + return VicCAN( + mcu_name=parts[1], + command_id=int(parts[2]), + data=[float(x) for x in parts[3:]], + ) From 225700bb868baefb25938a7d869a78140466bbf7 Mon Sep 17 00:00:00 2001 From: SHC-ASTRA <90978381+ASTRA-SHC@users.noreply.github.com> Date: Sun, 15 Feb 2026 00:47:20 -0600 Subject: [PATCH 03/25] tested it on testbed and had to change things --- src/anchor_pkg/anchor_pkg/anchor_node.py | 2 +- src/anchor_pkg/anchor_pkg/connector.py | 34 ++++++++++++++---------- src/anchor_pkg/anchor_pkg/convert.py | 6 ++--- 3 files changed, 24 insertions(+), 18 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index d29bca1..5f23e10 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -86,7 +86,7 @@ class Anchor(Node): try: logger.info("trying CAN connector") self.connector = CANConnector(self.get_logger()) - except (NoValidDeviceException, NoWorkingDeviceException): + except (NoValidDeviceException, NoWorkingDeviceException, TypeError): logger.info("CAN connector failed, trying serial connector") self.connector = SerialConnector(self.get_logger()) case _: diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index c70e0c5..ecfa0ef 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -63,19 +63,25 @@ class SerialConnector(Connector): self.logger.info(f"asking {port} for its name") serial_interface = serial.Serial(port, BAUD_RATE, timeout=1) + serial_interface.write( + b"can_relay_mode,on\n" + ) + for i in range(4): self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name") - response = serial_interface.read_until(bytes("\n", "utf8")) - try: - if b"can_relay_ready" in response: - args = response.decode("utf8").strip().split(",") - if len(args) == 2: - return args[1] - break - except UnicodeDecodeError as e: - self.logger.debug( - f"ignoring UnicodeDecodeError when asking for MCU name: {e}" - ) + for _ in range(4): + response = serial_interface.read_until(bytes("\n", "utf8")) + try: + if b"can_relay_ready" in response: + args: list[str] = response.decode("utf8").strip().split(",") + if len(args) == 2: + self.logger.info(f"we are talking to {args[1]}") + return args[1] + break + except UnicodeDecodeError as e: + self.logger.info( + f"ignoring UnicodeDecodeError when asking for MCU name: {e}" + ) if serial_interface.is_open: serial_interface.close() @@ -95,13 +101,13 @@ class SerialConnector(Connector): map( # get just device strings lambda p: p.device, filter( # make sure we have a known device - lambda p: (p.vid, p.pid, p.device) in KNOWN_USBS + lambda p: (p.vid, p.pid) in KNOWN_USBS and p.device is not None, comports, ), ) ) - self.logger.debug(f"found valid MCU ports: [ {', '.join(valid_ports)} ]") + self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]") return valid_ports def cleanup(self): @@ -126,7 +132,7 @@ class SerialConnector(Connector): # check each of our ports to make sure one of them is responding port = ports[0] - mcu_name = "mock" if self.override else self._get_name(port) + mcu_name = self._get_name(port) if not mcu_name: raise NoWorkingDeviceException( f"found {port}, but it did not respond with its name" diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index cd13178..d122226 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -6,15 +6,15 @@ def string_to_viccan(msg: str, mcu_name, logger: RcutilsLogger): # don't need an extra check because len of .split output is always >= 1 if parts[0] != "can_relay_fromvic": - logger.info(f"got non-CAN data from {mcu_name}: {msg}") + logger.debug(f"got non-CAN data from {mcu_name}: {msg}") return None elif len(parts) < 3: - logger.info( + logger.debug( f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}" ) return None elif len(parts) > 7: - logger.info(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}") + logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}") return None return VicCAN( From 809ca71208b842324450a8afd5f031ba82bea47f Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Sun, 15 Feb 2026 01:04:43 -0600 Subject: [PATCH 04/25] remove nested for loops --- src/anchor_pkg/anchor_pkg/connector.py | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index ecfa0ef..da3577c 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -69,19 +69,18 @@ class SerialConnector(Connector): for i in range(4): self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name") - for _ in range(4): - response = serial_interface.read_until(bytes("\n", "utf8")) - try: - if b"can_relay_ready" in response: - args: list[str] = response.decode("utf8").strip().split(",") - if len(args) == 2: - self.logger.info(f"we are talking to {args[1]}") - return args[1] - break - except UnicodeDecodeError as e: - self.logger.info( - f"ignoring UnicodeDecodeError when asking for MCU name: {e}" - ) + response = serial_interface.read_until(bytes("\n", "utf8")) + try: + if b"can_relay_ready" in response: + args: list[str] = response.decode("utf8").strip().split(",") + if len(args) == 2: + self.logger.info(f"we are talking to {args[1]}") + return args[1] + break + except UnicodeDecodeError as e: + self.logger.info( + f"ignoring UnicodeDecodeError when asking for MCU name: {e}" + ) if serial_interface.is_open: serial_interface.close() From 5c0194c543275548548c52543cd5129eb7cd4067 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Sun, 15 Feb 2026 01:05:37 -0600 Subject: [PATCH 05/25] remove KNOWN_USBS from anchor_node.py --- src/anchor_pkg/anchor_pkg/anchor_node.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 5f23e10..3d9df21 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -21,13 +21,6 @@ import threading from std_msgs.msg import String, Header from astra_msgs.msg import VicCAN -KNOWN_USBS = [ - (0x2E8A, 0x00C0), # Raspberry Pi Pico - (0x1A86, 0x55D4), # Adafruit Feather ESP32 V2 - (0x10C4, 0xEA60), # DOIT ESP32 Devkit V1 - (0x1A86, 0x55D3), # ESP32 S3 Development Board -] - class Anchor(Node): """ From b388275bba5014eb2260d6ce0a3c3edbcdea6200 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Sun, 15 Feb 2026 17:23:18 -0600 Subject: [PATCH 06/25] clean stuff up a bit to prep for CAN --- .envrc | 1 + flake.nix | 1 + src/anchor_pkg/anchor_pkg/connector.py | 150 +++++++++++++------------ src/anchor_pkg/anchor_pkg/convert.py | 20 +++- src/anchor_pkg/package.xml | 5 +- 5 files changed, 102 insertions(+), 75 deletions(-) diff --git a/.envrc b/.envrc index 3550a30..1306e13 100644 --- a/.envrc +++ b/.envrc @@ -1 +1,2 @@ use flake +[[ -d install ]] && source install/setup.$(echo $0 | grep -oE '[^/]+$') diff --git a/flake.nix b/flake.nix index f4d5bac..555e9db 100644 --- a/flake.nix +++ b/flake.nix @@ -28,6 +28,7 @@ (python313.withPackages ( p: with p; [ pyserial + python-can pygame scipy crccheck diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index da3577c..bdccf44 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -1,10 +1,16 @@ from abc import ABC, abstractmethod -import serial -import serial.tools.list_ports from astra_msgs.msg import VicCAN from rclpy.impl.rcutils_logger import RcutilsLogger from .convert import string_to_viccan +# CAN +import can +import can.interfaces.socketcan + +# Serial +import serial +import serial.tools.list_ports + KNOWN_USBS = [ (0x2E8A, 0x00C0), # Raspberry Pi Pico (0x1A86, 0x55D4), # Adafruit Feather ESP32 V2 @@ -33,6 +39,9 @@ class DeviceClosedException(Exception): class Connector(ABC): logger: RcutilsLogger + def __init__(self, logger: RcutilsLogger): + self.logger = logger + @abstractmethod def read(self) -> VicCAN | None: pass @@ -51,74 +60,8 @@ class SerialConnector(Connector): serial_interface: serial.Serial override: bool - def _get_name(self, port: str) -> str | None: - """ - Get the name of the MCU (if it works) - - returns: str name of the MCU, None if it doesn't work - """ - # attempt to open the serial port - serial_interface: serial.Serial - try: - self.logger.info(f"asking {port} for its name") - serial_interface = serial.Serial(port, BAUD_RATE, timeout=1) - - serial_interface.write( - b"can_relay_mode,on\n" - ) - - for i in range(4): - self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name") - response = serial_interface.read_until(bytes("\n", "utf8")) - try: - if b"can_relay_ready" in response: - args: list[str] = response.decode("utf8").strip().split(",") - if len(args) == 2: - self.logger.info(f"we are talking to {args[1]}") - return args[1] - break - except UnicodeDecodeError as e: - self.logger.info( - f"ignoring UnicodeDecodeError when asking for MCU name: {e}" - ) - - if serial_interface.is_open: - serial_interface.close() - except serial.SerialException as e: - self.logger.error(f"SerialException when asking for MCU name: {e}") - - return None - - def _find_ports(self) -> list[str]: - """ - Finds all valid ports but does not test them - - returns: all valid ports - """ - comports = serial.tools.list_ports.comports() - valid_ports = list( - map( # get just device strings - lambda p: p.device, - filter( # make sure we have a known device - lambda p: (p.vid, p.pid) in KNOWN_USBS - and p.device is not None, - comports, - ), - ) - ) - self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]") - return valid_ports - - def cleanup(self): - self.logger.info(f"closing serial port if open {self.port}") - try: - if self.serial_interface.is_open: - self.serial_interface.close() - except Exception as e: - self.logger.error(e) - def __init__(self, logger: RcutilsLogger): - self.logger = logger + super().__init__(logger) ports = self._find_ports() @@ -143,6 +86,61 @@ class SerialConnector(Connector): # if we fail at this point, it should crash because we've already tested the port self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=1) + def _find_ports(self) -> list[str]: + """ + Finds all valid ports but does not test them + + returns: all valid ports + """ + comports = serial.tools.list_ports.comports() + valid_ports = list( + map( # get just device strings + lambda p: p.device, + filter( # make sure we have a known device + lambda p: (p.vid, p.pid) in KNOWN_USBS and p.device is not None, + comports, + ), + ) + ) + self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]") + return valid_ports + + def _get_name(self, port: str) -> str | None: + """ + Get the name of the MCU (if it works) + + returns: str name of the MCU, None if it doesn't work + """ + # attempt to open the serial port + serial_interface: serial.Serial + try: + self.logger.info(f"asking {port} for its name") + serial_interface = serial.Serial(port, BAUD_RATE, timeout=1) + + serial_interface.write(b"can_relay_mode,on\n") + + for i in range(4): + self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name") + response = serial_interface.read_until(bytes("\n", "utf8")) + try: + if b"can_relay_ready" in response: + args: list[str] = response.decode("utf8").strip().split(",") + if len(args) == 2: + self.logger.info(f"we are talking to {args[1]}") + return args[1] + break + except UnicodeDecodeError as e: + self.logger.info( + f"ignoring UnicodeDecodeError when asking for MCU name: {e}" + ) + + if serial_interface.is_open: + serial_interface.close() + except serial.SerialException as e: + self.logger.error(f"SerialException when asking for MCU name: {e}") + + return None + def read(self) -> VicCAN | None: try: raw = str(self.serial_interface.readline(), "utf8") @@ -166,15 +164,23 @@ class SerialConnector(Connector): output = f"can_relay_tovic,{msg.mcu_name},{msg.command_id},{data}\n" self.serial_interface.write(bytes(output, "utf8")) + def cleanup(self): + self.logger.info(f"closing serial port if open {self.port}") + try: + if self.serial_interface.is_open: + self.serial_interface.close() + except Exception as e: + self.logger.error(e) + class CANConnector(Connector): def __init__(self, logger: RcutilsLogger): - pass + super().__init__(logger) class MockConnector(Connector): - def __init__(self, _: RcutilsLogger): - pass + def __init__(self, logger: RcutilsLogger): + super().__init__(logger) def read(self) -> VicCAN | None: return None diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index d122226..83113b0 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -1,7 +1,25 @@ from astra_msgs.msg import VicCAN from rclpy.impl.rcutils_logger import RcutilsLogger -def string_to_viccan(msg: str, mcu_name, logger: RcutilsLogger): +def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger): + """ + Converts the serial string VicCAN format to a ROS2 VicCAN message. + Does not fill out the Header of the message. + On a failure, it will log at a debug level why it failed. + + Parameters: + * msg: str + - The message in serial VicCAN format + * mcu_name: str + - The name of the MCU (e.g. core, citadel, arm) + * logger: RcutilsLogger + - A logger retrieved from node.get_logger() + + Returns: + * VicCAN | None + - The VicCAN message on a success or None on a failure + """ + parts: list[str] = msg.split(",") # don't need an extra check because len of .split output is always >= 1 diff --git a/src/anchor_pkg/package.xml b/src/anchor_pkg/package.xml index 4903787..5bb527f 100644 --- a/src/anchor_pkg/package.xml +++ b/src/anchor_pkg/package.xml @@ -3,13 +3,14 @@ anchor_pkg 0.0.0 - TODO: Package description - tristan + Anchor -- ROS and CAN relay node + Riley AGPL-3.0-only rclpy common_interfaces python3-serial + python3-can black From 9b96244a1bc275725c5eed409d51746b4612a7cd Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Tue, 17 Mar 2026 23:52:37 -0500 Subject: [PATCH 07/25] add can support --- src/anchor_pkg/anchor_pkg/connector.py | 230 +++++++++++++++++++++++++ src/anchor_pkg/launch/rover.launch.py | 3 +- 2 files changed, 232 insertions(+), 1 deletion(-) 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(), ) ) From c814f34ca6b13c80d82f62731f650fa791a316d7 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Wed, 18 Mar 2026 00:12:42 -0500 Subject: [PATCH 08/25] rewrite the launch file --- src/anchor_pkg/launch/rover.launch.py | 216 ++++++++++---------------- 1 file changed, 84 insertions(+), 132 deletions(-) diff --git a/src/anchor_pkg/launch/rover.launch.py b/src/anchor_pkg/launch/rover.launch.py index 4e7a435..c173a75 100644 --- a/src/anchor_pkg/launch/rover.launch.py +++ b/src/anchor_pkg/launch/rover.launch.py @@ -1,139 +1,91 @@ -#!/usr/bin/env python3 - from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown -from launch.substitutions import ( - LaunchConfiguration, - ThisLaunchFileDir, - PathJoinSubstitution, -) -from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, Shutdown from launch.conditions import IfCondition - - -# Prevent making __pycache__ directories -from sys import dont_write_bytecode - -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": - # Launch every node and pass "anchor" as the parameter - - nodes.append( - Node( - package="arm_pkg", - executable="arm", # change as needed - name="arm", - output="both", - parameters=[{"launch_mode": mode}], - on_exit=Shutdown(), - ) - ) - nodes.append( - Node( - package="core_pkg", - executable="core", # change as needed - name="core", - output="both", - parameters=[{"launch_mode": mode}], - on_exit=Shutdown(), - ) - ) - nodes.append( - Node( - package="core_pkg", - executable="ptz", # change as needed - name="ptz", - output="both", - condition=IfCondition(LaunchConfiguration("use_ptz", default="true")), - # Currently don't shutdown all nodes if the PTZ node fails, as it is not critical - # on_exit=Shutdown() # Uncomment if you want to shutdown on PTZ failure - ) - ) - nodes.append( - Node( - package="bio_pkg", - executable="bio", # change as needed - name="bio", - output="both", - parameters=[{"launch_mode": mode}], - on_exit=Shutdown(), - ) - ) - nodes.append( - Node( - package="anchor_pkg", - executable="anchor", # change as needed - name="anchor", - output="both", - parameters=[{"launch_mode": mode, "connector": connector}], - on_exit=Shutdown(), - ) - ) - elif mode in ["arm", "core", "bio", "ptz"]: - # Only launch the node corresponding to the provided mode. - if mode == "arm": - nodes.append( - Node( - package="arm_pkg", - executable="arm", - name="arm", - output="both", - parameters=[{"launch_mode": mode}], - on_exit=Shutdown(), - ) - ) - elif mode == "core": - nodes.append( - Node( - package="core_pkg", - executable="core", - name="core", - output="both", - parameters=[{"launch_mode": mode}], - on_exit=Shutdown(), - ) - ) - elif mode == "bio": - nodes.append( - Node( - package="bio_pkg", - executable="bio", - name="bio", - output="both", - parameters=[{"launch_mode": mode}], - on_exit=Shutdown(), - ) - ) - elif mode == "ptz": - nodes.append( - Node( - package="core_pkg", - executable="ptz", - name="ptz", - output="both", - on_exit=Shutdown(), # on fail, shutdown if this was the only node to be launched - ) - ) - else: - # If an invalid mode is provided, print an error. - print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.") - - return nodes +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node def generate_launch_description(): - declare_arg = DeclareLaunchArgument( - "mode", - default_value="anchor", - description="Launch mode: arm, core, bio, anchor, or ptz", + connector = LaunchConfiguration("connector") + use_ptz = LaunchConfiguration("use_ptz") + + ld = LaunchDescription() + + # arguments + ld.add_action( + DeclareLaunchArgument( + "connector", + default_value="auto", + description="Connector parameter for anchor node (default: auto)", + ) ) - return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)]) + ld.add_action( + DeclareLaunchArgument( + "use_ptz", + default_value="true", # must be string for launch system + description="Whether to launch PTZ node (default: true)", + ) + ) + + # nodes + ld.add_action( + Node( + package="arm_pkg", + executable="arm", + name="arm", + output="both", + parameters=[{"launch_mode": "anchor"}], + on_exit=Shutdown(), + ) + ) + + ld.add_action( + Node( + package="core_pkg", + executable="core", + name="core", + output="both", + parameters=[{"launch_mode": "anchor"}], + on_exit=Shutdown(), + ) + ) + + ld.add_action( + Node( + package="core_pkg", + executable="ptz", + name="ptz", + output="both", + condition=IfCondition(use_ptz), + ) + ) + + ld.add_action( + Node( + package="bio_pkg", + executable="bio", + name="bio", + output="both", + parameters=[{"launch_mode": "anchor"}], + on_exit=Shutdown(), + ) + ) + + ld.add_action( + Node( + package="anchor_pkg", + executable="anchor", + name="anchor", + output="both", + parameters=[ + { + "launch_mode": "anchor", + "connector": connector, + } + ], + on_exit=Shutdown(), + ) + ) + + return ld From 5e5a52438d0e4e60c3aa1279a051ba5e8a8e4874 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Wed, 18 Mar 2026 23:14:53 -0500 Subject: [PATCH 09/25] black fmt --- flake.lock | 23 ++++++++++++++++++++++- flake.nix | 16 ++++++++++++---- src/anchor_pkg/anchor_pkg/connector.py | 8 ++++++-- src/anchor_pkg/anchor_pkg/convert.py | 5 ++--- treefmt.nix | 8 ++++++++ 5 files changed, 50 insertions(+), 10 deletions(-) create mode 100644 treefmt.nix diff --git a/flake.lock b/flake.lock index 4c9cb33..5fd93a5 100644 --- a/flake.lock +++ b/flake.lock @@ -60,7 +60,8 @@ "nixpkgs": [ "nix-ros-overlay", "nixpkgs" - ] + ], + "treefmt-nix": "treefmt-nix" } }, "systems": { @@ -77,6 +78,26 @@ "repo": "default", "type": "github" } + }, + "treefmt-nix": { + "inputs": { + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1773297127, + "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=", + "owner": "numtide", + "repo": "treefmt-nix", + "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "treefmt-nix", + "type": "github" + } } }, "root": "root", diff --git a/flake.nix b/flake.nix index 555e9db..baf6853 100644 --- a/flake.nix +++ b/flake.nix @@ -4,6 +4,11 @@ inputs = { nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop"; nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!! + + treefmt-nix = { + url = "github:numtide/treefmt-nix"; + inputs.nixpkgs.follows = "nixpkgs"; + }; }; outputs = @@ -11,7 +16,8 @@ self, nix-ros-overlay, nixpkgs, - }: + ... + }@inputs: nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem ( system: let @@ -28,7 +34,7 @@ (python313.withPackages ( p: with p; [ pyserial - python-can + python-can pygame scipy crccheck @@ -62,7 +68,7 @@ moveit-msgs moveit-ros-planning moveit-ros-planning-interface - moveit-ros-visualization + moveit-ros-visualization moveit-configs-utils moveit-ros-move-group moveit-servo @@ -73,7 +79,7 @@ ompl joy ros2-controllers - chomp-motion-planner + chomp-motion-planner ]; } ) @@ -84,6 +90,8 @@ export QT_X11_NO_MITSHM=1 ''; }; + + formatter = (inputs.treefmt-nix.lib.evalModule pkgs ./treefmt.nix).config.build.wrapper; } ); diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index a0a8a7e..f85edfc 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -318,7 +318,9 @@ class CANConnector(Connector): } if mcu_name not in mcu_key_map: - self.logger.error(f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message") + self.logger.error( + f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message" + ) return mcu_key = mcu_key_map[mcu_name] & 0b111 @@ -352,7 +354,9 @@ class CANConnector(Connector): ) command &= 0x3F - arbitration_id = ((mcu_key & 0b111) << 8) | ((data_type_key & 0b11) << 6) | (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] = [] diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index 83113b0..0931cea 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -1,6 +1,7 @@ from astra_msgs.msg import VicCAN from rclpy.impl.rcutils_logger import RcutilsLogger + def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger): """ Converts the serial string VicCAN format to a ROS2 VicCAN message. @@ -27,9 +28,7 @@ def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger): logger.debug(f"got non-CAN data from {mcu_name}: {msg}") return None elif len(parts) < 3: - logger.debug( - f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}" - ) + logger.debug(f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}") return None elif len(parts) > 7: logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}") diff --git a/treefmt.nix b/treefmt.nix new file mode 100644 index 0000000..6bd693c --- /dev/null +++ b/treefmt.nix @@ -0,0 +1,8 @@ +{ pkgs, ... }: +{ + projectRootFile = "flake.nix"; + programs = { + nixfmt.enable = true; + black.enable = true; + }; +} From 7a3c4af1ce77ea4adc51bbaf2186c5d2dc66765b Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Wed, 18 Mar 2026 23:28:49 -0500 Subject: [PATCH 10/25] remove .envrc sourcing of install --- .envrc | 1 - 1 file changed, 1 deletion(-) diff --git a/.envrc b/.envrc index 1306e13..3550a30 100644 --- a/.envrc +++ b/.envrc @@ -1,2 +1 @@ use flake -[[ -d install ]] && source install/setup.$(echo $0 | grep -oE '[^/]+$') From 941e196316ce6144b11ff19eba27fe742dfca747 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Sat, 21 Mar 2026 18:14:44 -0500 Subject: [PATCH 11/25] implement review comments --- src/anchor_pkg/anchor_pkg/anchor_node.py | 145 +++++++++++++++-------- src/anchor_pkg/anchor_pkg/connector.py | 129 +++++++++++++------- src/anchor_pkg/anchor_pkg/convert.py | 61 ++++++---- 3 files changed, 220 insertions(+), 115 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 3d9df21..741b1b4 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -14,12 +14,11 @@ from .connector import ( NoValidDeviceException, NoWorkingDeviceException, ) -from .convert import string_to_viccan -import sys +from .convert import string_to_viccan, viccan_to_string import threading -from std_msgs.msg import String, Header from astra_msgs.msg import VicCAN +from std_msgs.msg import String class Anchor(Node): @@ -33,12 +32,16 @@ class Anchor(Node): - VicCAN messages for Arm node * /anchor/from_vic/bio - VicCAN messages for Bio node + * /anchor/to_vic/debug + - A string copy of the messages published to ./relay are published here Subscribers: * /anchor/from_vic/mock_mcu - For testing without an actual MCU, publish strings here as if they came from an MCU * /anchor/to_vic/relay - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU + * /anchor/to_vic/relay_string + - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector) """ connector: Connector @@ -59,31 +62,96 @@ class Anchor(Node): ), ) + self.declare_parameter( + "can_override", + "", + ParameterDescriptor( + name="can_override", + description="Overrides which CAN channel will be used. Defaults to ''.", + type=ParameterType.PARAMETER_STRING, + additional_constraints="Must be a valid CAN network that shows up in `ip link show`.", + ), + ) + + # ROS2 Topic Setup + + # Publishers + self.fromvic_debug_pub_ = self.create_publisher( # only used by serial + String, + "/anchor/from_vic/debug", + 20, + ) + self.fromvic_core_pub_ = self.create_publisher( + VicCAN, + "/anchor/from_vic/core", + 20, + ) + self.fromvic_arm_pub_ = self.create_publisher( + VicCAN, + "/anchor/from_vic/arm", + 20, + ) + self.fromvic_bio_pub_ = self.create_publisher( + VicCAN, + "/anchor/from_vic/bio", + 20, + ) + # Debug publisher + self.tovic_debug_pub_ = self.create_publisher( + VicCAN, + "/anchor/to_vic/debug", + 20, + ) + + # Subscribers + self.tovic_sub_ = self.create_subscription( + VicCAN, + "/anchor/to_vic/relay", + self.write_connector, + 20, + ) + self.mock_mcu_sub_ = self.create_subscription( + String, + "/anchor/from_vic/mock_mcu", + self.on_mock_fromvic, + 20, + ) + self.tovic_string_sub_ = self.create_subscription( + String, + "/anchor/to_vic/relay_string", + self.connector.write_raw, + 20, + ) + # Determine which connector to use. Options are Mock, Serial, and CAN connector_select = ( self.get_parameter("connector").get_parameter_value().string_value ) - + can_override = ( + self.get_parameter("can_override").get_parameter_value().string_value + ) match connector_select: case "serial": logger.info("using serial connector") - self.connector = SerialConnector(self.get_logger()) + self.connector = SerialConnector(logger, self.get_clock()) case "can": logger.info("using CAN connector") - self.connector = CANConnector(self.get_logger()) + self.connector = CANConnector(logger, self.get_clock(), can_override) case "mock": logger.info("using mock connector") - self.connector = MockConnector(self.get_logger()) + self.connector = MockConnector(logger, self.get_clock()) case "auto": logger.info("automatically determining connector") try: logger.info("trying CAN connector") - self.connector = CANConnector(self.get_logger()) + self.connector = CANConnector( + logger, self.get_clock(), can_override + ) except (NoValidDeviceException, NoWorkingDeviceException, TypeError): logger.info("CAN connector failed, trying serial connector") - self.connector = SerialConnector(self.get_logger()) + self.connector = SerialConnector(logger, self.get_clock()) case _: - self.get_logger().fatal( + logger.fatal( f"invalid value for connector parameter: {connector_select}" ) exit(1) @@ -91,46 +159,26 @@ class Anchor(Node): # Close devices on exit atexit.register(self.cleanup) - # ROS2 Topic Setup - - # Publishers - self.fromvic_debug_pub_ = self.create_publisher( - String, "/anchor/from_vic/debug", 20 - ) - self.fromvic_core_pub_ = self.create_publisher( - VicCAN, "/anchor/from_vic/core", 20 - ) - self.fromvic_arm_pub_ = self.create_publisher( - VicCAN, "/anchor/from_vic/arm", 20 - ) - self.fromvic_bio_pub_ = self.create_publisher( - VicCAN, "/anchor/from_vic/bio", 20 - ) - - # Subscribers - self.tovic_sub_ = self.create_subscription( - VicCAN, "/anchor/to_vic/relay", self.connector.write, 20 - ) - self.mock_mcu_sub_ = self.create_subscription( - String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20 - ) - def cleanup(self): self.connector.cleanup() - def read_MCU(self): - """Check the USB serial port for new data from the MCU, and publish string to appropriate topics""" - output = self.connector.read() + def read_connector(self): + """Check the connector for new data from the MCU, and publish string to appropriate topics""" + viccan, raw = self.connector.read() - if not output: - return + if raw: + self.fromvic_debug_pub_.publish(String(data=raw)) - self.relay_fromvic(output) + if viccan: + self.relay_fromvic(viccan) + + def write_connector(self, msg: VicCAN): + """Write to the connector and send a copy to /anchor/to_vic/debug""" + self.connector.write(msg) + self.tovic_debug_pub_.publish(viccan_to_string(msg)) def relay_fromvic(self, msg: VicCAN): - """Relay a string message from the MCU to the appropriate VicCAN topic""" - msg.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic") - + """Relay a message from the MCU to the appropriate VicCAN topic""" if msg.mcu_name == "core": self.fromvic_core_pub_.publish(msg) elif msg.mcu_name == "arm" or msg.mcu_name == "digit": @@ -139,10 +187,12 @@ class Anchor(Node): self.fromvic_bio_pub_.publish(msg) def on_mock_fromvic(self, msg: String): + """Relay a message as if it came from the MCU""" viccan = string_to_viccan( msg.data, "mock", self.get_logger(), + self.get_clock().now().to_msg(), ) if viccan: self.relay_fromvic(viccan) @@ -158,16 +208,9 @@ def main(args=None): rate = anchor_node.create_rate(100) # 100 Hz -- arbitrary rate while rclpy.ok(): - anchor_node.read_MCU() # Check the MCU for updates + anchor_node.read_connector() # Check the connector for updates rate.sleep() except (KeyboardInterrupt, ExternalShutdownException): print("Caught shutdown signal, shutting down...") finally: rclpy.try_shutdown() - - -if __name__ == "__main__": - signal.signal( - signal.SIGTERM, lambda signum, frame: sys.exit(0) - ) # Catch termination signals and exit cleanly - main() diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index f85edfc..871b5ea 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -1,7 +1,8 @@ from abc import ABC, abstractmethod from astra_msgs.msg import VicCAN +from rclpy.clock import Clock from rclpy.impl.rcutils_logger import RcutilsLogger -from .convert import string_to_viccan +from .convert import string_to_viccan as _string_to_viccan, viccan_to_string # CAN import can @@ -39,18 +40,36 @@ class DeviceClosedException(Exception): class Connector(ABC): logger: RcutilsLogger + clock: Clock - def __init__(self, logger: RcutilsLogger): + def string_to_viccan(self, msg: str, mcu_name: str): + """function currying so that we do not need to pass logger and clock every time""" + return _string_to_viccan( + msg, + mcu_name, + self.logger, + self.clock.now().to_msg(), + ) + + def __init__(self, logger: RcutilsLogger, clock: Clock): self.logger = logger + self.clock = clock @abstractmethod - def read(self) -> VicCAN | None: + def read(self) -> tuple[VicCAN | None, str | None]: + """ + Must return a tuple of (VicCAN, debug message or string repr of VicCAN) + """ pass @abstractmethod def write(self, msg: VicCAN): pass + @abstractmethod + def write_raw(self, msg: str): + pass + def cleanup(self): pass @@ -61,8 +80,8 @@ class SerialConnector(Connector): serial_interface: serial.Serial override: bool - def __init__(self, logger: RcutilsLogger): - super().__init__(logger) + def __init__(self, logger: RcutilsLogger, clock: Clock): + super().__init__(logger, clock) ports = self._find_ports() @@ -136,34 +155,36 @@ class SerialConnector(Connector): ) if serial_interface.is_open: + # turn relay mode off if it failed to respond with its name + serial_interface.write(b"can_relay_mode,off\n") serial_interface.close() except serial.SerialException as e: self.logger.error(f"SerialException when asking for MCU name: {e}") return None - def read(self) -> VicCAN | None: + def read(self) -> tuple[VicCAN | None, str | None]: try: raw = str(self.serial_interface.readline(), "utf8") if not raw: - return None + return (None, None) - return string_to_viccan(raw, self.mcu_name, self.logger) + return ( + self.string_to_viccan(raw, self.mcu_name), + raw, + ) except serial.SerialException as e: self.logger.error(f"SerialException: {e}") raise DeviceClosedException(f"serial port {self.port} closed unexpectedly") - except TypeError as e: - self.logger.error(f"TypeError: {e}") - raise DeviceClosedException(f"serial port {self.port} closed unexpectedly") except Exception: - pass # pretty much no other error matters + return (None, None) # pretty much no other error matters def write(self, msg: VicCAN): - # go from [ w, x, y, z ] -> "w,x,y,z" & round to 7 digits max - data = ",".join([str(round(x, 7)) for x in msg.data]) - output = f"can_relay_tovic,{msg.mcu_name},{msg.command_id},{data}\n" - self.serial_interface.write(bytes(output, "utf8")) + self.write_raw(viccan_to_string(msg)) + + def write_raw(self, msg: str): + self.serial_interface.write(bytes(msg, "utf8")) def cleanup(self): self.logger.info(f"closing serial port if open {self.port}") @@ -175,26 +196,35 @@ class SerialConnector(Connector): class CANConnector(Connector): - def __init__(self, logger: RcutilsLogger): - super().__init__(logger) + def __init__(self, logger: RcutilsLogger, clock: Clock, can_override: str): + super().__init__(logger, clock) 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() + 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}]" + if len(avail) == 0: + raise NoValidDeviceException("no CAN interfaces found") + + # filter to busses whose channel matches the can_override + if can_override: + avail = list( + filter( + lambda b: b.get("channel") == can_override, + avail, ) + ) - bus = avail[0] - self.can_channel = str(bus.get("channel")) - self.logger.info(f"found CAN interface '{self.can_channel}'") + 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( @@ -208,9 +238,9 @@ class CANConnector(Connector): ) if self.can_channel and self.can_channel.startswith("v"): - self.logger.warn("using virtual CAN interface; this is likely vcan*") + self.logger.warn("likely using virtual CAN interface") - def read(self) -> VicCAN | None: + def read(self) -> tuple[VicCAN | None, str | None]: if not self.can_bus: raise DeviceClosedException("CAN bus not initialized") @@ -221,7 +251,7 @@ class CANConnector(Connector): raise DeviceClosedException("CAN bus closed unexpectedly") if message is None: - return None + return (None, None) arbitration_id = message.arbitration_id & 0x7FF data_bytes = bytes(message.data) @@ -244,7 +274,7 @@ class CANConnector(Connector): self.logger.warn( f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}" ) - return None + return (None, None) data: list[float] = [] @@ -256,7 +286,7 @@ class CANConnector(Connector): self.logger.warn( f"received double payload with insufficient length {len(data_bytes)}; dropping frame" ) - return None + return (None, None) (value,) = struct.unpack(" VicCAN | None: - return None + def read(self) -> tuple[VicCAN | None, str | None]: + return (None, None) def write(self, msg: VicCAN): - print(msg) + pass + + def write_raw(self, msg: str): + pass diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index 0931cea..49a4bda 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -1,27 +1,19 @@ from astra_msgs.msg import VicCAN +from std_msgs.msg import Header +from builtin_interfaces.msg import Time from rclpy.impl.rcutils_logger import RcutilsLogger -def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger): +def string_to_viccan( + msg: str, mcu_name: str, logger: RcutilsLogger, time: Time +) -> VicCAN | None: """ Converts the serial string VicCAN format to a ROS2 VicCAN message. Does not fill out the Header of the message. - On a failure, it will log at a debug level why it failed. - - Parameters: - * msg: str - - The message in serial VicCAN format - * mcu_name: str - - The name of the MCU (e.g. core, citadel, arm) - * logger: RcutilsLogger - - A logger retrieved from node.get_logger() - - Returns: - * VicCAN | None - - The VicCAN message on a success or None on a failure + On a failure, it will log at a debug level why it failed and return None. """ - parts: list[str] = msg.split(",") + parts: list[str] = msg.strip().split(",") # don't need an extra check because len of .split output is always >= 1 if parts[0] != "can_relay_fromvic": @@ -34,8 +26,37 @@ def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger): logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}") return None - return VicCAN( - mcu_name=parts[1], - command_id=int(parts[2]), - data=[float(x) for x in parts[3:]], - ) + try: + command_id = int(parts[2]) + except ValueError: + logger.debug( + f"got garbage (non-integer command id) CAN data from {mcu_name}: {msg}" + ) + return None + + if command_id not in range(64): + logger.debug( + f"got garbage (wrong command id {command_id}) CAN data from {mcu_name}: {msg}" + ) + return None + + try: + return VicCAN( + header=Header( + stamp=time, + frame_id="from_vic", + ), + mcu_name=parts[1], + command_id=command_id, + data=[float(x) for x in parts[3:]], + ) + except ValueError: + logger.debug(f"got garbage (non-numerical) CAN data from {mcu_name}: {msg}") + return None + + +def viccan_to_string(viccan: VicCAN) -> str: + """Converts a ROS2 VicCAN message to the serial string VicCAN format.""" + # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max + data = "".join([f",{round(val,7)}" for val in viccan.data]) + return f"can_relay_tovic,{viccan.mcu_name}{data}\n" From fe46a2ab4d649f310f0ed334339c2aec6a7e7b17 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Mon, 23 Mar 2026 13:25:13 -0500 Subject: [PATCH 12/25] fix wrong order for initialization --- src/anchor_pkg/anchor_pkg/anchor_node.py | 66 ++++++++++++------------ 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 741b1b4..f3dbd16 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -73,6 +73,39 @@ class Anchor(Node): ), ) + # Determine which connector to use. Options are Mock, Serial, and CAN + connector_select = ( + self.get_parameter("connector").get_parameter_value().string_value + ) + can_override = ( + self.get_parameter("can_override").get_parameter_value().string_value + ) + match connector_select: + case "serial": + logger.info("using serial connector") + self.connector = SerialConnector(logger, self.get_clock()) + case "can": + logger.info("using CAN connector") + self.connector = CANConnector(logger, self.get_clock(), can_override) + case "mock": + logger.info("using mock connector") + self.connector = MockConnector(logger, self.get_clock()) + case "auto": + logger.info("automatically determining connector") + try: + logger.info("trying CAN connector") + self.connector = CANConnector( + logger, self.get_clock(), can_override + ) + except (NoValidDeviceException, NoWorkingDeviceException, TypeError): + logger.info("CAN connector failed, trying serial connector") + self.connector = SerialConnector(logger, self.get_clock()) + case _: + logger.fatal( + f"invalid value for connector parameter: {connector_select}" + ) + exit(1) + # ROS2 Topic Setup # Publishers @@ -123,39 +156,6 @@ class Anchor(Node): 20, ) - # Determine which connector to use. Options are Mock, Serial, and CAN - connector_select = ( - self.get_parameter("connector").get_parameter_value().string_value - ) - can_override = ( - self.get_parameter("can_override").get_parameter_value().string_value - ) - match connector_select: - case "serial": - logger.info("using serial connector") - self.connector = SerialConnector(logger, self.get_clock()) - case "can": - logger.info("using CAN connector") - self.connector = CANConnector(logger, self.get_clock(), can_override) - case "mock": - logger.info("using mock connector") - self.connector = MockConnector(logger, self.get_clock()) - case "auto": - logger.info("automatically determining connector") - try: - logger.info("trying CAN connector") - self.connector = CANConnector( - logger, self.get_clock(), can_override - ) - except (NoValidDeviceException, NoWorkingDeviceException, TypeError): - logger.info("CAN connector failed, trying serial connector") - self.connector = SerialConnector(logger, self.get_clock()) - case _: - logger.fatal( - f"invalid value for connector parameter: {connector_select}" - ) - exit(1) - # Close devices on exit atexit.register(self.cleanup) From f7efa604d2eeaa3089e35972695401f503c1dd26 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Mon, 23 Mar 2026 20:39:50 -0500 Subject: [PATCH 13/25] finish adding parameters --- README.md | 39 ++++++++++++++++++++++-- src/anchor_pkg/anchor_pkg/anchor_node.py | 24 +++++++++++++-- src/anchor_pkg/anchor_pkg/connector.py | 14 +++++++-- src/anchor_pkg/launch/rover.launch.py | 22 ++++++++++++- 4 files changed, 91 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 19d15ed..fbdf62d 100644 --- a/README.md +++ b/README.md @@ -60,6 +60,20 @@ $ ros2 launch anchor_pkg rover.launch.py # Must be run on a computer connected $ ros2 run headless_pkg headless_full # Optionally run in a separate shell on the same or different computer. ``` +### Using the Mock Connector + +Anchor provides a mock connector meant for testing and scripting purposes. You can select the mock connector by running anchor with this command: + +```bash +$ ros2 launch anchor_pkg rover.launch.py connector:="mock" +``` + +You can see all data sent to it in a string format with this command: + +```bash +$ ros2 topic echo /anchor/to_vic/debug +``` + ### Testing Serial You can fake the presence of a Serial device (i.e., MCU) by using the following command: @@ -68,10 +82,31 @@ You can fake the presence of a Serial device (i.e., MCU) by using the following $ socat -dd -v pty,rawer,crnl,link=/tmp/ttyACM9 pty,rawer,crnl,link=/tmp/ttyOUT ``` -When you go to run anchor, use the `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so: +When you go to run anchor, use the `serial_override` ROS2 parameter to point it to the fake serial port, like so: ```bash -$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py +$ ros2 launch anchor_pkg rover.launch.py connector:=serial serial_override:=/tmp/ttyACM9 +``` + +### Testing CAN + +You can create a virtual CAN network by using the following commands to create and then enable it: + +```bash +sudo ip link add dev vcan0 type vcan +sudo ip link set vcan0 up +``` + +When you go to run anchor, use the `can_override` ROS2 parameter to point it to the virtual CAN network, like so: + +```bash +$ ros2 launch anchor_pkg rover.launch.py connector:=can can_override:=vcan0 +``` + +Once you're done, you should delete the virtual network so that anchor doesn't get confused if you plug in a real CAN adapter: + +```bash +$ sudo ip link delete vcan0 ``` ### Connecting the GuliKit Controller diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index f3dbd16..6b06947 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -51,6 +51,8 @@ class Anchor(Node): logger = self.get_logger() + # ROS2 Parameter Setup + self.declare_parameter( "connector", "auto", @@ -73,6 +75,17 @@ class Anchor(Node): ), ) + self.declare_parameter( + "serial_override", + "", + ParameterDescriptor( + name="serial_override", + description="Overrides which serial port will be used. Defaults to ''.", + type=ParameterType.PARAMETER_STRING, + additional_constraints="Must be a valid path to a serial device file that shows up in `ls /dev/tty*`.", + ), + ) + # Determine which connector to use. Options are Mock, Serial, and CAN connector_select = ( self.get_parameter("connector").get_parameter_value().string_value @@ -80,10 +93,15 @@ class Anchor(Node): can_override = ( self.get_parameter("can_override").get_parameter_value().string_value ) + serial_override = ( + self.get_parameter("serial_override").get_parameter_value().string_value + ) match connector_select: case "serial": logger.info("using serial connector") - self.connector = SerialConnector(logger, self.get_clock()) + self.connector = SerialConnector( + logger, self.get_clock(), serial_override + ) case "can": logger.info("using CAN connector") self.connector = CANConnector(logger, self.get_clock(), can_override) @@ -99,7 +117,9 @@ class Anchor(Node): ) except (NoValidDeviceException, NoWorkingDeviceException, TypeError): logger.info("CAN connector failed, trying serial connector") - self.connector = SerialConnector(logger, self.get_clock()) + self.connector = SerialConnector( + logger, self.get_clock(), serial_override + ) case _: logger.fatal( f"invalid value for connector parameter: {connector_select}" diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 871b5ea..88e17d8 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -78,12 +78,19 @@ class SerialConnector(Connector): port: str mcu_name: str serial_interface: serial.Serial - override: bool - def __init__(self, logger: RcutilsLogger, clock: Clock): + def __init__(self, logger: RcutilsLogger, clock: Clock, serial_override: str = ""): super().__init__(logger, clock) ports = self._find_ports() + mcu_name: str | None = None + + if serial_override: + logger.warn( + f"using serial_override: `{serial_override}`! this will bypass several checks." + ) + ports = [serial_override] + mcu_name = "override" if len(ports) <= 0: raise NoValidDeviceException("no valid serial device found") @@ -94,7 +101,8 @@ class SerialConnector(Connector): # check each of our ports to make sure one of them is responding port = ports[0] - mcu_name = self._get_name(port) + # we might already have a name by now if we overrode earlier + mcu_name = mcu_name or self._get_name(port) if not mcu_name: raise NoWorkingDeviceException( f"found {port}, but it did not respond with its name" diff --git a/src/anchor_pkg/launch/rover.launch.py b/src/anchor_pkg/launch/rover.launch.py index c173a75..824ddd3 100644 --- a/src/anchor_pkg/launch/rover.launch.py +++ b/src/anchor_pkg/launch/rover.launch.py @@ -7,6 +7,8 @@ from launch_ros.actions import Node def generate_launch_description(): connector = LaunchConfiguration("connector") + serial_override = LaunchConfiguration("serial_override") + can_override = LaunchConfiguration("can_override") use_ptz = LaunchConfiguration("use_ptz") ld = LaunchDescription() @@ -16,7 +18,23 @@ def generate_launch_description(): DeclareLaunchArgument( "connector", default_value="auto", - description="Connector parameter for anchor node (default: auto)", + description="Connector parameter for anchor node (default: 'auto')", + ) + ) + + ld.add_action( + DeclareLaunchArgument( + "serial_override", + default_value="", + description="Serial port override parameter for anchor node (default: '')", + ) + ) + + ld.add_action( + DeclareLaunchArgument( + "can_override", + default_value="auto", + description="CAN network override parameter for anchor node (default: '')", ) ) @@ -82,6 +100,8 @@ def generate_launch_description(): { "launch_mode": "anchor", "connector": connector, + "serial_override": serial_override, + "can_override": can_override, } ], on_exit=Shutdown(), From e570d371c6f0dca3ce325dae93d1f9ab00a6cf9e Mon Sep 17 00:00:00 2001 From: SHC-ASTRA <90978381+ASTRA-SHC@users.noreply.github.com> Date: Wed, 1 Apr 2026 01:48:40 -0500 Subject: [PATCH 14/25] fix a plethora of bugs related to the serial connector --- src/anchor_pkg/anchor_pkg/anchor_node.py | 26 +++++++++++++++++++++--- src/anchor_pkg/anchor_pkg/connector.py | 1 + src/anchor_pkg/anchor_pkg/convert.py | 4 ++-- src/anchor_pkg/launch/rover.launch.py | 2 +- 4 files changed, 27 insertions(+), 6 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 6b06947..8edc8b5 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -1,9 +1,9 @@ +from warnings import deprecated import rclpy from rclpy.node import Node from rclpy.executors import ExternalShutdownException from rcl_interfaces.msg import ParameterDescriptor, ParameterType -import signal import atexit from .connector import ( @@ -14,7 +14,7 @@ from .connector import ( NoValidDeviceException, NoWorkingDeviceException, ) -from .convert import string_to_viccan, viccan_to_string +from .convert import string_to_viccan import threading from astra_msgs.msg import VicCAN @@ -163,6 +163,12 @@ class Anchor(Node): self.write_connector, 20, ) + self.tovic_sub_legacy_ = self.create_subscription( + String, + "/anchor/relay", + self.write_connector_legacy, + 20, + ) self.mock_mcu_sub_ = self.create_subscription( String, "/anchor/from_vic/mock_mcu", @@ -195,7 +201,21 @@ class Anchor(Node): def write_connector(self, msg: VicCAN): """Write to the connector and send a copy to /anchor/to_vic/debug""" self.connector.write(msg) - self.tovic_debug_pub_.publish(viccan_to_string(msg)) + self.tovic_debug_pub_.publish(msg) + + @deprecated("Use /anchor/to_vic/relay instead of /anchor/relay") + def write_connector_legacy(self, msg: String): + """Write to the connector by first attempting to convert String to VicCAN""" + # please do not reference this code. ~riley + for cmd in msg.data.split("\n"): + viccan = string_to_viccan( + cmd, + "anchor", + self.get_logger(), + self.get_clock().now().to_msg(), + ) + if viccan: + self.write_connector(viccan) def relay_fromvic(self, msg: VicCAN): """Relay a message from the MCU to the appropriate VicCAN topic""" diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 88e17d8..58c33d9 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -217,6 +217,7 @@ class CANConnector(Connector): # filter to busses whose channel matches the can_override if can_override: + self.logger.info(f"overrode can interface with {can_override}") avail = list( filter( lambda b: b.get("channel") == can_override, diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index 49a4bda..cfecfce 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -16,7 +16,7 @@ def string_to_viccan( parts: list[str] = msg.strip().split(",") # don't need an extra check because len of .split output is always >= 1 - if parts[0] != "can_relay_fromvic": + if not parts[0].startswith("can_relay_"): logger.debug(f"got non-CAN data from {mcu_name}: {msg}") return None elif len(parts) < 3: @@ -59,4 +59,4 @@ def viccan_to_string(viccan: VicCAN) -> str: """Converts a ROS2 VicCAN message to the serial string VicCAN format.""" # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max data = "".join([f",{round(val,7)}" for val in viccan.data]) - return f"can_relay_tovic,{viccan.mcu_name}{data}\n" + return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n" diff --git a/src/anchor_pkg/launch/rover.launch.py b/src/anchor_pkg/launch/rover.launch.py index 824ddd3..7c987dc 100644 --- a/src/anchor_pkg/launch/rover.launch.py +++ b/src/anchor_pkg/launch/rover.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): ld.add_action( DeclareLaunchArgument( "can_override", - default_value="auto", + default_value="", description="CAN network override parameter for anchor node (default: '')", ) ) From 327539467ce73ee1e4837eeae4941853c5de0952 Mon Sep 17 00:00:00 2001 From: SHC-ASTRA <90978381+ASTRA-SHC@users.noreply.github.com> Date: Wed, 1 Apr 2026 02:50:12 -0500 Subject: [PATCH 15/25] fixed can connector --- src/anchor_pkg/anchor_pkg/connector.py | 127 +++++++++---------------- src/core_pkg/core_pkg/core_ptz.py | 2 +- 2 files changed, 46 insertions(+), 83 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 58c33d9..6c3bf0b 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -21,6 +21,16 @@ KNOWN_USBS = [ ] BAUD_RATE = 115200 +MCU_IDS = [ + "broadcast", + "core", + "arm", + "digit", + "faerie", + "citadel", + "libs", + ] + class NoValidDeviceException(Exception): pass @@ -269,17 +279,9 @@ class CANConnector(Connector): 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: + try: + mcu_name = MCU_IDS[mcu_key] + except IndexError: self.logger.warn( f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}" ) @@ -296,7 +298,7 @@ class CANConnector(Connector): f"received double payload with insufficient length {len(data_bytes)}; dropping frame" ) return (None, None) - (value,) = struct.unpack("d", data_bytes[:8]) data = [float(value)] elif data_type_key == 1: if len(data_bytes) < 8: @@ -304,7 +306,7 @@ class CANConnector(Connector): f"received float32x2 payload with insufficient length {len(data_bytes)}; dropping frame" ) return (None, None) - v1, v2 = struct.unpack("ff", data_bytes[:8]) data = [float(v1), float(v2)] elif data_type_key == 2: if len(data_bytes) < 8: @@ -312,7 +314,7 @@ class CANConnector(Connector): f"received int16x4 payload with insufficient length {len(data_bytes)}; dropping frame" ) return (None, None) - i1, i2, i3, i4 = struct.unpack("hhhh", data_bytes[:8]) data = [float(i1), float(i2), float(i3), float(i4)] else: self.logger.warn( @@ -344,92 +346,53 @@ class CANConnector(Connector): if not self.can_bus: raise DeviceClosedException("CAN bus not initialized") - # Build 11-bit arbitration ID according to the VicCAN scheme: + # build 11-bit arbitration ID according to VicCAN spec: # bits 10..8: targeted MCU key # bits 7..6: data type key # bits 5..0: command - # Map MCU name to 3-bit key. - mcu_name = (msg.mcu_name or "").lower() - mcu_key_map: dict[str, int] = { - "broadcast": 1, - "core": 2, - "arm": 3, - "digit": 4, - "faerie": 5, - "citadel": 6, - } - - if mcu_name not in mcu_key_map: + # map MCU name to 3-bit key. + try: + mcu_id = MCU_IDS.index((msg.mcu_name or "").lower()) + except ValueError: self.logger.error( f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message" ) return - mcu_key = mcu_key_map[mcu_name] & 0b111 - - # Infer data type key from payload length according to the table: + # determine data type from length: # 0: double x1, 1: float32 x2, 2: int16 x4, 3: empty - data_len = len(msg.data) - if data_len == 0: - data_type_key = 3 - elif data_len == 1: - data_type_key = 0 - elif data_len == 2: - data_type_key = 1 - elif data_len == 4: - data_type_key = 2 - else: - # Fallback: treat any other non-zero length as float32 x2 - self.logger.warn( - f"unexpected VicCAN data length {data_len}; encoding as float32 x2 (key=1) and truncating/padding as needed" - ) - data_type_key = 1 - - # Command is limited to 6 bits. - command = int(msg.command_id) - if command < 0: - self.logger.error(f"invalid negative command_id for CAN frame: {command}") - return - if command > 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): + match data_len := len(msg.data): + case 0: + data_type = 3 + data = bytes() + case 1: + data_type = 0 + data = struct.pack(">d", *msg.data) + case 2: + data_type = 1 + data = struct.pack(">ff", *msg.data) + case 4: + data_type = 2 + data = struct.pack(">hhhh", *[ int(x) for x in msg.data]) + case _: self.logger.error( - f"non-numeric VicCAN data value: {value}; dropping message" + f"unexpected VicCAN data length: {data_len}; 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" + # command is limited to 6 bits. + command = int(msg.command_id) + if command < 0 or command > 0x3F: + self.logger.error( + f"invalid command_id for CAN frame: {command}; dropping message" ) - raw_bytes = raw_bytes[:8] + return try: can_message = can.Message( - arbitration_id=arbitration_id, - data=raw_bytes, + arbitration_id=(mcu_id << 8) | (data_type << 6) | command, + data=data, is_extended_id=False, ) except Exception as e: diff --git a/src/core_pkg/core_pkg/core_ptz.py b/src/core_pkg/core_pkg/core_ptz.py index eafcc74..3e853a4 100644 --- a/src/core_pkg/core_pkg/core_ptz.py +++ b/src/core_pkg/core_pkg/core_ptz.py @@ -262,7 +262,7 @@ class PtzNode(Node): f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}" ) self.debug_pub.publish(msg) - self.get_logger().info(message_text) + self.get_logger().debug(message_text) def run_async_func(self, coro): """Run an async function in the event loop.""" From 4ef226c09437d1285b997439a61c7ba329603851 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Wed, 1 Apr 2026 03:31:21 -0500 Subject: [PATCH 16/25] nix fmt --- src/anchor_pkg/anchor_pkg/connector.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 6c3bf0b..e9859bb 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -22,14 +22,14 @@ KNOWN_USBS = [ BAUD_RATE = 115200 MCU_IDS = [ - "broadcast", - "core", - "arm", - "digit", - "faerie", - "citadel", - "libs", - ] + "broadcast", + "core", + "arm", + "digit", + "faerie", + "citadel", + "libs", +] class NoValidDeviceException(Exception): @@ -374,7 +374,7 @@ class CANConnector(Connector): data = struct.pack(">ff", *msg.data) case 4: data_type = 2 - data = struct.pack(">hhhh", *[ int(x) for x in msg.data]) + data = struct.pack(">hhhh", *[int(x) for x in msg.data]) case _: self.logger.error( f"unexpected VicCAN data length: {data_len}; dropping message" From 89b31949145c78295ab77914328e57a1f1779615 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Thu, 2 Apr 2026 19:41:05 -0500 Subject: [PATCH 17/25] update documentation and accept 3-value VicCAN messages --- src/anchor_pkg/anchor_pkg/anchor_node.py | 5 ++++- src/anchor_pkg/anchor_pkg/connector.py | 4 +++- src/anchor_pkg/anchor_pkg/convert.py | 2 ++ 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 8edc8b5..b9cf992 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -42,6 +42,9 @@ class Anchor(Node): - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU * /anchor/to_vic/relay_string - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector) + * /anchor/relay + - Legacy method for talking to connectors. Takes String as input, but does not send the raw strings to connectors. + Instead, it converts them to VicCAN messages first. """ connector: Connector @@ -203,7 +206,7 @@ class Anchor(Node): self.connector.write(msg) self.tovic_debug_pub_.publish(msg) - @deprecated("Use /anchor/to_vic/relay instead of /anchor/relay") + @deprecated("Use /anchor/to_vic/relay or /anchor/to_vic/relay_string instead of /anchor/relay") def write_connector_legacy(self, msg: String): """Write to the connector by first attempting to convert String to VicCAN""" # please do not reference this code. ~riley diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index e9859bb..0e8fffe 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -372,8 +372,10 @@ class CANConnector(Connector): case 2: data_type = 1 data = struct.pack(">ff", *msg.data) - case 4: + case 3 | 4: # 3 gets padded and is treated as 4 data_type = 2 + # pad till we have 4 otherwise struct.pack will freak out + msg.data = (msg.data + [0])[:4] data = struct.pack(">hhhh", *[int(x) for x in msg.data]) case _: self.logger.error( diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index cfecfce..57b2c44 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -57,6 +57,8 @@ def string_to_viccan( def viccan_to_string(viccan: VicCAN) -> str: """Converts a ROS2 VicCAN message to the serial string VicCAN format.""" + # make sure we accept 3 digits and treat it as 4 + if len(viccan.data) == 3: viccan.data.append("0") # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max data = "".join([f",{round(val,7)}" for val in viccan.data]) return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n" From 410d3706ede25f506150224d25f171b39ee822dc Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Thu, 2 Apr 2026 19:49:07 -0500 Subject: [PATCH 18/25] update README with mock connector instructions --- README.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/README.md b/README.md index fbdf62d..fe84505 100644 --- a/README.md +++ b/README.md @@ -74,6 +74,19 @@ You can see all data sent to it in a string format with this command: $ ros2 topic echo /anchor/to_vic/debug ``` +To send data to it, use the normal topic: + +```bash +$ ros2 topic pub /anchor/to_vic/relay astra_msgs/msg/VicCAN '{mcu_name: "core", command_id: 50, data: [0.0, 2.0, 0.0, 1.0]}' + +``` + +To emulate receiving data from a microcontroller, publish to the dedicated topic: + +```bash +$ ros2 topic pub /anchor/from_vic/mock_mcu std_msgs/msg/String '{data: "can_relay_fromvic,arm,55,0.0,450.0,900.0,0.0"}' +``` + ### Testing Serial You can fake the presence of a Serial device (i.e., MCU) by using the following command: From bc9183d59acd88833f8cfdd49f2604f6536f8973 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Tue, 7 Apr 2026 21:52:52 -0500 Subject: [PATCH 19/25] make mock mcu use VicCAN messages --- README.md | 2 +- src/anchor_pkg/anchor_pkg/anchor_node.py | 13 +++---------- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index fe84505..b045857 100644 --- a/README.md +++ b/README.md @@ -84,7 +84,7 @@ $ ros2 topic pub /anchor/to_vic/relay astra_msgs/msg/VicCAN '{mcu_name: "core", To emulate receiving data from a microcontroller, publish to the dedicated topic: ```bash -$ ros2 topic pub /anchor/from_vic/mock_mcu std_msgs/msg/String '{data: "can_relay_fromvic,arm,55,0.0,450.0,900.0,0.0"}' +$ ros2 topic pub /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN '{mcu_name: "arm", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}' ``` ### Testing Serial diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index b9cf992..f831015 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -37,7 +37,7 @@ class Anchor(Node): Subscribers: * /anchor/from_vic/mock_mcu - - For testing without an actual MCU, publish strings here as if they came from an MCU + - For testing without an actual MCU, publish ViCAN messages here as if they came from an MCU * /anchor/to_vic/relay - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU * /anchor/to_vic/relay_string @@ -229,16 +229,9 @@ class Anchor(Node): elif msg.mcu_name == "citadel" or msg.mcu_name == "digit": self.fromvic_bio_pub_.publish(msg) - def on_mock_fromvic(self, msg: String): + def on_mock_fromvic(self, msg: VicCAN): """Relay a message as if it came from the MCU""" - viccan = string_to_viccan( - msg.data, - "mock", - self.get_logger(), - self.get_clock().now().to_msg(), - ) - if viccan: - self.relay_fromvic(viccan) + self.relay_fromvic(msg) def main(args=None): From ec7f2729348b9089073e8becb8f8e5be28f037ac Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Tue, 7 Apr 2026 22:16:08 -0500 Subject: [PATCH 20/25] clean up code --- src/anchor_pkg/anchor_pkg/anchor_node.py | 10 ++++------ src/anchor_pkg/anchor_pkg/connector.py | 2 +- src/anchor_pkg/anchor_pkg/convert.py | 3 ++- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index f831015..7c475a6 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -175,7 +175,7 @@ class Anchor(Node): self.mock_mcu_sub_ = self.create_subscription( String, "/anchor/from_vic/mock_mcu", - self.on_mock_fromvic, + self.relay_fromvic, 20, ) self.tovic_string_sub_ = self.create_subscription( @@ -206,7 +206,9 @@ class Anchor(Node): self.connector.write(msg) self.tovic_debug_pub_.publish(msg) - @deprecated("Use /anchor/to_vic/relay or /anchor/to_vic/relay_string instead of /anchor/relay") + @deprecated( + "Use /anchor/to_vic/relay or /anchor/to_vic/relay_string instead of /anchor/relay" + ) def write_connector_legacy(self, msg: String): """Write to the connector by first attempting to convert String to VicCAN""" # please do not reference this code. ~riley @@ -229,10 +231,6 @@ class Anchor(Node): elif msg.mcu_name == "citadel" or msg.mcu_name == "digit": self.fromvic_bio_pub_.publish(msg) - def on_mock_fromvic(self, msg: VicCAN): - """Relay a message as if it came from the MCU""" - self.relay_fromvic(msg) - def main(args=None): try: diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 0e8fffe..7241884 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -372,7 +372,7 @@ class CANConnector(Connector): case 2: data_type = 1 data = struct.pack(">ff", *msg.data) - case 3 | 4: # 3 gets padded and is treated as 4 + case 3 | 4: # 3 gets padded and is treated as 4 data_type = 2 # pad till we have 4 otherwise struct.pack will freak out msg.data = (msg.data + [0])[:4] diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index 57b2c44..4452497 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -58,7 +58,8 @@ def string_to_viccan( def viccan_to_string(viccan: VicCAN) -> str: """Converts a ROS2 VicCAN message to the serial string VicCAN format.""" # make sure we accept 3 digits and treat it as 4 - if len(viccan.data) == 3: viccan.data.append("0") + if len(viccan.data) == 3: + viccan.data.append("0") # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max data = "".join([f",{round(val,7)}" for val in viccan.data]) return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n" From b09b55bee06894a183ba82419f5c7bac2f9c5a84 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Tue, 7 Apr 2026 22:19:55 -0500 Subject: [PATCH 21/25] fix bug because apparently python has arrays --- src/anchor_pkg/anchor_pkg/connector.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 7241884..14a9065 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -375,7 +375,8 @@ class CANConnector(Connector): case 3 | 4: # 3 gets padded and is treated as 4 data_type = 2 # pad till we have 4 otherwise struct.pack will freak out - msg.data = (msg.data + [0])[:4] + msg.data.append(0) + msg.data = msg.data[:4] data = struct.pack(">hhhh", *[int(x) for x in msg.data]) case _: self.logger.error( From 0a257abf43c6360b992b751726b47356ce4a91f3 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Tue, 7 Apr 2026 23:44:38 -0500 Subject: [PATCH 22/25] make the pad 3 -> logic consistent --- src/anchor_pkg/anchor_pkg/connector.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 14a9065..d2a6bb5 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -372,10 +372,10 @@ class CANConnector(Connector): case 2: data_type = 1 data = struct.pack(">ff", *msg.data) - case 3 | 4: # 3 gets padded and is treated as 4 + case 3 | 4: # 3 gets treated as 4 data_type = 2 - # pad till we have 4 otherwise struct.pack will freak out - msg.data.append(0) + if data_len == 3: + msg.data.append(0) msg.data = msg.data[:4] data = struct.pack(">hhhh", *[int(x) for x in msg.data]) case _: From 010d2da0b6c9c0d93817716ad1db8f31fcd8058f Mon Sep 17 00:00:00 2001 From: David Date: Tue, 7 Apr 2026 23:45:40 -0500 Subject: [PATCH 23/25] fix: string number --- src/anchor_pkg/anchor_pkg/connector.py | 2 +- src/anchor_pkg/anchor_pkg/convert.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index d2a6bb5..e94a59c 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -257,7 +257,7 @@ class CANConnector(Connector): ) if self.can_channel and self.can_channel.startswith("v"): - self.logger.warn("likely using virtual CAN interface") + self.logger.warn("CAN interface is likely virtual") def read(self) -> tuple[VicCAN | None, str | None]: if not self.can_bus: diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index 4452497..bef3301 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -59,7 +59,7 @@ def viccan_to_string(viccan: VicCAN) -> str: """Converts a ROS2 VicCAN message to the serial string VicCAN format.""" # make sure we accept 3 digits and treat it as 4 if len(viccan.data) == 3: - viccan.data.append("0") + viccan.data.append(0) # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max data = "".join([f",{round(val,7)}" for val in viccan.data]) return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n" From 30bb32a66b6430502d921820970d72cde9e77ac3 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Wed, 8 Apr 2026 00:09:04 -0500 Subject: [PATCH 24/25] remove extraneous slice --- src/anchor_pkg/anchor_pkg/connector.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index e94a59c..0d4790b 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -376,7 +376,6 @@ class CANConnector(Connector): data_type = 2 if data_len == 3: msg.data.append(0) - msg.data = msg.data[:4] data = struct.pack(">hhhh", *[int(x) for x in msg.data]) case _: self.logger.error( From 88574524cfa1296100733016b6e2aecb2e587a92 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Wed, 8 Apr 2026 00:15:39 -0500 Subject: [PATCH 25/25] clarify the mock connector usage in the README --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index b045857..a8fba7e 100644 --- a/README.md +++ b/README.md @@ -68,20 +68,20 @@ Anchor provides a mock connector meant for testing and scripting purposes. You c $ ros2 launch anchor_pkg rover.launch.py connector:="mock" ``` -You can see all data sent to it in a string format with this command: +To see all data that would be sent over the CAN network (and thus to the microcontrollers), use this command: ```bash $ ros2 topic echo /anchor/to_vic/debug ``` -To send data to it, use the normal topic: +To send data to the mock connector (as if you were a ROS2 node), use the normal relay topic: ```bash $ ros2 topic pub /anchor/to_vic/relay astra_msgs/msg/VicCAN '{mcu_name: "core", command_id: 50, data: [0.0, 2.0, 0.0, 1.0]}' ``` -To emulate receiving data from a microcontroller, publish to the dedicated topic: +To send data to the mock connector (as if you were a microcontroller), publish to the dedicated topic: ```bash $ ros2 topic pub /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN '{mcu_name: "arm", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}'