fix a handful of oversights in the anchor refactor. add a testing script

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,7 +1,7 @@
 from warnings import deprecated
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
+from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 
 import atexit
@@ -14,8 +14,7 @@ from .connector import (
     NoValidDeviceException,
     NoWorkingDeviceException,
 )
-from .convert import string_to_viccan
-import threading
+from .convert import string_to_viccan, viccan_to_string
 
 from astra_msgs.msg import VicCAN
 from std_msgs.msg import String
@@ -25,26 +24,25 @@ class Anchor(Node):
     """
     Publishers:
     * /anchor/from_vic/debug
-        - Every string received from the MCU is published here for debugging
+        - Every string received from the MCU is published here for debugging (String)
     * /anchor/from_vic/core
         - VicCAN messages for Core node
     * /anchor/from_vic/arm
-        - VicCAN messages for Arm node
+        - VicCAN messages for Arm node (also receives digit messages)
     * /anchor/from_vic/bio
-        - VicCAN messages for Bio node
+        - VicCAN messages for Bio node (also receives digit messages)
     * /anchor/to_vic/debug
-        - A string copy of the messages published to ./relay are published here
+        - String copy of all messages sent to the connector
 
     Subscribers:
     * /anchor/from_vic/mock_mcu
-        - For testing without an actual MCU, publish ViCAN messages here as if they came from an MCU
+        - For testing without an actual MCU, publish VicCAN 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)
+        - Send raw strings to connectors. Does not work for 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.
+        - (Deprecated) Legacy method. Takes String, converts to VicCAN, then sends to connector.
     """
 
     connector: Connector
@@ -152,9 +150,9 @@ class Anchor(Node):
             "/anchor/from_vic/bio",
             20,
         )
-        # Debug publisher
+        # Debug publisher for outgoing messages
         self.tovic_debug_pub_ = self.create_publisher(
-            VicCAN,
+            String,
             "/anchor/to_vic/debug",
             20,
         )
@@ -173,7 +171,7 @@ class Anchor(Node):
             20,
         )
         self.mock_mcu_sub_ = self.create_subscription(
-            String,
+            VicCAN,
             "/anchor/from_vic/mock_mcu",
             self.relay_fromvic,
             20,
@@ -181,10 +179,13 @@ class Anchor(Node):
         self.tovic_string_sub_ = self.create_subscription(
             String,
             "/anchor/to_vic/relay_string",
-            self.connector.write_raw,
+            self.write_connector_raw,
             20,
         )
 
+        # Timer to poll connector for incoming data at 100 Hz
+        self.read_timer_ = self.create_timer(0.01, self.read_connector)
+
         # Close devices on exit
         atexit.register(self.cleanup)
 
@@ -204,6 +205,11 @@ 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(String(data=viccan_to_string(msg)))
+
+    def write_connector_raw(self, msg: String):
+        """Write raw string to the connector and send a copy to /anchor/to_vic/debug"""
+        self.connector.write_raw(msg)
         self.tovic_debug_pub_.publish(msg)
 
     @deprecated(
@@ -226,25 +232,23 @@ class Anchor(Node):
         """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":
+        if 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":
+        if msg.mcu_name == "citadel" or msg.mcu_name == "digit":
             self.fromvic_bio_pub_.publish(msg)
 
 
 def main(args=None):
+    rclpy.init(args=args)
+    anchor_node = Anchor()
+    executor = MultiThreadedExecutor()
+    executor.add_node(anchor_node)
+
     try:
-        rclpy.init(args=args)
-        anchor_node = Anchor()
-
-        thread = threading.Thread(target=rclpy.spin, args=(anchor_node,), daemon=True)
-        thread.start()
-
-        rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
-        while rclpy.ok():
-            anchor_node.read_connector()  # Check the connector for updates
-            rate.sleep()
+        executor.spin()
     except (KeyboardInterrupt, ExternalShutdownException):
-        print("Caught shutdown signal, shutting down...")
+        pass
     finally:
+        executor.shutdown()
+        anchor_node.destroy_node()
         rclpy.try_shutdown()

src/anchor_pkg/anchor_pkg/connector.py

@@ -1,5 +1,6 @@
 from abc import ABC, abstractmethod
 from astra_msgs.msg import VicCAN
+from std_msgs.msg import String
 from rclpy.clock import Clock
 from rclpy.impl.rcutils_logger import RcutilsLogger
 from .convert import string_to_viccan as _string_to_viccan, viccan_to_string
@@ -77,7 +78,7 @@ class Connector(ABC):
         pass
 
     @abstractmethod
-    def write_raw(self, msg: str):
+    def write_raw(self, msg: String):
         pass
 
     def cleanup(self):
@@ -199,10 +200,10 @@ class SerialConnector(Connector):
             return (None, None)  # pretty much no other error matters
 
     def write(self, msg: VicCAN):
-        self.write_raw(viccan_to_string(msg))
+        self.write_raw(String(data=viccan_to_string(msg)))
 
-    def write_raw(self, msg: str):
-        self.serial_interface.write(bytes(msg, "utf8"))
+    def write_raw(self, msg: String):
+        self.serial_interface.write(bytes(msg.data, "utf8"))
 
     def cleanup(self):
         self.logger.info(f"closing serial port if open {self.port}")
@@ -411,8 +412,10 @@ class CANConnector(Connector):
             self.logger.error(f"CAN error while sending: {e}")
             raise DeviceClosedException("CAN bus closed unexpectedly")
 
-    def write_raw(self, msg: str):
-        self.logger.warn(f"write_raw is not supported for CANConnector. msg: {msg}")
+    def write_raw(self, msg: String):
+        self.logger.warn(
+            f"write_raw is not supported for CANConnector. msg: {msg.data}"
+        )
 
     def cleanup(self):
         try:
@@ -434,5 +437,5 @@ class MockConnector(Connector):
     def write(self, msg: VicCAN):
         pass
 
-    def write_raw(self, msg: str):
+    def write_raw(self, msg: String):
         pass