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

Refactor anchor & add direct CAN connector

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]}'

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

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"