fix a plethora of bugs related to the serial connector

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

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,

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"

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