refactor(anchor): buffer bytes instead of string

Resolves Riley's comments, makes it so UTF-8 characters over 1 byte aren't destroyed

README.md

@@ -60,6 +60,33 @@ $ ros2 launch anchor_pkg rover.launch.py  # Must be run on a computer connected
 $ ros2 run headless_pkg headless_full  # Optionally run in a separate shell on the same or different computer.
 ```
 
+### Using the Mock Connector
+
+Anchor provides a mock connector meant for testing and scripting purposes. You can select the mock connector by running anchor with this command:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:="mock"
+```
+
+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:
@@ -68,10 +95,31 @@ You can fake the presence of a Serial device (i.e., MCU) by using the following
 $ socat -dd -v pty,rawer,crnl,link=/tmp/ttyACM9 pty,rawer,crnl,link=/tmp/ttyOUT
 ```
 
-When you go to run anchor, use the `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so:
+When you go to run anchor, use the `serial_override` ROS2 parameter to point it to the fake serial port, like so:
 
 ```bash
-$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
+$ ros2 launch anchor_pkg rover.launch.py connector:=serial serial_override:=/tmp/ttyACM9
+```
+
+### Testing CAN
+
+You can create a virtual CAN network by using the following commands to create and then enable it:
+
+```bash
+sudo ip link add dev vcan0 type vcan
+sudo ip link set vcan0 up
+```
+
+When you go to run anchor, use the `can_override` ROS2 parameter to point it to the virtual CAN network, like so:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:=can can_override:=vcan0
+```
+
+Once you're done, you should delete the virtual network so that anchor doesn't get confused if you plug in a real CAN adapter:
+
+```bash
+$ sudo ip link delete vcan0
 ```
 
 ### Connecting the GuliKit Controller

auto_start/auto_start_headless_full.sh

@@ -26,4 +26,3 @@ source $SCRIPT_DIR/../install/setup.bash
 
 # Launch the ROS 2 node
 ros2 run headless_pkg headless_full
-

auto_start/start_rosbag.sh

@@ -3,10 +3,10 @@ set -e
 
 SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &>/dev/null && pwd)
 
-[[ -z "$ANCHOR_WS" ]] && ANCHOR_WS="$SCRIPT_DIR/.."
-[[ -z "$AUTONOMY_WS" ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
+[[ -z $ANCHOR_WS ]] && ANCHOR_WS="$SCRIPT_DIR/.."
+[[ -z $AUTONOMY_WS ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
 BAG_LOCATION="$HOME/bags/autostart"
-[[ ! -d "$BAG_LOCATION" ]] && mkdir -p "$BAG_LOCATION"
+[[ ! -d $BAG_LOCATION ]] && mkdir -p "$BAG_LOCATION"
 
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
@@ -16,7 +16,6 @@ done
 
 echo "[INFO] Network interface is up!"
 
-
 if command -v nixos-rebuild; then
   echo "[INFO] running on NixOS"
 else

flake.lock

@@ -24,16 +24,16 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1770108954,
-        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
+        "lastModified": 1775216071,
+        "narHash": "sha256-PrPW70Fh1uLx3JxNV/NLeXjUhgfrZmi7ac8LJOlS0q4=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
+        "rev": "197a2b55c4ed24f8b885a5b20b65f426fb6d57ca",
         "type": "github"
       },
       "original": {
         "owner": "lopsided98",
-        "ref": "develop",
+        "ref": "master",
         "repo": "nix-ros-overlay",
         "type": "github"
       }

flake.nix

@@ -2,7 +2,7 @@
   description = "Development environment for ASTRA Anchor";
 
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
 
     treefmt-nix = {
@@ -31,6 +31,8 @@
           name = "ASTRA Anchor";
           packages = with pkgs; [
             colcon
+            socat
+            can-utils
             (python313.withPackages (
               p: with p; [
                 pyserial
@@ -96,7 +98,8 @@
     );
 
   nixConfig = {
-    extra-substituters = [ "https://ros.cachix.org" ];
-    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
+    # Cache to pull ros packages from
+    extra-substituters = [ "https://ros.cachix.org" "https://attic.iid.ciirc.cvut.cz/ros" ];
+    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" "ros:JR95vUYsShSqfA1VTYoFt1Nz6uXasm5QrcOsGry9f6Q=" ];
   };
 }

scripts/reset-repo.bash

@@ -0,0 +1,38 @@
+#!/usr/bin/env bash
+
+repo_root="$(git rev-parse --show-toplevel)"
+
+if [[ -z $repo_root ]]; then
+  echo "script must be run from within a git repo" >&2
+  exit 1
+fi
+
+cd $repo_root
+
+echo "this will nuke all of your current un-commited git changes, including any changes to submodules and any gitignored files. is this okay? (y/N)"
+read okay
+
+if [[ $okay != "y" ]]; then
+  echo "you didn't say exactly 'y'. aborting." >&2
+  exit 2
+fi
+
+echo
+
+echo "ok say goodbye to everything in this repo"
+git submodule deinit --all -f && echo "- submodules gone"
+git clean -fdx && echo "- gitignored changes gone"
+git add -A
+git reset HEAD --hard && echo "- everything else gone"
+git submodule update --init --recursive && echo "- brought the submodules back"
+echo
+
+echo "in theory that should've done it. let's make sure"
+status=$(git status --porcelain)
+echo $status
+if [[ -z $status ]]; then
+  echo "nice, all clean!"
+else
+  echo "uhh that's not supposed to be there. this is probably a bug in this script. good luck!" >&2
+  exit 3
+fi

scripts/test-connectors.bash

@@ -0,0 +1,463 @@
+#!/usr/bin/env bash
+
+# test script for anchor connectors (mock, serial, CAN)
+
+set -o pipefail
+
+repo_root="$(git rev-parse --show-toplevel)"
+
+if [[ -z $repo_root ]]; then
+  echo "script must be run from within the rover-ros2 repo" >&2
+  exit 1
+fi
+
+cd "$repo_root"
+
+# colors
+BOLD='\033[1m'
+RED='\033[1;31m'
+GREEN='\033[1;32m'
+YELLOW='\033[1;33m'
+NC='\033[0m'
+
+TESTS_PASSED=0
+TESTS_FAILED=0
+
+log() {
+  echo -e "${BOLD}${YELLOW}info:${NC} ${1}"
+}
+
+pass() {
+  echo -e "${BOLD}${GREEN}pass:${NC} ${1}"
+  TESTS_PASSED=$((TESTS_PASSED + 1))
+}
+
+fail() {
+  echo -e "${BOLD}${RED}fail:${NC} ${1}"
+  TESTS_FAILED=$((TESTS_FAILED + 1))
+}
+
+cleanup() {
+  log "cleaning up"
+  if [[ -n $ANCHOR_PID ]]; then
+    kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
+    wait "$ANCHOR_PID" 2>/dev/null || true
+  fi
+  if [[ -n $SOCAT_PID ]]; then
+    kill -INT "$SOCAT_PID" 2>/dev/null || true
+    wait "$SOCAT_PID" 2>/dev/null || true
+  fi
+  rm -f /tmp/ttyACM9 /tmp/ttyOUT 2>/dev/null || true
+}
+
+trap cleanup EXIT
+
+source_ros2() {
+  source install/setup.bash
+}
+
+wait_for_topic() {
+  local topic="$1"
+  local timeout="${2:-5}"
+  local count=0
+  while ! ros2 topic list 2>/dev/null | grep -q "^${topic}$"; do
+    sleep 0.5
+    count=$((count + 1))
+    if [[ $count -ge $((timeout * 2)) ]]; then
+      return 1
+    fi
+  done
+  return 0
+}
+
+# run a ROS pub/echo test
+# usage: ros_pubsub_test <echo_topic> <pub_topic> <msg_type> <msg_data>
+# returns the echo output via stdout
+ros_pubsub_test() {
+  local echo_topic="$1"
+  local pub_topic="$2"
+  local msg_type="$3"
+  local msg_data="$4"
+
+  timeout 5 bash -c "
+    ros2 topic echo --once $echo_topic &
+    ECHO_PID=\$!
+    sleep 0.5
+    ros2 topic pub --once $pub_topic $msg_type \"$msg_data\" >/dev/null 2>&1
+    wait \$ECHO_PID
+  " 2>/dev/null || true
+}
+
+test_mock_connector() {
+  log "testing mock connector"
+
+  log "starting anchor with mock connector"
+  setsid ros2 run anchor_pkg anchor --ros-args -p connector:=mock &
+  ANCHOR_PID=$!
+  sleep 2
+
+  if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
+    fail "mock connector: anchor failed to start"
+    return 1
+  fi
+
+  if ! wait_for_topic "/anchor/to_vic/relay" 10; then
+    fail "mock connector: topics not available"
+    kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
+    return 1
+  fi
+
+  log "anchor started successfully"
+
+  # test: relay -> debug
+  log "testing relay -> debug"
+  local output
+  output=$(ros_pubsub_test "/anchor/to_vic/debug" "/anchor/to_vic/relay" \
+    "astra_msgs/msg/VicCAN" '{mcu_name: \"core\", command_id: 50, data: [1.0, 2.0, 3.0, 4.0]}')
+
+  if [[ -n $output ]] && echo "$output" | grep -q "can_relay_tovic,core,50"; then
+    pass "mock connector: relay -> debug"
+  else
+    fail "mock connector: relay -> debug"
+  fi
+
+  # test: mock_mcu -> from_vic/core
+  log "testing mock_mcu (core) -> from_vic/core"
+  output=$(ros_pubsub_test "/anchor/from_vic/core" "/anchor/from_vic/mock_mcu" \
+    "astra_msgs/msg/VicCAN" '{mcu_name: \"core\", command_id: 10, data: [100.0, 200.0]}')
+
+  if [[ -n $output ]] && echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 10"; then
+    pass "mock connector: mock_mcu -> from_vic/core"
+  else
+    fail "mock connector: mock_mcu -> from_vic/core"
+  fi
+
+  # test: mock_mcu -> from_vic/arm
+  log "testing mock_mcu (arm) -> from_vic/arm"
+  output=$(ros_pubsub_test "/anchor/from_vic/arm" "/anchor/from_vic/mock_mcu" \
+    "astra_msgs/msg/VicCAN" '{mcu_name: \"arm\", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}')
+
+  if [[ -n $output ]] && echo "$output" | grep -q "mcu_name: arm" && echo "$output" | grep -q "command_id: 55"; then
+    pass "mock connector: mock_mcu -> from_vic/arm"
+  else
+    fail "mock connector: mock_mcu -> from_vic/arm"
+  fi
+
+  # test: mock_mcu -> from_vic/bio
+  log "testing mock_mcu (citadel) -> from_vic/bio"
+  output=$(ros_pubsub_test "/anchor/from_vic/bio" "/anchor/from_vic/mock_mcu" \
+    "astra_msgs/msg/VicCAN" '{mcu_name: \"citadel\", command_id: 20, data: [5.0]}')
+
+  if echo "$output" | grep -q "mcu_name: citadel" && echo "$output" | grep -q "command_id: 20"; then
+    pass "mock connector: mock_mcu -> from_vic/bio"
+  else
+    fail "mock connector: mock_mcu -> from_vic/bio"
+  fi
+
+  # test: relay_string -> debug
+  log "testing relay_string -> debug"
+  output=$(ros_pubsub_test "/anchor/to_vic/debug" "/anchor/to_vic/relay_string" \
+    "std_msgs/msg/String" '{data: \"test_raw_string_data\"}')
+
+  if [[ -n $output ]] && echo "$output" | grep -q "test_raw_string_data"; then
+    pass "mock connector: relay_string -> debug"
+  else
+    fail "mock connector: relay_string -> debug"
+  fi
+
+  kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
+  wait "$ANCHOR_PID" 2>/dev/null || true
+  ANCHOR_PID=""
+}
+
+test_serial_connector() {
+  log "testing serial connector"
+
+  log "creating virtual serial ports with socat"
+  socat pty,raw,echo=0,link=/tmp/ttyACM9 pty,raw,echo=0,link=/tmp/ttyOUT 2>/dev/null &
+  SOCAT_PID=$!
+  sleep 2
+
+  if ! kill -0 "$SOCAT_PID" 2>/dev/null; then
+    fail "serial connector: failed to create virtual serial ports"
+    return 1
+  fi
+
+  log "starting anchor with serial connector (override: /tmp/ttyACM9)"
+  setsid ros2 run anchor_pkg anchor --ros-args -p connector:=serial -p serial_override:=/tmp/ttyACM9 &
+  ANCHOR_PID=$!
+  sleep 2
+
+  if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
+    fail "serial connector: anchor failed to start"
+    kill -INT "$SOCAT_PID" 2>/dev/null || true
+    return 1
+  fi
+
+  if ! wait_for_topic "/anchor/to_vic/relay" 10; then
+    fail "serial connector: topics not available"
+    kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
+    kill -INT "$SOCAT_PID" 2>/dev/null || true
+    return 1
+  fi
+
+  pass "serial connector: anchor starts with virtual serial"
+
+  # test: relay -> serial output (VicCAN encoding)
+  log "testing relay -> serial output"
+
+  local serial_out_file
+  serial_out_file=$(mktemp)
+
+  # Start head first (blocks waiting for input), then publish
+  timeout 5 head -n1 /tmp/ttyOUT >"$serial_out_file" &
+  local head_pid=$!
+  sleep 0.3
+  ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN \
+    '{mcu_name: "core", command_id: 30, data: [1.0, 2.0, 3.0, 4.0]}' >/dev/null 2>&1
+  wait $head_pid 2>/dev/null || true
+
+  local serial_out
+  serial_out=$(cat "$serial_out_file")
+  rm -f "$serial_out_file"
+
+  if [[ -n $serial_out ]] && echo "$serial_out" | grep -q "can_relay_tovic,core,30"; then
+    pass "serial connector: relay -> serial output"
+  else
+    fail "serial connector: relay -> serial output (got: $serial_out)"
+  fi
+
+  # test: serial input -> from_vic/core
+  log "testing serial input -> from_vic/core"
+
+  local output
+  output=$(timeout 5 bash -c '
+    ros2 topic echo --once /anchor/from_vic/core &
+    ECHO_PID=$!
+    sleep 0.5
+    echo "can_relay_fromvic,core,15,10.0,20.0,30.0,40.0" > /tmp/ttyOUT
+    sleep 0.5
+    echo "can_relay_fromvic,core,15,10.0,20.0,30.0,40.0" > /tmp/ttyOUT
+    wait $ECHO_PID
+  ' 2>/dev/null) || true
+
+  if echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 15"; then
+    pass "serial connector: serial input -> from_vic/core"
+  else
+    fail "serial connector: serial input -> from_vic/core (got: $output)"
+  fi
+
+  # test: relay_string -> debug
+  log "testing relay_string -> debug"
+  output=$(ros_pubsub_test "/anchor/to_vic/debug" "/anchor/to_vic/relay_string" \
+    "std_msgs/msg/String" '{data: \"serial_test_string\"}')
+
+  if [[ -n $output ]] && echo "$output" | grep -q "serial_test_string"; then
+    pass "serial connector: relay_string -> debug"
+  else
+    fail "serial connector: relay_string -> debug"
+  fi
+
+  kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
+  wait "$ANCHOR_PID" 2>/dev/null || true
+  ANCHOR_PID=""
+  kill -INT "$SOCAT_PID" 2>/dev/null || true
+  wait "$SOCAT_PID" 2>/dev/null || true
+  SOCAT_PID=""
+}
+
+test_can_connector() {
+  log "testing CAN connector"
+
+  log "starting anchor with CAN connector (override: vcan0)"
+  setsid ros2 run anchor_pkg anchor --ros-args -p connector:=can -p can_override:=vcan0 &
+  ANCHOR_PID=$!
+  sleep 2
+
+  if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
+    fail "CAN connector: anchor failed to start"
+    return 1
+  fi
+
+  if ! wait_for_topic "/anchor/to_vic/relay" 10; then
+    fail "CAN connector: topics not available"
+    kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
+    return 1
+  fi
+
+  log "anchor started successfully"
+  sleep 1
+
+  # test: relay -> CAN bus
+  # core=1, int16x4=2, cmd=30 -> id = (1<<8)|(2<<6)|30 = 0x19E
+  log "testing relay -> CAN bus"
+
+  local output
+  output=$(timeout 8 bash -c '
+    candump -n 1 vcan0 &
+    DUMP_PID=$!
+    sleep 1
+    ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 30, data: [1, 2, 3, 4]}" >/dev/null 2>&1
+    sleep 0.5
+    ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 30, data: [1, 2, 3, 4]}" >/dev/null 2>&1
+    wait $DUMP_PID
+  ' 2>/dev/null) || true
+
+  if echo "$output" | grep -qi "19E"; then
+    pass "CAN connector: relay -> CAN bus"
+  else
+    fail "CAN connector: relay -> CAN bus (got: $output)"
+  fi
+
+  # test: CAN -> from_vic/core
+  log "testing CAN bus -> from_vic/core"
+
+  output=$(timeout 5 bash -c '
+    ros2 topic echo --once /anchor/from_vic/core &
+    ECHO_PID=$!
+    sleep 1
+    cansend vcan0 18F#000A0014001E0028
+    sleep 0.5
+    cansend vcan0 18F#000A0014001E0028
+    wait $ECHO_PID
+  ' 2>/dev/null) || true
+
+  if echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 15"; then
+    pass "CAN connector: CAN -> from_vic/core"
+  else
+    fail "CAN connector: CAN -> from_vic/core"
+  fi
+
+  # test: CAN -> from_vic/arm
+  log "testing CAN bus -> from_vic/arm"
+
+  output=$(timeout 5 bash -c '
+    ros2 topic echo --once /anchor/from_vic/arm &
+    ECHO_PID=$!
+    sleep 1
+    cansend vcan0 294#00640096012C01F4
+    sleep 0.5
+    cansend vcan0 294#00640096012C01F4
+    wait $ECHO_PID
+  ' 2>/dev/null) || true
+
+  if echo "$output" | grep -q "mcu_name: arm" && echo "$output" | grep -q "command_id: 20"; then
+    pass "CAN connector: CAN -> from_vic/arm"
+  else
+    fail "CAN connector: CAN -> from_vic/arm"
+  fi
+
+  # test: CAN double data type (data_type_key=0)
+  log "testing CAN double data type"
+
+  output=$(timeout 8 bash -c '
+    ros2 topic echo --once /anchor/from_vic/core &
+    ECHO_PID=$!
+    sleep 1
+    cansend vcan0 105#3FF0000000000000
+    sleep 0.5
+    cansend vcan0 105#3FF0000000000000
+    sleep 0.5
+    cansend vcan0 105#3FF0000000000000
+    wait $ECHO_PID
+  ' 2>/dev/null) || true
+
+  if echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 5"; then
+    pass "CAN connector: double data type"
+  else
+    fail "CAN connector: double data type"
+  fi
+
+  # test: CAN float32x2 data type (data_type_key=1)
+  log "testing CAN float32x2 data type"
+
+  output=$(timeout 8 bash -c '
+    ros2 topic echo --once /anchor/from_vic/core &
+    ECHO_PID=$!
+    sleep 1
+    cansend vcan0 14A#3F80000040000000
+    sleep 0.5
+    cansend vcan0 14A#3F80000040000000
+    sleep 0.5
+    cansend vcan0 14A#3F80000040000000
+    wait $ECHO_PID
+  ' 2>/dev/null) || true
+
+  if echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 10"; then
+    pass "CAN connector: float32x2 data type"
+  else
+    fail "CAN connector: float32x2 data type"
+  fi
+
+  kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
+  wait "$ANCHOR_PID" 2>/dev/null || true
+  ANCHOR_PID=""
+}
+
+check_prerequisites() {
+  log "checking prerequisites"
+  local missing=0
+
+  if [[ ! -f install/setup.bash ]]; then
+    fail "install/setup.bash not found; run 'colcon build --symlink-install' first"
+    missing=1
+  fi
+
+  if ! command -v socat &>/dev/null; then
+    fail "socat not found; install it or use 'nix develop'"
+    missing=1
+  fi
+
+  if ! command -v cansend &>/dev/null || ! command -v candump &>/dev/null; then
+    fail "can-utils (cansend/candump) not found; install it or use 'nix develop'"
+    missing=1
+  fi
+
+  if ! ip link show vcan0 &>/dev/null; then
+    fail "vcan0 interface not found"
+    log "  create it with:"
+    log "    sudo ip link add dev vcan0 type vcan"
+    log "    sudo ip link set vcan0 up"
+    missing=1
+  elif ! ip link show vcan0 | grep -q ",UP"; then
+    fail "vcan0 exists but is not UP"
+    log "  enable it with: sudo ip link set vcan0 up"
+    missing=1
+  fi
+
+  if [[ $missing -eq 1 ]]; then
+    echo ""
+    log "prerequisites not met"
+    exit 1
+  fi
+
+  log "all prerequisites met"
+}
+
+main() {
+  echo ""
+  log "anchor connector test suite"
+  echo ""
+
+  check_prerequisites
+
+  log "sourcing ROS2 workspace"
+  source_ros2
+
+  test_mock_connector
+  test_serial_connector
+  test_can_connector
+
+  echo ""
+  log "test summary"
+  echo -e "${BOLD}${GREEN}passed:${NC} $TESTS_PASSED"
+  echo -e "${BOLD}${RED}failed:${NC} $TESTS_FAILED"
+  echo ""
+
+  if [[ $TESTS_FAILED -gt 0 ]]; then
+    exit 1
+  fi
+  exit 0
+}
+
+main "$@"

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,11 +1,9 @@
+from warnings import deprecated
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
+from rclpy.executors import ExternalShutdownException, SingleThreadedExecutor
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 
-import signal
-import atexit
-
 from .connector import (
     Connector,
     MockConnector,
@@ -14,12 +12,10 @@ from .connector import (
     NoValidDeviceException,
     NoWorkingDeviceException,
 )
-from .convert import string_to_viccan
-import sys
-import threading
+from .convert import string_to_viccan, viccan_to_string
 
-from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
+from std_msgs.msg import String
 
 
 class Anchor(Node):
@@ -33,12 +29,19 @@ class Anchor(Node):
         - VicCAN messages for Arm node
     * /anchor/from_vic/bio
         - VicCAN messages for Bio node
+    * /anchor/to_vic/debug
+        - A string copy of the messages published to ./relay are published here
 
     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 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)
+    * /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
@@ -48,6 +51,8 @@ class Anchor(Node):
 
         logger = self.get_logger()
 
+        # ROS2 Parameter Setup
+
         self.declare_parameter(
             "connector",
             "auto",
@@ -59,115 +64,192 @@ class Anchor(Node):
             ),
         )
 
+        self.declare_parameter(
+            "can_override",
+            "",
+            ParameterDescriptor(
+                name="can_override",
+                description="Overrides which CAN channel will be used. Defaults to ''.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be a valid CAN network that shows up in `ip link show`.",
+            ),
+        )
+
+        self.declare_parameter(
+            "serial_override",
+            "",
+            ParameterDescriptor(
+                name="serial_override",
+                description="Overrides which serial port will be used. Defaults to ''.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be a valid path to a serial device file that shows up in `ls /dev/tty*`.",
+            ),
+        )
+
         # Determine which connector to use. Options are Mock, Serial, and CAN
         connector_select = (
             self.get_parameter("connector").get_parameter_value().string_value
         )
-
+        can_override = (
+            self.get_parameter("can_override").get_parameter_value().string_value
+        )
+        serial_override = (
+            self.get_parameter("serial_override").get_parameter_value().string_value
+        )
         match connector_select:
             case "serial":
                 logger.info("using serial connector")
-                self.connector = SerialConnector(self.get_logger())
+                self.connector = SerialConnector(
+                    logger, self.get_clock(), serial_override
+                )
             case "can":
                 logger.info("using CAN connector")
-                self.connector = CANConnector(self.get_logger())
+                self.connector = CANConnector(logger, self.get_clock(), can_override)
             case "mock":
                 logger.info("using mock connector")
-                self.connector = MockConnector(self.get_logger())
+                self.connector = MockConnector(logger, self.get_clock())
             case "auto":
                 logger.info("automatically determining connector")
                 try:
                     logger.info("trying CAN connector")
-                    self.connector = CANConnector(self.get_logger())
+                    self.connector = CANConnector(
+                        logger, self.get_clock(), can_override
+                    )
                 except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
                     logger.info("CAN connector failed, trying serial connector")
-                    self.connector = SerialConnector(self.get_logger())
+                    self.connector = SerialConnector(
+                        logger, self.get_clock(), serial_override
+                    )
             case _:
-                self.get_logger().fatal(
+                logger.fatal(
                     f"invalid value for connector parameter: {connector_select}"
                 )
                 exit(1)
 
-        # Close devices on exit
-        atexit.register(self.cleanup)
-
         # ROS2 Topic Setup
 
         # Publishers
-        self.fromvic_debug_pub_ = self.create_publisher(
-            String, "/anchor/from_vic/debug", 20
+        self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
+            String,
+            "/anchor/from_vic/debug",
+            20,
         )
         self.fromvic_core_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/core", 20
+            VicCAN,
+            "/anchor/from_vic/core",
+            20,
         )
         self.fromvic_arm_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/arm", 20
+            VicCAN,
+            "/anchor/from_vic/arm",
+            20,
         )
         self.fromvic_bio_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/bio", 20
+            VicCAN,
+            "/anchor/from_vic/bio",
+            20,
+        )
+        # Debug publisher
+        self.tovic_debug_pub_ = self.create_publisher(
+            String,
+            "/anchor/to_vic/debug",
+            20,
         )
 
         # Subscribers
         self.tovic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/to_vic/relay", self.connector.write, 20
+            VicCAN,
+            "/anchor/to_vic/relay",
+            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, 20
+            VicCAN,
+            "/anchor/from_vic/mock_mcu",
+            self.relay_fromvic,
+            20,
+        )
+        self.tovic_string_sub_ = self.create_subscription(
+            String,
+            "/anchor/to_vic/relay_string",
+            self.write_connector_raw,
+            20,
         )
 
-    def cleanup(self):
+        # poll at 100Hz for incoming data
+        self.read_timer_ = self.create_timer(0.01, self.read_connector)
+
+    def destroy_node(self):
+        self.get_logger().info("closing connector")
         self.connector.cleanup()
+        super().destroy_node()
 
-    def read_MCU(self):
-        """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
-        output = self.connector.read()
+    def read_connector(self):
+        """Check the connector for new data from the MCU, and publish string to appropriate topics"""
+        viccan, raw = self.connector.read()
 
-        if not output:
-            return
+        if raw:
+            self.fromvic_debug_pub_.publish(String(data=raw))
 
-        self.relay_fromvic(output)
-
-    def relay_fromvic(self, msg: VicCAN):
-        """Relay a string message from the MCU to the appropriate VicCAN topic"""
-        msg.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
-
-        if msg.mcu_name == "core":
-            self.fromvic_core_pub_.publish(msg)
-        elif 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":
-            self.fromvic_bio_pub_.publish(msg)
-
-    def on_mock_fromvic(self, msg: String):
-        viccan = string_to_viccan(
-            msg.data,
-            "mock",
-            self.get_logger(),
-        )
         if viccan:
             self.relay_fromvic(viccan)
 
+    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(
+        "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"""
+        if msg.mcu_name == "core":
+            self.fromvic_core_pub_.publish(msg)
+        if msg.mcu_name == "arm" or msg.mcu_name == "digit":
+            self.fromvic_arm_pub_.publish(msg)
+        if msg.mcu_name == "citadel" or msg.mcu_name == "digit":
+            self.fromvic_bio_pub_.publish(msg)
+
 
 def main(args=None):
-    try:
     rclpy.init(args=args)
     anchor_node = Anchor()
+    executor = SingleThreadedExecutor()
+    executor.add_node(anchor_node)
 
-        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_MCU()  # Check the MCU for updates
-            rate.sleep()
+    try:
+        executor.spin()
     except (KeyboardInterrupt, ExternalShutdownException):
-        print("Caught shutdown signal, shutting down...")
+        pass
     finally:
+        # don't accept any more jobs
+        executor.shutdown()
+        # make the node quit processing things
+        anchor_node.destroy_node()
+        # shut down everything else
         rclpy.try_shutdown()
-
-
-if __name__ == "__main__":
-    signal.signal(
-        signal.SIGTERM, lambda signum, frame: sys.exit(0)
-    )  # Catch termination signals and exit cleanly
-    main()

src/anchor_pkg/anchor_pkg/connector.py

@@ -1,7 +1,11 @@
 from abc import ABC, abstractmethod
+from time import monotonic
+from typing import TYPE_CHECKING
 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
+from .convert import string_to_viccan as _string_to_viccan, viccan_to_string
 
 # CAN
 import can
@@ -18,8 +22,21 @@ KNOWN_USBS = [
     (0x10C4, 0xEA60),  # DOIT ESP32 Devkit V1
     (0x1A86, 0x55D3),  # ESP32 S3 Development Board
 ]
+
 BAUD_RATE = 115200
 
+SERIAL_READ_TIMEOUT = 0.5  # seconds
+
+MCU_IDS = [
+    "broadcast",
+    "core",
+    "arm",
+    "digit",
+    "faerie",
+    "citadel",
+    "libs",
+]
+
 
 class NoValidDeviceException(Exception):
     pass
@@ -39,18 +56,36 @@ class DeviceClosedException(Exception):
 
 class Connector(ABC):
     logger: RcutilsLogger
+    clock: Clock
 
-    def __init__(self, logger: RcutilsLogger):
+    def string_to_viccan(self, msg: str, mcu_name: str):
+        """function currying so that we do not need to pass logger and clock every time"""
+        return _string_to_viccan(
+            msg,
+            mcu_name,
+            self.logger,
+            self.clock.now().to_msg(),
+        )
+
+    def __init__(self, logger: RcutilsLogger, clock: Clock):
         self.logger = logger
+        self.clock = clock
 
     @abstractmethod
-    def read(self) -> VicCAN | None:
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        """
+        Must return a tuple of (VicCAN, debug message or string repr of VicCAN)
+        """
         pass
 
     @abstractmethod
     def write(self, msg: VicCAN):
         pass
 
+    @abstractmethod
+    def write_raw(self, msg: String):
+        pass
+
     def cleanup(self):
         pass
 
@@ -59,12 +94,23 @@ class SerialConnector(Connector):
     port: str
     mcu_name: str
     serial_interface: serial.Serial
-    override: bool
 
-    def __init__(self, logger: RcutilsLogger):
-        super().__init__(logger)
+    def __init__(self, logger: RcutilsLogger, clock: Clock, serial_override: str = ""):
+        super().__init__(logger, clock)
 
         ports = self._find_ports()
+        mcu_name: str | None = None
+
+        # Serial buffering
+        self._serial_buffer: bytes = b""
+        self._last_read_time = monotonic()
+
+        if serial_override:
+            logger.warn(
+                f"using serial_override: `{serial_override}`! this will bypass several checks."
+            )
+            ports = [serial_override]
+            mcu_name = "override"
 
         if len(ports) <= 0:
             raise NoValidDeviceException("no valid serial device found")
@@ -75,7 +121,8 @@ class SerialConnector(Connector):
 
         # check each of our ports to make sure one of them is responding
         port = ports[0]
-        mcu_name = self._get_name(port)
+        # we might already have a name by now if we overrode earlier
+        mcu_name = mcu_name or self._get_name(port)
         if not mcu_name:
             raise NoWorkingDeviceException(
                 f"found {port}, but it did not respond with its name"
@@ -85,7 +132,7 @@ class SerialConnector(Connector):
         self.mcu_name = mcu_name
 
         # if we fail at this point, it should crash because we've already tested the port
-        self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=1)
+        self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=0)
 
     def _find_ports(self) -> list[str]:
         """
@@ -136,34 +183,93 @@ class SerialConnector(Connector):
                     )
 
             if serial_interface.is_open:
+                # turn relay mode off if it failed to respond with its name
+                serial_interface.write(b"can_relay_mode,off\n")
                 serial_interface.close()
         except serial.SerialException as e:
             self.logger.error(f"SerialException when asking for MCU name: {e}")
 
         return None
 
-    def read(self) -> VicCAN | None:
-        try:
-            raw = str(self.serial_interface.readline(), "utf8")
+    def _try_readline(self) -> str | None:
+        """Attempts to read a full string from the MCU without blocking.
 
-            if not raw:
+        When pyserial is used with 'timeout=0', reads are performed non-blocking.
+        When used with interface.readline(), this breaks the assumption that a returned
+        string will be a completed attempt by the MCU to send a string; it may be
+        cut off between the start of the string and the newline, removing information
+        and rendering the string(s) useless; thus, to get around the downside of readline()
+        not waiting for a newline while still not blocking, this function manually
+        implements a serial input buffer and newline timeout.
+
+        If readline() returns a non-empty string, send it if it ends with a newline
+        (readline() will not read past any newline); otherwise, save the read string.
+        This buffered string should be pre-pended to the next readline() result.
+
+        If readline() does not receive a non-empty string after the last non-newline-
+        terminated readline() result within the manual timeout, send the contents of the
+        buffer as if it ended with a newline, and clear the buffer.
+
+        Returns:
+            str: A hopefully-complete string read from the MCU via the serial interface.
+        """
+        if TYPE_CHECKING:
+            assert type(self.serial_interface) == serial.Serial
+
+        # Warn on buffer timeout, as the only scenarios that would trigger this are
+        # a microcontroller output that isn't newline-terminated (bad), or the MCU is
+        # hanging (also bad).
+        if (
+            self._serial_buffer
+            and (monotonic() - self._last_read_time) > SERIAL_READ_TIMEOUT
+        ):
+            self.logger.warn(
+                f"Serial buffer timeout, last received '{self._serial_buffer}'."
+            )
+            result = self._serial_buffer
+            self._serial_buffer = b""
+            self._last_read_time = monotonic()
+            return str(result, "utf8").strip()
+
+        # No try-except here so caller catches it instead.
+        raw = self.serial_interface.readline()
+
+        # Empty or whitespace-only string
+        if not raw or not raw.strip():
             return None
 
-            return string_to_viccan(raw, self.mcu_name, self.logger)
+        # Add to buffer or send finished buffer
+        if not (raw.endswith(b"\n") or raw.endswith(b"\r")):  # unfinished string
+            self._serial_buffer += raw
+            self._last_read_time = monotonic()
+            return None
+        else:
+            result = self._serial_buffer + raw
+            self._serial_buffer = b""
+            return str(result, "utf8").strip()
+
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        try:
+            raw = self._try_readline()
+
+            if not raw:
+                return (None, None)
+
+            return (
+                self.string_to_viccan(raw, self.mcu_name),
+                raw,
+            )
         except serial.SerialException as e:
             self.logger.error(f"SerialException: {e}")
             raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
-        except TypeError as e:
-            self.logger.error(f"TypeError: {e}")
-            raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
         except Exception:
-            pass  # pretty much no other error matters
+            return (None, None)  # pretty much no other error matters
 
     def write(self, msg: VicCAN):
-        # go from [ w, x, y, z ] -> "w,x,y,z" & round to 7 digits max
-        data = ",".join([str(round(x, 7)) for x in msg.data])
-        output = f"can_relay_tovic,{msg.mcu_name},{msg.command_id},{data}\n"
-        self.serial_interface.write(bytes(output, "utf8"))
+        self.write_raw(String(data=viccan_to_string(msg)))
+
+    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}")
@@ -175,17 +281,27 @@ class SerialConnector(Connector):
 
 
 class CANConnector(Connector):
-    def __init__(self, logger: RcutilsLogger):
-        super().__init__(logger)
+    def __init__(self, logger: RcutilsLogger, clock: Clock, can_override: str):
+        super().__init__(logger, clock)
 
         self.can_channel: str | None = None
         self.can_bus: can.BusABC | None = None
 
-        if self.can_channel is None:
         avail = can.interfaces.socketcan.SocketcanBus._detect_available_configs()
 
         if len(avail) == 0:
             raise NoValidDeviceException("no CAN interfaces found")
+
+        # 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,
+                    avail,
+                )
+            )
+
         if (l := len(avail)) > 1:
             channels = ", ".join(str(b.get("channel")) for b in avail)
             raise MultipleValidDevicesException(
@@ -208,9 +324,9 @@ class CANConnector(Connector):
             )
 
         if self.can_channel and self.can_channel.startswith("v"):
-            self.logger.warn("using virtual CAN interface; this is likely vcan*")
+            self.logger.warn("CAN interface is likely virtual")
 
-    def read(self) -> VicCAN | None:
+    def read(self) -> tuple[VicCAN | None, str | None]:
         if not self.can_bus:
             raise DeviceClosedException("CAN bus not initialized")
 
@@ -221,7 +337,7 @@ class CANConnector(Connector):
             raise DeviceClosedException("CAN bus closed unexpectedly")
 
         if message is None:
-            return None
+            return (None, None)
 
         arbitration_id = message.arbitration_id & 0x7FF
         data_bytes = bytes(message.data)
@@ -230,21 +346,13 @@ 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}"
             )
-            return None
+            return (None, None)
 
         data: list[float] = []
 
@@ -256,33 +364,33 @@ class CANConnector(Connector):
                     self.logger.warn(
                         f"received double payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
-                    return None
-                (value,) = struct.unpack("<d", data_bytes[:8])
+                    return (None, None)
+                (value,) = struct.unpack(">d", data_bytes[:8])
                 data = [float(value)]
             elif data_type_key == 1:
                 if len(data_bytes) < 8:
                     self.logger.warn(
                         f"received float32x2 payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
-                    return None
-                v1, v2 = struct.unpack("<ff", data_bytes[:8])
+                    return (None, None)
+                v1, v2 = struct.unpack(">ff", data_bytes[:8])
                 data = [float(v1), float(v2)]
             elif data_type_key == 2:
                 if len(data_bytes) < 8:
                     self.logger.warn(
                         f"received int16x4 payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
-                    return None
-                i1, i2, i3, i4 = struct.unpack("<hhhh", data_bytes[:8])
+                    return (None, None)
+                i1, i2, i3, i4 = struct.unpack(">hhhh", data_bytes[:8])
                 data = [float(i1), float(i2), float(i3), float(i4)]
             else:
                 self.logger.warn(
                     f"received CAN frame with unknown data_type_key {data_type_key}; id=0x{arbitration_id:X}"
                 )
-                return None
+                return (None, None)
         except struct.error as e:
             self.logger.error(f"error unpacking CAN payload: {e}")
-            return None
+            return (None, None)
 
         viccan = VicCAN(
             mcu_name=mcu_name,
@@ -295,98 +403,65 @@ class CANConnector(Connector):
             f"decoded as VicCAN(mcu_name={viccan.mcu_name}, command_id={viccan.command_id}, data={viccan.data})"
         )
 
-        return viccan
+        return (
+            viccan,
+            f"{viccan.mcu_name},{viccan.command_id},"
+            + ",".join(map(str, list(viccan.data))),
+        )
 
     def write(self, msg: VicCAN):
         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:
@@ -403,6 +478,11 @@ class CANConnector(Connector):
             self.logger.error(f"CAN error while sending: {e}")
             raise DeviceClosedException("CAN bus closed unexpectedly")
 
+    def write_raw(self, msg: String):
+        self.logger.warn(
+            f"write_raw is not supported for CANConnector. msg: {msg.data}"
+        )
+
     def cleanup(self):
         try:
             if self.can_bus is not None:
@@ -413,11 +493,15 @@ class CANConnector(Connector):
 
 
 class MockConnector(Connector):
-    def __init__(self, logger: RcutilsLogger):
-        super().__init__(logger)
+    def __init__(self, logger: RcutilsLogger, clock: Clock):
+        super().__init__(logger, clock)
+        # No hardware interface for MockConnector. Publish to `/anchor/from_vic/mock_mcu` instead.
 
-    def read(self) -> VicCAN | None:
-        return None
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        return (None, None)
 
     def write(self, msg: VicCAN):
-        print(msg)
+        pass
+
+    def write_raw(self, msg: String):
+        pass

src/anchor_pkg/anchor_pkg/convert.py

@@ -1,30 +1,22 @@
 from astra_msgs.msg import VicCAN
+from std_msgs.msg import Header
+from builtin_interfaces.msg import Time
 from rclpy.impl.rcutils_logger import RcutilsLogger
 
 
-def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger):
+def string_to_viccan(
+    msg: str, mcu_name: str, logger: RcutilsLogger, time: Time
+) -> VicCAN | None:
     """
     Converts the serial string VicCAN format to a ROS2 VicCAN message.
     Does not fill out the Header of the message.
-    On a failure, it will log at a debug level why it failed.
-
-    Parameters:
-      * msg: str
-        - The message in serial VicCAN format
-      * mcu_name: str
-        - The name of the MCU (e.g. core, citadel, arm)
-      * logger: RcutilsLogger
-        - A logger retrieved from node.get_logger()
-
-    Returns:
-      * VicCAN | None
-        - The VicCAN message on a success or None on a failure
+    On a failure, it will log at a debug level why it failed and return None.
     """
 
-    parts: list[str] = msg.split(",")
+    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:
@@ -34,8 +26,40 @@ def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger):
         logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}")
         return None
 
+    try:
+        command_id = int(parts[2])
+    except ValueError:
+        logger.debug(
+            f"got garbage (non-integer command id) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+
+    if command_id not in range(64):
+        logger.debug(
+            f"got garbage (wrong command id {command_id}) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+
+    try:
         return VicCAN(
+            header=Header(
+                stamp=time,
+                frame_id="from_vic",
+            ),
             mcu_name=parts[1],
-        command_id=int(parts[2]),
+            command_id=command_id,
             data=[float(x) for x in parts[3:]],
         )
+    except ValueError:
+        logger.debug(f"got garbage (non-numerical) CAN data from {mcu_name}: {msg}")
+        return None
+
+
+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},{viccan.command_id}{data}\n"

src/anchor_pkg/launch/rover.launch.py

@@ -7,6 +7,8 @@ from launch_ros.actions import Node
 
 def generate_launch_description():
     connector = LaunchConfiguration("connector")
+    serial_override = LaunchConfiguration("serial_override")
+    can_override = LaunchConfiguration("can_override")
     use_ptz = LaunchConfiguration("use_ptz")
 
     ld = LaunchDescription()
@@ -16,7 +18,23 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "connector",
             default_value="auto",
-            description="Connector parameter for anchor node (default: auto)",
+            description="Connector parameter for anchor node (default: 'auto')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "serial_override",
+            default_value="",
+            description="Serial port override parameter for anchor node (default: '')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "can_override",
+            default_value="",
+            description="CAN network override parameter for anchor node (default: '')",
         )
     )
 
@@ -82,6 +100,8 @@ def generate_launch_description():
                 {
                     "launch_mode": "anchor",
                     "connector": connector,
+                    "serial_override": serial_override,
+                    "can_override": can_override,
                 }
             ],
             on_exit=Shutdown(),

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

treefmt.nix

@@ -1,8 +1,9 @@
-{ pkgs, ... }:
+{ ... }:
 {
   projectRootFile = "flake.nix";
   programs = {
     nixfmt.enable = true;
     black.enable = true;
+    shfmt.enable = true;
   };
 }