chore(anchor): use new version commit hash field name

Refactor anchor & add direct CAN connector

README.md

@@ -68,23 +68,23 @@ 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 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

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,4 +1,5 @@
 from warnings import deprecated
+import time
 import rclpy
 from rclpy.node import Node
 from rclpy.executors import ExternalShutdownException
@@ -17,8 +18,9 @@ from .connector import (
 from .convert import string_to_viccan
 import threading
 
-from astra_msgs.msg import VicCAN
+from builtin_interfaces.msg import Time
 from std_msgs.msg import String
+from astra_msgs.msg import VicCAN, McuVersion
 
 
 class Anchor(Node):
@@ -37,7 +39,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
@@ -47,6 +49,8 @@ class Anchor(Node):
           Instead, it converts them to VicCAN messages first.
     """
 
+    ASTRA_EPOCH = time.struct_time((2022, 1, 1, 0, 0, 0, 0, 0, 0))  # January 1, 2022
+
     connector: Connector
 
     def __init__(self):
@@ -158,6 +162,12 @@ class Anchor(Node):
             "/anchor/to_vic/debug",
             20,
         )
+        # MCU Version publisher
+        self.mcu_version_pub_ = self.create_publisher(
+            McuVersion,
+            "/anchor/from_vic/mcu_version",
+            20,
+        )
 
         # Subscribers
         self.tovic_sub_ = self.create_subscription(
@@ -173,9 +183,9 @@ class Anchor(Node):
             20,
         )
         self.mock_mcu_sub_ = self.create_subscription(
-            String,
+            VicCAN,
             "/anchor/from_vic/mock_mcu",
-            self.on_mock_fromvic,
+            self.relay_fromvic,
             20,
         )
         self.tovic_string_sub_ = self.create_subscription(
@@ -185,6 +195,8 @@ class Anchor(Node):
             20,
         )
 
+        self.mcu_versions: dict[str, McuVersion] = {}
+
         # Close devices on exit
         atexit.register(self.cleanup)
 
@@ -206,7 +218,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,16 +243,25 @@ 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):
+        # MCU Versioning information
-        """Relay a message as if it came from the MCU"""
+        if msg.command_id in (46, 47) and msg.mcu_name not in self.mcu_versions:
-        viccan = string_to_viccan(
+            self.mcu_versions[msg.mcu_name] = McuVersion(mcu_name=msg.mcu_name)
-            msg.data,
+
-            "mock",
+        if msg.command_id == 46:  # commit hashes
-            self.get_logger(),
+            self.mcu_versions[msg.mcu_name].project_commit_fragment = msg.data[0]
-            self.get_clock().now().to_msg(),
+            self.mcu_versions[msg.mcu_name].astra_lib_commit_fragment = msg.data[1]
-        )
+        elif msg.command_id == 47:  # build timestamp and version numbers
-        if viccan:
+            version_msg = self.mcu_versions[msg.mcu_name]
-            self.relay_fromvic(viccan)
+            version_msg.build_time = Time(
+                sec=int(time.mktime(self.ASTRA_EPOCH) + msg.data[0])
+            )
+            # is_main and is_dirty is in msg.data[1]
+            # Out of 1 byte, it looks like [lib_isdirty][lib_ismain][proj_isdirty][proj_ismain]
+            version_msg.astra_lib_is_dirty = bool(int(msg.data[1]) >> 3 & 0x1)
+            version_msg.astra_lib_is_main = bool(int(msg.data[1]) >> 2 & 0x1)
+            version_msg.project_is_dirty = bool(int(msg.data[1]) >> 1 & 0x1)
+            version_msg.project_is_main = bool(int(msg.data[1]) & 0x1)
+            self.mcu_version_pub_.publish(version_msg)
 
 
 def main(args=None):

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:
@@ -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
+                if data_len == 3:
-                msg.data = (msg.data + [0])[:4]
+                    msg.data.append(0)
                 data = struct.pack(">hhhh", *[int(x) for x in msg.data])
             case _:
                 self.logger.error(

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"