Merge pull request #31 from SHC-ASTRA/can-refactor

Refactor anchor & add direct CAN connector

README.md

@@ -68,12 +68,25 @@ 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 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 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]}'
+```
+
 ### Testing Serial
 
 You can fake the presence of a Serial device (i.e., MCU) by using the following command:

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
@@ -37,11 +37,14 @@ 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
         - 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
@@ -163,10 +166,16 @@ 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",
-            self.on_mock_fromvic,
+            self.relay_fromvic,
             20,
         )
         self.tovic_string_sub_ = self.create_subscription(
@@ -195,7 +204,23 @@ 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 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
+        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"""
@@ -206,17 +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: 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)
-
 
 def main(args=None):
     try:

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
@@ -217,6 +227,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,
@@ -246,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:
@@ -268,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}"
             )
@@ -295,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])
+                (value,) = struct.unpack(">d", data_bytes[:8])
                 data = [float(value)]
             elif data_type_key == 1:
                 if len(data_bytes) < 8:
@@ -303,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])
+                v1, v2 = struct.unpack(">ff", data_bytes[:8])
                 data = [float(v1), float(v2)]
             elif data_type_key == 2:
                 if len(data_bytes) < 8:
@@ -311,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])
+                i1, i2, i3, i4 = struct.unpack(">hhhh", data_bytes[:8])
                 data = [float(i1), float(i2), float(i3), float(i4)]
             else:
                 self.logger.warn(
@@ -343,92 +346,55 @@ 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 3 | 4:  # 3 gets treated as 4
+                data_type = 2
+                if data_len == 3:
+                    msg.data.append(0)
+                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]"
+        # 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"
             )
-                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]
+            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:

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:
@@ -57,6 +57,9 @@ 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}{data}\n"
+    return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n"

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: '')",
         )
     )

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."""