refactor: (headless) finish integrating Core cmd_vel

README.md

@@ -60,33 +60,6 @@ $ 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:
@@ -95,31 +68,10 @@ 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 `serial_override` ROS2 parameter to point it to the fake serial port, like so:
+When you go to run anchor, use the `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so:
 
 ```bash
-$ ros2 launch anchor_pkg rover.launch.py connector:=serial serial_override:=/tmp/ttyACM9
+$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
-```
-
-### 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_anchor.sh

@@ -1,12 +1,12 @@
 #!/usr/bin/env bash
 set -e
 
-SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &>/dev/null && pwd)
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
 
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
-  echo "[INFO] Waiting for active network interface..."
+    echo "[INFO] Waiting for active network interface..."
-  sleep 1
+    sleep 1
 done
 
 echo "[INFO] Network interface is up!"
@@ -16,9 +16,9 @@ echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
 if command -v nixos-rebuild; then
-  echo "[INFO] running on NixOS"
+    echo "[INFO] running on NixOS"
 else
-  source /opt/ros/humble/setup.bash
+    source /opt/ros/humble/setup.bash
 fi
 
 # Source your workspace setup script

auto_start/auto_start_headless_full.sh

@@ -1,12 +1,12 @@
 #!/usr/bin/env bash
 set -e
 
-SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &>/dev/null && pwd)
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
 
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
-  echo "[INFO] Waiting for active network interface..."
+    echo "[INFO] Waiting for active network interface..."
-  sleep 1
+    sleep 1
 done
 
 echo "[INFO] Network interface is up!"
@@ -16,9 +16,9 @@ echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
 if command -v nixos-rebuild; then
-  echo "[INFO] running on NixOS"
+    echo "[INFO] running on NixOS"
 else
-  source /opt/ros/humble/setup.bash
+    source /opt/ros/humble/setup.bash
 fi
 
 # Source your workspace setup script
@@ -26,3 +26,4 @@ source $SCRIPT_DIR/../install/setup.bash
 
 # Launch the ROS 2 node
 ros2 run headless_pkg headless_full
+

auto_start/start_rosbag.sh

@@ -1,25 +1,26 @@
 #!/usr/bin/env bash
 set -e
 
-SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &>/dev/null && pwd)
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
 
-[[ -z $ANCHOR_WS ]] && ANCHOR_WS="$SCRIPT_DIR/.."
+[[ -z "$ANCHOR_WS" ]] && ANCHOR_WS="$SCRIPT_DIR/.."
-[[ -z $AUTONOMY_WS ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
+[[ -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
-  echo "[INFO] Waiting for active network interface..."
+    echo "[INFO] Waiting for active network interface..."
-  sleep 1
+    sleep 1
 done
 
 echo "[INFO] Network interface is up!"
 
+
 if command -v nixos-rebuild; then
-  echo "[INFO] running on NixOS"
+    echo "[INFO] running on NixOS"
 else
-  source /opt/ros/humble/setup.bash
+    source /opt/ros/humble/setup.bash
 fi
 source $ANCHOR_WS/install/setup.bash
 [[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash

flake.lock

@@ -24,16 +24,16 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1775216071,
+        "lastModified": 1770108954,
-        "narHash": "sha256-PrPW70Fh1uLx3JxNV/NLeXjUhgfrZmi7ac8LJOlS0q4=",
+        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "197a2b55c4ed24f8b885a5b20b65f426fb6d57ca",
+        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
         "type": "github"
       },
       "original": {
         "owner": "lopsided98",
-        "ref": "master",
+        "ref": "develop",
         "repo": "nix-ros-overlay",
         "type": "github"
       }
@@ -60,8 +60,7 @@
         "nixpkgs": [
           "nix-ros-overlay",
           "nixpkgs"
-        ],
+        ]
-        "treefmt-nix": "treefmt-nix"
       }
     },
     "systems": {
@@ -78,26 +77,6 @@
         "repo": "default",
         "type": "github"
       }
-    },
-    "treefmt-nix": {
-      "inputs": {
-        "nixpkgs": [
-          "nixpkgs"
-        ]
-      },
-      "locked": {
-        "lastModified": 1773297127,
-        "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=",
-        "owner": "numtide",
-        "repo": "treefmt-nix",
-        "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016",
-        "type": "github"
-      },
-      "original": {
-        "owner": "numtide",
-        "repo": "treefmt-nix",
-        "type": "github"
-      }
     }
   },
   "root": "root",

flake.nix

@@ -2,13 +2,8 @@
   description = "Development environment for ASTRA Anchor";
 
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
-
-    treefmt-nix = {
-      url = "github:numtide/treefmt-nix";
-      inputs.nixpkgs.follows = "nixpkgs";
-    };
   };
 
   outputs =
@@ -16,8 +11,7 @@
       self,
       nix-ros-overlay,
       nixpkgs,
-      ...
+    }:
-    }@inputs:
     nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
       system:
       let
@@ -31,12 +25,9 @@
           name = "ASTRA Anchor";
           packages = with pkgs; [
             colcon
-            socat
-            can-utils
             (python313.withPackages (
               p: with p; [
                 pyserial
-                python-can
                 pygame
                 scipy
                 crccheck
@@ -70,7 +61,7 @@
                   moveit-msgs
                   moveit-ros-planning
                   moveit-ros-planning-interface
-                  moveit-ros-visualization
+		  moveit-ros-visualization
                   moveit-configs-utils
                   moveit-ros-move-group
                   moveit-servo
@@ -81,7 +72,7 @@
                   ompl
                   joy
                   ros2-controllers
-                  chomp-motion-planner
+		  chomp-motion-planner
                 ];
               }
             )
@@ -92,14 +83,11 @@
             export QT_X11_NO_MITSHM=1
           '';
         };
-
-        formatter = (inputs.treefmt-nix.lib.evalModule pkgs ./treefmt.nix).config.build.wrapper;
       }
     );
 
   nixConfig = {
-    # Cache to pull ros packages from
+    extra-substituters = [ "https://ros.cachix.org" ];
-    extra-substituters = [ "https://ros.cachix.org" "https://attic.iid.ciirc.cvut.cz/ros" ];
+    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
-    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" "ros:JR95vUYsShSqfA1VTYoFt1Nz6uXasm5QrcOsGry9f6Q=" ];
   };
 }

scripts/reset-repo.bash

@@ -1,38 +0,0 @@
-#!/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

@@ -1,463 +0,0 @@
-#!/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,21 +1,28 @@
-from warnings import deprecated
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException, SingleThreadedExecutor
+from rclpy.executors import ExternalShutdownException
-from rcl_interfaces.msg import ParameterDescriptor, ParameterType
+from std_srvs.srv import Empty
 
-from .connector import (
+import signal
-    Connector,
+import time
-    MockConnector,
+import atexit
-    SerialConnector,
-    CANConnector,
-    NoValidDeviceException,
-    NoWorkingDeviceException,
-)
-from .convert import string_to_viccan, viccan_to_string
 
+import serial
+import serial.tools.list_ports
+import os
+import sys
+import threading
+import glob
+
+from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
-from std_msgs.msg import String
+
+KNOWN_USBS = [
+    (0x2E8A, 0x00C0),  # Raspberry Pi Pico
+    (0x1A86, 0x55D4),  # Adafruit Feather ESP32 V2
+    (0x10C4, 0xEA60),  # DOIT ESP32 Devkit V1
+    (0x1A86, 0x55D3),  # ESP32 S3 Development Board
+]
 
 
 class Anchor(Node):
@@ -29,227 +36,330 @@ 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 VicCAN messages here as if they came from an MCU
+        - For testing without an actual MCU, publish strings 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)
+        - Publish raw strings to this topic to send directly to the MCU for debugging
-    * /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
-
     def __init__(self):
-        super().__init__("anchor_node")
+        # Initalize node with name
+        super().__init__("anchor_node")  # previously 'serial_publisher'
 
-        logger = self.get_logger()
+        self.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
 
-        # ROS2 Parameter Setup
+        # Serial port override
+        if port_override := os.getenv("PORT_OVERRIDE"):
+            self.serial_port = port_override
 
-        self.declare_parameter(
+        ##################################################
-            "connector",
+        # Serial MCU Discovery
-            "auto",
-            ParameterDescriptor(
-                name="connector",
-                description="Declares which MCU connector should be used. Defaults to 'auto'.",
-                type=ParameterType.PARAMETER_STRING,
-                additional_constraints="Must be 'serial', 'can', 'mock', or 'auto'.",
-            ),
-        )
 
-        self.declare_parameter(
+        # If there was not a port override, look for a MCU over USB for Serial.
-            "can_override",
+        if self.serial_port is None:
-            "",
+            comports = serial.tools.list_ports.comports()
-            ParameterDescriptor(
+            real_ports = list(
-                name="can_override",
+                filter(
-                description="Overrides which CAN channel will be used. Defaults to ''.",
+                    lambda p: p.vid is not None
-                type=ParameterType.PARAMETER_STRING,
+                    and p.pid is not None
-                additional_constraints="Must be a valid CAN network that shows up in `ip link show`.",
+                    and p.device is not None,
-            ),
+                    comports,
-        )
-
-        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(
-                    logger, self.get_clock(), serial_override
                 )
-            case "can":
+            )
-                logger.info("using CAN connector")
+            recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
-                self.connector = CANConnector(logger, self.get_clock(), can_override)
-            case "mock":
-                logger.info("using mock connector")
-                self.connector = MockConnector(logger, self.get_clock())
-            case "auto":
-                logger.info("automatically determining connector")
-                try:
-                    logger.info("trying CAN connector")
-                    self.connector = CANConnector(
-                        logger, self.get_clock(), can_override
-                    )
-                except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
-                    logger.info("CAN connector failed, trying serial connector")
-                    self.connector = SerialConnector(
-                        logger, self.get_clock(), serial_override
-                    )
-            case _:
-                logger.fatal(
-                    f"invalid value for connector parameter: {connector_select}"
-                )
-                exit(1)
 
+            if len(recog_ports) == 1:  # Found singular recognized MCU
+                found_port = recog_ports[0]
+                self.get_logger().info(
+                    f"Selecting MCU '{found_port.description}' at {found_port.device}."
+                )
+                self.serial_port = found_port.device  # String, location of device file; e.g., '/dev/ttyACM0'
+            elif len(recog_ports) > 1:  # Found multiple recognized MCUs
+                # Kinda jank log message
+                self.get_logger().error(
+                    f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
+                )
+                # Don't set self.serial_port; later if-statement will exit()
+            elif (
+                len(recog_ports) == 0 and len(real_ports) > 0
+            ):  # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU
+                self.get_logger().error(
+                    f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
+                )
+                # Don't set self.serial_port; later if-statement will exit()
+            else:  # Found jack shit
+                self.get_logger().error("No valid Serial ports specified or found.")
+                # Don't set self.serial_port; later if-statement will exit()
+
+        # We still don't have a serial port; fall back to legacy discovery (Areeb's code)
+        # Loop through all serial devices on the computer to check for the MCU
+        if self.serial_port is None:
+            self.get_logger().warning("Falling back to legacy MCU discovery...")
+            ports = Anchor.list_serial_ports()
+            for _ in range(4):
+                if self.serial_port is not None:
+                    break
+                for port in ports:
+                    try:
+                        # connect and send a ping command
+                        ser = serial.Serial(port, 115200, timeout=1)
+                        # (f"Checking port {port}...")
+                        ser.write(b"ping\n")
+                        response = ser.read_until(bytes("\n", "utf8"))
+
+                        # if pong is in response, then we are talking with the MCU
+                        if b"pong" in response:
+                            self.serial_port = port
+                            self.get_logger().info(f"Found MCU at {self.serial_port}!")
+                            break
+                    except:
+                        pass
+
+        # If port is still None then we ain't finding no mcu
+        if self.serial_port is None:
+            self.get_logger().error("Unable to find MCU. Exiting...")
+            time.sleep(1)
+            sys.exit(1)
+        # Found a Serial port, try to open it; above code has not officially opened a Serial port
+        else:
+            self.get_logger().debug(
+                f"Attempting to open Serial port '{self.serial_port}'..."
+            )
+            try:
+                self.serial_interface = serial.Serial(
+                    self.serial_port, 115200, timeout=1
+                )
+
+                # Attempt to get name of connected MCU
+                self.serial_interface.write(
+                    b"can_relay_mode,on\n"
+                )  # can_relay_ready,[mcu]
+                mcu_name: str = ""
+                for _ in range(4):
+                    response = self.serial_interface.read_until(bytes("\n", "utf8"))
+                    try:
+                        if b"can_relay_ready" in response:
+                            args: list[str] = response.decode("utf8").strip().split(",")
+                            if len(args) == 2:
+                                mcu_name = args[1]
+                            break
+                    except UnicodeDecodeError:
+                        pass  # ignore malformed responses
+                self.get_logger().info(
+                    f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
+                )
+
+            except serial.SerialException as e:
+                self.get_logger().error(
+                    f"Could not open Serial port '{self.serial_port}' for reason:"
+                )
+                self.get_logger().error(e.strerror)
+                time.sleep(1)
+                sys.exit(1)
+
+        # Close serial port on exit
+        atexit.register(self.cleanup)
+
+        ##################################################
         # ROS2 Topic Setup
 
-        # Publishers
+        # New pub/sub with VicCAN
-        self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
+        self.fromvic_debug_pub_ = self.create_publisher(
-            String,
+            String, "/anchor/from_vic/debug", 20
-            "/anchor/from_vic/debug",
-            20,
         )
         self.fromvic_core_pub_ = self.create_publisher(
-            VicCAN,
+            VicCAN, "/anchor/from_vic/core", 20
-            "/anchor/from_vic/core",
-            20,
         )
         self.fromvic_arm_pub_ = self.create_publisher(
-            VicCAN,
+            VicCAN, "/anchor/from_vic/arm", 20
-            "/anchor/from_vic/arm",
-            20,
         )
         self.fromvic_bio_pub_ = self.create_publisher(
-            VicCAN,
+            VicCAN, "/anchor/from_vic/bio", 20
-            "/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.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(
-            VicCAN,
+            String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
-            "/anchor/from_vic/mock_mcu",
-            self.relay_fromvic,
-            20,
         )
-        self.tovic_string_sub_ = self.create_subscription(
+        self.tovic_sub_ = self.create_subscription(
-            String,
+            VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
-            "/anchor/to_vic/relay_string",
+        )
-            self.write_connector_raw,
+        self.tovic_debug_sub_ = self.create_subscription(
-            20,
+            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
         )
 
-        # poll at 100Hz for incoming data
+        # Create publishers
-        self.read_timer_ = self.create_timer(0.01, self.read_connector)
+        self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
+        self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10)
+        self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
 
-    def destroy_node(self):
+        self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
-        self.get_logger().info("closing connector")
-        self.connector.cleanup()
-        super().destroy_node()
 
-    def read_connector(self):
+        # Create a subscriber
-        """Check the connector for new data from the MCU, and publish string to appropriate topics"""
+        self.relay_sub = self.create_subscription(
-        viccan, raw = self.connector.read()
+            String, "/anchor/relay", self.on_relay_tovic_string, 10
+        )
 
-        if raw:
+    def read_MCU(self):
-            self.fromvic_debug_pub_.publish(String(data=raw))
+        """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
+        try:
+            output = str(self.serial_interface.readline(), "utf8")
 
-        if viccan:
+            if output:
-            self.relay_fromvic(viccan)
+                self.relay_fromvic(output)
+                # All output over debug temporarily
+                # self.get_logger().info(f"[MCU] {output}")
+                msg = String()
+                msg.data = output
+                self.debug_pub.publish(msg)
+                if output.startswith("can_relay_fromvic,core"):
+                    self.core_pub.publish(msg)
+                elif output.startswith("can_relay_fromvic,arm") or output.startswith(
+                    "can_relay_fromvic,digit"
+                ):  # digit for voltage readings
+                    self.arm_pub.publish(msg)
+                if output.startswith("can_relay_fromvic,citadel") or output.startswith(
+                    "can_relay_fromvic,digit"
+                ):  # digit for SHT sensor
+                    self.bio_pub.publish(msg)
+                # msg = String()
+                # msg.data = output
+                # self.debug_pub.publish(msg)
+                return
+        except serial.SerialException as e:
+            print(f"SerialException: {e}")
+            print("Closing serial port.")
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
+            exit(1)
+        except TypeError as e:
+            print(f"TypeError: {e}")
+            print("Closing serial port.")
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
+            exit(1)
+        except Exception as e:
+            print(f"Exception: {e}")
+            # print("Closing serial port.")
+            # if self.ser.is_open:
+            #     self.ser.close()
+            # exit(1)
 
-    def write_connector(self, msg: VicCAN):
+    def on_mock_fromvic(self, msg: String):
-        """Write to the connector and send a copy to /anchor/to_vic/debug"""
+        """For testing without an actual MCU, publish strings here as if they came from an MCU"""
-        self.connector.write(msg)
+        # self.get_logger().info(f"Got command from mock MCU: {msg}")
-        self.tovic_debug_pub_.publish(String(data=viccan_to_string(msg)))
+        self.relay_fromvic(msg.data)
 
-    def write_connector_raw(self, msg: String):
+    def on_relay_tovic_viccan(self, msg: VicCAN):
-        """Write raw string to the connector and send a copy to /anchor/to_vic/debug"""
+        """Relay a VicCAN message to the MCU"""
-        self.connector.write_raw(msg)
+        output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
-        self.tovic_debug_pub_.publish(msg)
+        for num in msg.data:
+            output += f",{round(num, 7)}"  # limit to 7 decimal places
+        output += "\n"
+        # self.get_logger().info(f"VicCAN relay to MCU: {output}")
+        self.serial_interface.write(bytes(output, "utf8"))
 
-    @deprecated(
+    def relay_fromvic(self, msg: str):
-        "Use /anchor/to_vic/relay or /anchor/to_vic/relay_string instead of /anchor/relay"
+        """Relay a string message from the MCU to the appropriate VicCAN topic"""
-    )
+        self.fromvic_debug_pub_.publish(String(data=msg))
-    def write_connector_legacy(self, msg: String):
+        parts = msg.strip().split(",")
-        """Write to the connector by first attempting to convert String to VicCAN"""
+        if len(parts) > 0 and parts[0] != "can_relay_fromvic":
-        # please do not reference this code. ~riley
+            self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
-        for cmd in msg.data.split("\n"):
+            return
-            viccan = string_to_viccan(
+
-                cmd,
+        # String validation
-                "anchor",
+        malformed: bool = False
-                self.get_logger(),
+        malformed_reason: str = ""
-                self.get_clock().now().to_msg(),
+        if len(parts) < 3 or len(parts) > 7:
+            malformed = True
+            malformed_reason = (
+                f"invalid argument count (expected [3,7], got {len(parts)})"
             )
-            if viccan:
+        elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
-                self.write_connector(viccan)
+            malformed = True
+            malformed_reason = f"invalid mcu_name '{parts[1]}'"
+        elif not (parts[2].isnumeric()) or int(parts[2]) < 0:
+            malformed = True
+            malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
+        else:
+            for x in parts[3:]:
+                try:
+                    float(x)
+                except ValueError:
+                    malformed = True
+                    malformed_reason = f"data '{x}' is not a float"
+                    break
 
-    def relay_fromvic(self, msg: VicCAN):
+        if malformed:
-        """Relay a message from the MCU to the appropriate VicCAN topic"""
+            self.get_logger().warning(
-        if msg.mcu_name == "core":
+                f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}"
-            self.fromvic_core_pub_.publish(msg)
+            )
-        if msg.mcu_name == "arm" or msg.mcu_name == "digit":
+            return
-            self.fromvic_arm_pub_.publish(msg)
+
-        if msg.mcu_name == "citadel" or msg.mcu_name == "digit":
+        # Have valid VicCAN message
-            self.fromvic_bio_pub_.publish(msg)
+
+        output = VicCAN()
+        output.mcu_name = parts[1]
+        output.command_id = int(parts[2])
+        if len(parts) > 3:
+            output.data = [float(x) for x in parts[3:]]
+        output.header = Header(
+            stamp=self.get_clock().now().to_msg(), frame_id="from_vic"
+        )
+
+        # self.get_logger().info(f"Relaying from MCU: {output}")
+        if output.mcu_name == "core":
+            self.fromvic_core_pub_.publish(output)
+        elif output.mcu_name == "arm" or output.mcu_name == "digit":
+            self.fromvic_arm_pub_.publish(output)
+        elif output.mcu_name == "citadel" or output.mcu_name == "digit":
+            self.fromvic_bio_pub_.publish(output)
+
+    def on_relay_tovic_string(self, msg: String):
+        """Relay a raw string message to the MCU for debugging"""
+        message = msg.data
+        # self.get_logger().info(f"Sending command to MCU: {msg}")
+        self.serial_interface.write(bytes(message, "utf8"))
+
+    @staticmethod
+    def list_serial_ports():
+        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
+
+    def cleanup(self):
+        print("Cleaning up before terminating...")
+        if self.serial_interface.is_open:
+            self.serial_interface.close()
 
 
 def main(args=None):
-    rclpy.init(args=args)
-    anchor_node = Anchor()
-    executor = SingleThreadedExecutor()
-    executor.add_node(anchor_node)
-
     try:
-        executor.spin()
+        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_MCU()  # Check the MCU for updates
+            rate.sleep()
     except (KeyboardInterrupt, ExternalShutdownException):
-        pass
+        print("Caught shutdown signal, shutting down...")
     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,507 +0,0 @@
-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 as _string_to_viccan, viccan_to_string
-
-# CAN
-import can
-import can.interfaces.socketcan
-import struct
-
-# Serial
-import serial
-import serial.tools.list_ports
-
-KNOWN_USBS = [
-    (0x2E8A, 0x00C0),  # Raspberry Pi Pico
-    (0x1A86, 0x55D4),  # Adafruit Feather ESP32 V2
-    (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
-
-
-class NoWorkingDeviceException(Exception):
-    pass
-
-
-class MultipleValidDevicesException(Exception):
-    pass
-
-
-class DeviceClosedException(Exception):
-    pass
-
-
-class Connector(ABC):
-    logger: RcutilsLogger
-    clock: Clock
-
-    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) -> 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
-
-
-class SerialConnector(Connector):
-    port: str
-    mcu_name: str
-    serial_interface: serial.Serial
-
-    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")
-        if (l := len(ports)) > 1:
-            raise MultipleValidDevicesException(
-                f"too many ({l}) valid serial devices found"
-            )
-
-        # check each of our ports to make sure one of them is responding
-        port = ports[0]
-        # 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"
-            )
-
-        self.port = port
-        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=0)
-
-    def _find_ports(self) -> list[str]:
-        """
-        Finds all valid ports but does not test them
-
-        returns: all valid ports
-        """
-        comports = serial.tools.list_ports.comports()
-        valid_ports = list(
-            map(  # get just device strings
-                lambda p: p.device,
-                filter(  # make sure we have a known device
-                    lambda p: (p.vid, p.pid) in KNOWN_USBS and p.device is not None,
-                    comports,
-                ),
-            )
-        )
-        self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]")
-        return valid_ports
-
-    def _get_name(self, port: str) -> str | None:
-        """
-        Get the name of the MCU (if it works)
-
-        returns: str name of the MCU, None if it doesn't work
-        """
-        # attempt to open the serial port
-        serial_interface: serial.Serial
-        try:
-            self.logger.info(f"asking {port} for its name")
-            serial_interface = serial.Serial(port, BAUD_RATE, timeout=1)
-
-            serial_interface.write(b"can_relay_mode,on\n")
-
-            for i in range(4):
-                self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name")
-                response = serial_interface.read_until(bytes("\n", "utf8"))
-                try:
-                    if b"can_relay_ready" in response:
-                        args: list[str] = response.decode("utf8").strip().split(",")
-                        if len(args) == 2:
-                            self.logger.info(f"we are talking to {args[1]}")
-                            return args[1]
-                        break
-                except UnicodeDecodeError as e:
-                    self.logger.info(
-                        f"ignoring UnicodeDecodeError when asking for MCU name: {e}"
-                    )
-
-            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 _try_readline(self) -> str | None:
-        """Attempts to read a full string from the MCU without blocking.
-
-        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
-
-        # 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 Exception:
-            return (None, None)  # pretty much no other error matters
-
-    def write(self, msg: VicCAN):
-        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}")
-        try:
-            if self.serial_interface.is_open:
-                self.serial_interface.close()
-        except Exception as e:
-            self.logger.error(e)
-
-
-class CANConnector(Connector):
-    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
-
-        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(
-                f"too many ({l}) CAN interfaces found: [{channels}]"
-            )
-
-        bus = avail[0]
-        self.can_channel = str(bus.get("channel"))
-        self.logger.info(f"found CAN interface '{self.can_channel}'")
-
-        try:
-            self.can_bus = can.Bus(
-                interface="socketcan",
-                channel=self.can_channel,
-                bitrate=1_000_000,
-            )
-        except can.CanError as e:
-            raise NoWorkingDeviceException(
-                f"could not open CAN channel '{self.can_channel}': {e}"
-            )
-
-        if self.can_channel and self.can_channel.startswith("v"):
-            self.logger.warn("CAN interface is likely virtual")
-
-    def read(self) -> tuple[VicCAN | None, str | None]:
-        if not self.can_bus:
-            raise DeviceClosedException("CAN bus not initialized")
-
-        try:
-            message = self.can_bus.recv(timeout=0.0)
-        except can.CanError as e:
-            self.logger.error(f"CAN error while receiving: {e}")
-            raise DeviceClosedException("CAN bus closed unexpectedly")
-
-        if message is None:
-            return (None, None)
-
-        arbitration_id = message.arbitration_id & 0x7FF
-        data_bytes = bytes(message.data)
-
-        mcu_key = (arbitration_id >> 8) & 0b111
-        data_type_key = (arbitration_id >> 6) & 0b11
-        command = arbitration_id & 0x3F
-
-        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, None)
-
-        data: list[float] = []
-
-        try:
-            if data_type_key == 3:
-                data = []
-            elif data_type_key == 0:
-                if len(data_bytes) < 8:
-                    self.logger.warn(
-                        f"received double payload with insufficient length {len(data_bytes)}; dropping frame"
-                    )
-                    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, 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, 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, None)
-        except struct.error as e:
-            self.logger.error(f"error unpacking CAN payload: {e}")
-            return (None, None)
-
-        viccan = VicCAN(
-            mcu_name=mcu_name,
-            command_id=int(command),
-            data=data,
-        )
-
-        self.logger.debug(
-            f"received CAN frame id=0x{message.arbitration_id:X}, "
-            f"decoded as VicCAN(mcu_name={viccan.mcu_name}, command_id={viccan.command_id}, data={viccan.data})"
-        )
-
-        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 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.
-        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
-
-        # determine data type from length:
-        # 0: double x1, 1: float32 x2, 2: int16 x4, 3: empty
-        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"unexpected VicCAN data length: {data_len}; dropping message"
-                )
-                return
-
-        # 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"
-            )
-            return
-
-        try:
-            can_message = can.Message(
-                arbitration_id=(mcu_id << 8) | (data_type << 6) | command,
-                data=data,
-                is_extended_id=False,
-            )
-        except Exception as e:
-            self.logger.error(f"failed to construct CAN message: {e}")
-            return
-
-        try:
-            self.can_bus.send(can_message)
-            self.logger.debug(
-                f"sent CAN frame id=0x{can_message.arbitration_id:X}, "
-                f"data={list(can_message.data)}"
-            )
-        except can.CanError as e:
-            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:
-                self.logger.info("shutting down CAN bus")
-                self.can_bus.shutdown()
-        except Exception as e:
-            self.logger.error(e)
-
-
-class MockConnector(Connector):
-    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) -> tuple[VicCAN | None, str | None]:
-        return (None, None)
-
-    def write(self, msg: VicCAN):
-        pass
-
-    def write_raw(self, msg: String):
-        pass

src/anchor_pkg/anchor_pkg/convert.py

@@ -1,65 +0,0 @@
-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, 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 and return None.
-    """
-
-    parts: list[str] = msg.strip().split(",")
-
-    # don't need an extra check because len of .split output is always >= 1
-    if not parts[0].startswith("can_relay_"):
-        logger.debug(f"got non-CAN data from {mcu_name}: {msg}")
-        return None
-    elif len(parts) < 3:
-        logger.debug(f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}")
-        return None
-    elif len(parts) > 7:
-        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=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

@@ -1,111 +1,190 @@
+#!/usr/bin/env python3
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, Shutdown
+from launch.actions import (
+    DeclareLaunchArgument,
+    OpaqueFunction,
+    Shutdown,
+    IncludeLaunchDescription,
+)
+from launch.substitutions import (
+    LaunchConfiguration,
+    ThisLaunchFileDir,
+    PathJoinSubstitution,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+# Prevent making __pycache__ directories
+from sys import dont_write_bytecode
+
+dont_write_bytecode = True
+
+
+def launch_setup(context, *args, **kwargs):
+    # Retrieve the resolved value of the launch argument 'mode'
+    mode = LaunchConfiguration("mode").perform(context)
+    nodes = []
+
+    if mode == "anchor":
+        # Launch every node and pass "anchor" as the parameter
+
+        nodes.append(
+            Node(
+                package="arm_pkg",
+                executable="arm",  # change as needed
+                name="arm",
+                output="both",
+                parameters=[{"launch_mode": mode}],
+                on_exit=Shutdown(),
+            )
+        )
+        nodes.append(
+            Node(
+                package="core_pkg",
+                executable="core",  # change as needed
+                name="core",
+                output="both",
+                parameters=[
+                    {"launch_mode": mode},
+                    {"use_ros2_control": LaunchConfiguration("use_ros2_control", default=False)},
+                ],
+                on_exit=Shutdown(),
+            )
+        )
+        nodes.append(
+            Node(
+                package="core_pkg",
+                executable="ptz",  # change as needed
+                name="ptz",
+                output="both",
+                condition=IfCondition(LaunchConfiguration("use_ptz", default="true")),
+                # Currently don't shutdown all nodes if the PTZ node fails, as it is not critical
+                # on_exit=Shutdown()  # Uncomment if you want to shutdown on PTZ failure
+            )
+        )
+        nodes.append(
+            Node(
+                package="bio_pkg",
+                executable="bio",  # change as needed
+                name="bio",
+                output="both",
+                parameters=[{"launch_mode": mode}],
+                on_exit=Shutdown(),
+            )
+        )
+        nodes.append(
+            Node(
+                package="anchor_pkg",
+                executable="anchor",  # change as needed
+                name="anchor",
+                output="both",
+                parameters=[{"launch_mode": mode}],
+                on_exit=Shutdown(),
+            )
+        )
+    elif mode in ["arm", "core", "bio", "ptz"]:
+        # Only launch the node corresponding to the provided mode.
+        if mode == "arm":
+            nodes.append(
+                Node(
+                    package="arm_pkg",
+                    executable="arm",
+                    name="arm",
+                    output="both",
+                    parameters=[{"launch_mode": mode}],
+                    on_exit=Shutdown(),
+                )
+            )
+        elif mode == "core":
+            nodes.append(
+                Node(
+                    package="core_pkg",
+                    executable="core",
+                    name="core",
+                    output="both",
+                    parameters=[{"launch_mode": mode}],
+                    on_exit=Shutdown(),
+                )
+            )
+        elif mode == "bio":
+            nodes.append(
+                Node(
+                    package="bio_pkg",
+                    executable="bio",
+                    name="bio",
+                    output="both",
+                    parameters=[{"launch_mode": mode}],
+                    on_exit=Shutdown(),
+                )
+            )
+        elif mode == "ptz":
+            nodes.append(
+                Node(
+                    package="core_pkg",
+                    executable="ptz",
+                    name="ptz",
+                    output="both",
+                    on_exit=Shutdown(),  # on fail, shutdown if this was the only node to be launched
+                )
+            )
+    else:
+        # If an invalid mode is provided, print an error.
+        print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
+
+    return nodes
 
 
 def generate_launch_description():
-    connector = LaunchConfiguration("connector")
+    declare_arg = DeclareLaunchArgument(
-    serial_override = LaunchConfiguration("serial_override")
+        "mode",
-    can_override = LaunchConfiguration("can_override")
+        default_value="anchor",
-    use_ptz = LaunchConfiguration("use_ptz")
+        description="Launch mode: arm, core, bio, anchor, or ptz",
-
-    ld = LaunchDescription()
-
-    # arguments
-    ld.add_action(
-        DeclareLaunchArgument(
-            "connector",
-            default_value="auto",
-            description="Connector parameter for anchor node (default: 'auto')",
-        )
     )
 
-    ld.add_action(
+    ros2_control_arg = DeclareLaunchArgument(
-        DeclareLaunchArgument(
+        "use_ros2_control",
-            "serial_override",
+        default_value="false",
-            default_value="",
+        description="Whether to use DiffDriveController for driving instead of direct Twist",
-            description="Serial port override parameter for anchor node (default: '')",
-        )
     )
 
-    ld.add_action(
+    rsp = IncludeLaunchDescription(
-        DeclareLaunchArgument(
+        PythonLaunchDescriptionSource(
-            "can_override",
+            PathJoinSubstitution(
-            default_value="",
+                [
-            description="CAN network override parameter for anchor node (default: '')",
+                    FindPackageShare("core_description"),
-        )
+                    "launch",
+                    "robot_state_publisher.launch.py",
+                ]
+            )
+        ),
+        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+        launch_arguments={("hardware_mode", "physical")},
     )
 
-    ld.add_action(
+    controllers = IncludeLaunchDescription(
-        DeclareLaunchArgument(
+        PythonLaunchDescriptionSource(
-            "use_ptz",
+            PathJoinSubstitution(
-            default_value="true",  # must be string for launch system
+                [
-            description="Whether to launch PTZ node (default: true)",
+                    FindPackageShare("core_description"),
-        )
+                    "launch",
+                    "spawn_controllers.launch.py",
+                ]
+            )
+        ),
+        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+        launch_arguments={("hardware_mode", "physical")},
     )
 
-    # nodes
+    return LaunchDescription(
-    ld.add_action(
+        [
-        Node(
+            declare_arg,
-            package="arm_pkg",
+            ros2_control_arg,
-            executable="arm",
+            rsp,
-            name="arm",
+            controllers,
-            output="both",
+            OpaqueFunction(function=launch_setup),
-            parameters=[{"launch_mode": "anchor"}],
+        ]
-            on_exit=Shutdown(),
-        )
     )
-
-    ld.add_action(
-        Node(
-            package="core_pkg",
-            executable="core",
-            name="core",
-            output="both",
-            parameters=[{"launch_mode": "anchor"}],
-            on_exit=Shutdown(),
-        )
-    )
-
-    ld.add_action(
-        Node(
-            package="core_pkg",
-            executable="ptz",
-            name="ptz",
-            output="both",
-            condition=IfCondition(use_ptz),
-        )
-    )
-
-    ld.add_action(
-        Node(
-            package="bio_pkg",
-            executable="bio",
-            name="bio",
-            output="both",
-            parameters=[{"launch_mode": "anchor"}],
-            on_exit=Shutdown(),
-        )
-    )
-
-    ld.add_action(
-        Node(
-            package="anchor_pkg",
-            executable="anchor",
-            name="anchor",
-            output="both",
-            parameters=[
-                {
-                    "launch_mode": "anchor",
-                    "connector": connector,
-                    "serial_override": serial_override,
-                    "can_override": can_override,
-                }
-            ],
-            on_exit=Shutdown(),
-        )
-    )
-
-    return ld

src/anchor_pkg/package.xml

@@ -3,14 +3,13 @@
 <package format="3">
   <name>anchor_pkg</name>
   <version>0.0.0</version>
-  <description>Anchor -- ROS and CAN relay node</description>
+  <description>TODO: Package description</description>
-  <maintainer email="rjm0037@uah.edu">Riley</maintainer>
+  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
   <depend>common_interfaces</depend>
   <depend>python3-serial</depend>
-  <depend>python3-can</depend>
 
   <build_depend>black</build_depend>
 

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().debug(message_text)
+        self.get_logger().info(message_text)
 
     def run_async_func(self, coro):
         """Run an async function in the event loop."""

src/headless_pkg/src/headless_node.py

@@ -138,6 +138,11 @@ class Headless(Node):
             self.get_parameter("use_old_topics").get_parameter_value().bool_value
         )
 
+        self.declare_parameter("use_cmd_vel", False)
+        self.use_cmd_vel = (
+            self.get_parameter("use_cmd_vel").get_parameter_value().bool_value
+        )
+
         self.declare_parameter("use_bio", False)
         self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value
 
@@ -145,7 +150,6 @@ class Headless(Node):
         self.use_arm_ik = (
             self.get_parameter("use_arm_ik").get_parameter_value().bool_value
         )
-
         # NOTE: only applicable if use_old_topics == True
         self.declare_parameter("use_new_arm_manual_scheme", True)
         self.use_new_arm_manual_scheme = (
@@ -155,6 +159,13 @@ class Headless(Node):
         )
 
         # Check parameter validity
+        if self.use_cmd_vel:
+            self.get_logger().info("Using cmd_vel for core control")
+            global CORE_MODE
+            CORE_MODE = "twist"
+        else:
+            self.get_logger().info("Using astra_msgs/CoreControl for core control")
+
         if self.use_arm_ik and self.use_old_topics:
             self.get_logger().fatal("Old topics do not support arm IK control.")
             sys.exit(1)
@@ -184,6 +195,9 @@ class Headless(Node):
             self.core_twist_pub_ = self.create_publisher(
                 Twist, "/core/twist", qos_profile=control_qos
             )
+            self.core_cmd_vel_pub_ = self.create_publisher(
+                TwistStamped, "/diff_controller/cmd_vel", qos_profile=control_qos
+            )
             self.core_state_pub_ = self.create_publisher(
                 CoreCtrlState, "/core/control/state", qos_profile=control_qos
             )
@@ -231,13 +245,19 @@ class Headless(Node):
         # Rumble when node is ready (returns False if rumble not supported)
         self.gamepad.rumble(0.7, 0.8, 150)
 
+        # Added so you can tell when it starts running after changing the constant logging to debug from info
+        self.get_logger().info("Defaulting to Core mode. Ready.")
+
     def stop_all(self):
         if self.use_old_topics:
             self.core_publisher.publish(CORE_STOP_MSG)
             self.arm_publisher.publish(ARM_STOP_MSG)
             self.bio_publisher.publish(BIO_STOP_MSG)
         else:
-            self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
+            if self.use_cmd_vel:
+                self.core_cmd_vel_pub_.publish(self.core_cmd_vel_stop_msg())
+            else:
+                self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
             if self.use_arm_ik:
                 self.arm_ik_twist_publisher.publish(self.arm_ik_twist_stop_msg())
             else:
@@ -332,18 +352,28 @@ class Headless(Node):
             self.core_publisher.publish(input)
 
         else:  # New topics
-            input = Twist()
+            twist = Twist()
 
             # Forward/back and Turn
-            input.linear.x = -1.0 * left_stick_y
+            twist.linear.x = -1.0 * left_stick_y
-            input.angular.z = -1.0 * copysign(
+            twist.angular.z = -1.0 * copysign(
                 right_stick_x**2, right_stick_x
             )  # Exponent for finer control (curve)
 
+            # This kinda looks dumb being seperate from the following block, but this
+            # maintains the separation between modifying the control message and sending it
+            if self.use_cmd_vel:
+                twist.linear.x *= 1.5
+                twist.angular.z *= 0.5
+
             # Publish
-            self.core_twist_pub_.publish(input)
+            if self.use_cmd_vel:
-            self.get_logger().info(
+                header = Header(stamp=self.get_clock().now().to_msg())
-                f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
+                self.core_cmd_vel_pub_.publish(TwistStamped(header=header, twist=twist))
+            else:
+                self.core_twist_pub_.publish(twist)
+            self.get_logger().debug(
+                f"[Core Ctrl] Linear: {round(twist.linear.x, 2)}, Angular: {round(twist.angular.z, 2)}"
             )
 
             # Brake mode
@@ -643,6 +673,11 @@ class Headless(Node):
         else:
             pass  # TODO: implement new bio control topics
 
+    def core_cmd_vel_stop_msg(self):
+        return TwistStamped(
+            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg())
+        )
+
     def arm_manual_stop_msg(self):
         return JointJog(
             header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg()),

treefmt.nix

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