feat(core): change rover_platform parameter to override, default with hostname instead of environment variable

Launch file uses clucky, testbed, auto. Node was using core, testbed, auto. Replaced core with clucky in the node.

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

@@ -31,8 +31,6 @@
           name = "ASTRA Anchor";
           packages = with pkgs; [
             colcon
-            socat
-            can-utils
             (python313.withPackages (
               p: with p; [
                 pyserial

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 the rover-ros2 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,9 +1,11 @@
 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
 
+import atexit
+
 from .connector import (
     Connector,
     MockConnector,
@@ -12,7 +14,8 @@ from .connector import (
     NoValidDeviceException,
     NoWorkingDeviceException,
 )
-from .convert import string_to_viccan, viccan_to_string
+from .convert import string_to_viccan
+import threading
 
 from astra_msgs.msg import VicCAN
 from std_msgs.msg import String
@@ -22,25 +25,26 @@ class Anchor(Node):
     """
     Publishers:
     * /anchor/from_vic/debug
-        - Every string received from the MCU is published here for debugging (String)
+        - Every string received from the MCU is published here for debugging
     * /anchor/from_vic/core
         - VicCAN messages for Core node
     * /anchor/from_vic/arm
-        - VicCAN messages for Arm node (also receives digit messages)
+        - VicCAN messages for Arm node
     * /anchor/from_vic/bio
-        - VicCAN messages for Bio node (also receives digit messages)
+        - VicCAN messages for Bio node
     * /anchor/to_vic/debug
-        - String copy of all messages sent to the connector
+        - 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 ViCAN messages here as if they came from an MCU
     * /anchor/to_vic/relay
         - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
     * /anchor/to_vic/relay_string
-        - Send raw strings to connectors. Does not work for CANConnector
+        - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector)
     * /anchor/relay
-        - (Deprecated) Legacy topic. Takes String, converts to VicCAN, then sends to connector
+        - 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
@@ -50,7 +54,7 @@ class Anchor(Node):
 
         logger = self.get_logger()
 
-        # ROS2 parameter setup
+        # ROS2 Parameter Setup
 
         self.declare_parameter(
             "connector",
@@ -85,7 +89,7 @@ class Anchor(Node):
             ),
         )
 
-        # determine which connector to use. options are Mock, Serial, and CAN
+        # Determine which connector to use. Options are Mock, Serial, and CAN
         connector_select = (
             self.get_parameter("connector").get_parameter_value().string_value
         )
@@ -125,9 +129,9 @@ class Anchor(Node):
                 )
                 exit(1)
 
-        # ROS2 topic setup
+        # ROS2 Topic Setup
 
-        # publishers
+        # Publishers
         self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
             String,
             "/anchor/from_vic/debug",
@@ -148,14 +152,14 @@ class Anchor(Node):
             "/anchor/from_vic/bio",
             20,
         )
-        # debug publisher for outgoing messages
+        # Debug publisher
         self.tovic_debug_pub_ = self.create_publisher(
-            String,
+            VicCAN,
             "/anchor/to_vic/debug",
             20,
         )
 
-        # subscribers
+        # Subscribers
         self.tovic_sub_ = self.create_subscription(
             VicCAN,
             "/anchor/to_vic/relay",
@@ -169,7 +173,7 @@ class Anchor(Node):
             20,
         )
         self.mock_mcu_sub_ = self.create_subscription(
-            VicCAN,
+            String,
             "/anchor/from_vic/mock_mcu",
             self.relay_fromvic,
             20,
@@ -177,17 +181,15 @@ class Anchor(Node):
         self.tovic_string_sub_ = self.create_subscription(
             String,
             "/anchor/to_vic/relay_string",
-            self.write_connector_raw,
+            self.connector.write_raw,
             20,
         )
 
-        # poll at 100Hz for incoming data
+        # Close devices on exit
-        self.read_timer_ = self.create_timer(0.01, self.read_connector)
+        atexit.register(self.cleanup)
 
-    def destroy_node(self):
+    def cleanup(self):
-        self.get_logger().info("closing connector")
         self.connector.cleanup()
-        super().destroy_node()
 
     def read_connector(self):
         """Check the connector for new data from the MCU, and publish string to appropriate topics"""
@@ -202,11 +204,6 @@ class Anchor(Node):
     def write_connector(self, msg: VicCAN):
         """Write to the connector and send a copy to /anchor/to_vic/debug"""
         self.connector.write(msg)
-        self.tovic_debug_pub_.publish(String(data=viccan_to_string(msg)))
-
-    def write_connector_raw(self, msg: String):
-        """Write raw string to the connector and send a copy to /anchor/to_vic/debug"""
-        self.connector.write_raw(msg)
         self.tovic_debug_pub_.publish(msg)
 
     @deprecated(
@@ -229,26 +226,25 @@ class Anchor(Node):
         """Relay a message from the MCU to the appropriate VicCAN topic"""
         if msg.mcu_name == "core":
             self.fromvic_core_pub_.publish(msg)
-        if msg.mcu_name == "arm" or msg.mcu_name == "digit":
+        elif 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":
+        elif msg.mcu_name == "citadel" or msg.mcu_name == "digit":
             self.fromvic_bio_pub_.publish(msg)
 
 
 def main(args=None):
-    rclpy.init(args=args)
-    anchor_node = Anchor()
-    executor = 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_connector()  # Check the connector 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()

src/anchor_pkg/anchor_pkg/connector.py

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

src/anchor_pkg/launch/rover.launch.py

@@ -1,8 +1,10 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, Shutdown
+from launch.actions import DeclareLaunchArgument, Shutdown, IncludeLaunchDescription
 from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 def generate_launch_description():
@@ -46,50 +48,24 @@ def generate_launch_description():
         )
     )
 
+    ld.add_action(
+        DeclareLaunchArgument(
+            "use_ros2_control",
+            default_value="false",
+            description="Whether to use DiffDriveController for driving instead of direct Twist",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "rover_platform_override",
+            default_value="",
+            description="Override the rover platform (either clucky or testbed). If unset, hostname is used; defaults to clucky without hostname.",
+            choices=["clucky", "testbed", ""],
+        )
+    )
+
     # nodes
-    ld.add_action(
-        Node(
-            package="arm_pkg",
-            executable="arm",
-            name="arm",
-            output="both",
-            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",
@@ -108,4 +84,91 @@ def generate_launch_description():
         )
     )
 
+    ld.add_action(
+        Node(
+            package="arm_pkg",
+            executable="arm",
+            name="arm",
+            output="both",
+            parameters=[{"launch_mode": "anchor"}],
+            on_exit=Shutdown(),
+        )
+    )
+
+    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="core_pkg",
+            executable="core",
+            name="core",
+            output="both",
+            parameters=[
+                {"launch_mode": "anchor"},
+                {
+                    "use_ros2_control": LaunchConfiguration(
+                        "use_ros2_control", default=False
+                    )
+                },
+                {
+                    "rover_platform_override": LaunchConfiguration(
+                        "rover_platform_override", default="auto"
+                    )
+                },
+            ],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="core_pkg",
+            executable="ptz",
+            name="ptz",
+            output="both",
+            condition=IfCondition(use_ptz),
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("core_description"),
+                        "launch",
+                        "robot_state_publisher.launch.py",
+                    ]
+                )
+            ),
+            condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+            launch_arguments={("hardware_mode", "physical")},
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("core_description"),
+                        "launch",
+                        "spawn_controllers.launch.py",
+                    ]
+                )
+            ),
+            condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+            launch_arguments={("hardware_mode", "physical")},
+        )
+    )
+
     return ld

src/anchor_pkg/package.xml

@@ -3,14 +3,20 @@
 <package format="3">
   <name>anchor_pkg</name>
   <version>0.0.0</version>
-  <description>Anchor -- ROS and CAN relay node</description>
+  <description>ASTRA VicCAN driver package, using python-can and pyserial.</description>
   <maintainer email="rjm0037@uah.edu">Riley</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>python3-serial</depend>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>python3-can</depend>
+  <exec_depend>python3-serial</exec_depend>
+  <exec_depend>python3-can</exec_depend>
+
+  <exec_depend>core_pkg</exec_depend>
+  <exec_depend>arm_pkg</exec_depend>
+  <exec_depend>bio_pkg</exec_depend>
+  <exec_depend>core_description</exec_depend>
 
   <build_depend>black</build_depend>
 

src/anchor_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/anchor_pkg
 [install]
 install_scripts=$base/lib/anchor_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/anchor_pkg/setup.py

@@ -6,7 +6,7 @@ package_name = "anchor_pkg"
 
 setup(
     name=package_name,
-    version="0.0.0",
+    version="1.0.0",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
@@ -17,8 +17,8 @@ setup(
     zip_safe=True,
     maintainer="tristan",
     maintainer_email="tristanmcginnis26@gmail.com",
-    description="Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN",
+    description="ASTRA VicCAN driver package, using python-can and pyserial.",
-    license="All Rights Reserved",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
     },

src/arm_pkg/arm_pkg/arm_node.py

@@ -1,5 +1,4 @@
 import sys
-import threading
 import signal
 import math
 from warnings import deprecated
@@ -13,7 +12,7 @@ from std_msgs.msg import String, Header
 from sensor_msgs.msg import JointState
 from control_msgs.msg import JointJog
 from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual  # TODO: Old topics
-from astra_msgs.msg import ArmFeedback, VicCAN, RevMotorState
+from astra_msgs.msg import ArmFeedback, ArmCtrlState, VicCAN, RevMotorState
 
 control_qos = qos.QoSProfile(
     history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -26,8 +25,6 @@ control_qos = qos.QoSProfile(
     # liveliness_lease_duration=Duration(seconds=5),
 )
 
-thread = None
-
 
 class ArmNode(Node):
     """Relay between Anchor and Basestation/Headless/Moveit2 for Arm related topics."""
@@ -115,10 +112,10 @@ class ArmNode(Node):
 
         # Control
 
-        # Manual: /arm/manual/joint_jog is published by Basestation or Headless
+        # Manual: /arm/control/joint_jog is published by Basestation or Headless
         self.man_jointjog_sub_ = self.create_subscription(
             JointJog,
-            "/arm/manual/joint_jog",
+            "/arm/control/joint_jog",
             self.jointjog_callback,
             qos_profile=control_qos,
         )
@@ -129,6 +126,13 @@ class ArmNode(Node):
             self.joint_command_callback,
             qos_profile=control_qos,
         )
+        # State: /arm/control/state is published by Basestation or Headless
+        self.man_state_sub_ = self.create_subscription(
+            ArmCtrlState,
+            "/arm/control/state",
+            self.man_state_callback,
+            qos_profile=control_qos,
+        )
 
         # Feedback
 
@@ -148,9 +152,14 @@ class ArmNode(Node):
 
         # Combined Socket and Digit feedback
         self.arm_feedback_new = ArmFeedback()
+        self.arm_feedback_new.axis0_motor.id = 1
+        self.arm_feedback_new.axis1_motor.id = 2
+        self.arm_feedback_new.axis2_motor.id = 3
+        self.arm_feedback_new.axis3_motor.id = 4
 
         # IK Arm pose
         self.saved_joint_state = JointState()
+        self.saved_joint_state.header.frame_id = "base_link"
         self.saved_joint_state.name = self.all_joint_names
         # ... initialize with zeros
         self.saved_joint_state.position = [0.0] * len(self.saved_joint_state.name)
@@ -173,10 +182,54 @@ class ArmNode(Node):
         # Deadzone
         velocities = [vel if abs(vel) > 0.05 else 0.0 for vel in velocities]
 
-        self.send_velocities(velocities, msg.header)
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=39,
+                data=velocities[0:4],
+                header=msg.header,
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="digit",
+                command_id=39,
+                data=velocities[4:6],
+                header=msg.header,
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="digit",
+                command_id=26,
+                data=[velocities[6]],
+                header=msg.header,
+            )
+        )
 
         # TODO: use msg.duration
 
+    def man_state_callback(self, msg: ArmCtrlState):
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=18,
+                data=[1.0 if msg.brake_mode else 0.0],
+                header=Header(stamp=self.get_clock().now().to_msg()),
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=34,
+                data=[1.0 if msg.laser else 0.0],
+                header=Header(stamp=self.get_clock().now().to_msg()),
+            )
+        )
+
     def joint_command_callback(self, msg: JointState):
         if len(msg.position) < 7 and len(msg.velocity) < 7:
             self.get_logger().debug("Ignoring malformed /joint_command message.")
@@ -202,12 +255,12 @@ class ArmNode(Node):
 
         # Send Axis 0-3
         self.anchor_tovic_pub_.publish(
-            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:3], header=header)
+            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:4], header=header)
         )
         # Send Wrist yaw and roll
         # TODO: Verify embedded
         self.anchor_tovic_pub_.publish(
-            VicCAN(mcu_name="digit", command_id=43, data=velocities[4:5], header=header)
+            VicCAN(mcu_name="digit", command_id=43, data=velocities[4:6], header=header)
         )
         # Send End Effector Gripper
         # TODO: Verify m/s received correctly by embedded
@@ -290,6 +343,8 @@ class ArmNode(Node):
                 )
                 return
 
+        self.arm_feedback_new.header.stamp = msg.header.stamp
+
         match msg.command_id:
             case 53:  # REV SPARK MAX feedback
                 motorId = round(msg.data[0])
@@ -376,11 +431,14 @@ class ArmNode(Node):
                 )
                 return
 
+        self.arm_feedback_new.header.stamp = msg.header.stamp
+
         match msg.command_id:
             case 54:  # Board voltages
                 self.arm_feedback_new.digit_voltage.vbatt = float(msg.data[0]) / 100.0
                 self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0
                 self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0
+                self.arm_feedback_new.digit_voltage.header.stamp = msg.header.stamp
             case 55:  # Arm joint positions
                 self.saved_joint_state.position[4] = math.radians(
                     msg.data[0]

src/arm_pkg/package.xml

@@ -3,14 +3,15 @@
 <package format="3">
   <name>arm_pkg</name>
   <version>1.0.0</version>
-  <description>Core arm package which handles ROS2 commnuication.</description>
+  <description>Relays topics related to Arm between VicCAN (through Anchor) and basestation.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>python3-numpy</depend>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>astra_msgs</depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/arm_pkg/setup.py

@@ -12,9 +12,9 @@ setup(
     ],
     install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="David",
+    maintainer="David Sharpe",
     maintainer_email="ds0196@uah.edu",
-    description="Relays topics related to the arm between VicCAN (through Anchor) and basestation.",
+    description="Relays topics related to Arm between VicCAN (through Anchor) and basestation.",
     license="AGPL-3.0-only",
     entry_points={
         "console_scripts": ["arm = arm_pkg.arm_node:main"],

src/bio_pkg/package.xml

@@ -2,14 +2,16 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>bio_pkg</name>
-  <version>0.0.0</version>
+  <version>1.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Biosensor package to handle command interpretation and embedded interfacing.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>astra_msgs</depend>
+  <exec_depend>common_interfaces</exec_depend>
+
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/bio_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/bio_pkg
 [install]
 install_scripts=$base/lib/bio_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/bio_pkg/setup.py

@@ -14,8 +14,8 @@ setup(
     zip_safe=True,
     maintainer="tristan",
     maintainer_email="tristanmcginnis26@gmail.com",
-    description="TODO: Package description",
+    description="Relays topics related to Biosensor between VicCAN (through Anchor) and basestation.",
-    license="TODO: License declaration",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": ["bio = bio_pkg.bio_node:main"],
     },

src/core_pkg/core_pkg/core_headless.py

@@ -1,112 +0,0 @@
-import rclpy
-from rclpy.node import Node
-
-import pygame
-
-import time
-
-import serial
-import sys
-import threading
-import glob
-import os
-
-import importlib
-from std_msgs.msg import String
-from astra_msgs.msg import CoreControl
-
-
-os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
-os.environ["SDL_AUDIODRIVER"] = (
-    "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
-)
-
-
-max_speed = 90  # Max speed as a duty cycle percentage (1-100)
-
-
-class Headless(Node):
-    def __init__(self):
-        # Initialize pygame first
-        pygame.init()
-        pygame.joystick.init()
-
-        # Wait for a gamepad to be connected
-        self.gamepad = None
-        print("Waiting for gamepad connection...")
-        while pygame.joystick.get_count() == 0:
-            # Process any pygame events to keep it responsive
-            for event in pygame.event.get():
-                if event.type == pygame.QUIT:
-                    pygame.quit()
-                    sys.exit(0)
-            time.sleep(1.0)  # Check every second
-            print("No gamepad found. Waiting...")
-
-        # Initialize the gamepad
-        self.gamepad = pygame.joystick.Joystick(0)
-        self.gamepad.init()
-        print(f"Gamepad Found: {self.gamepad.get_name()}")
-
-        # Now initialize the ROS2 node
-        super().__init__("core_headless")
-        self.create_timer(0.15, self.send_controls)
-        self.publisher = self.create_publisher(CoreControl, "/core/control", 10)
-        self.lastMsg = (
-            String()
-        )  # Used to ignore sending controls repeatedly when they do not change
-
-    def run(self):
-        # This thread makes all the update processes run in the background
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-
-        try:
-            while rclpy.ok():
-                self.send_controls()
-                time.sleep(0.1)  # Small delay to avoid CPU hogging
-        except KeyboardInterrupt:
-            sys.exit(0)
-
-    def send_controls(self):
-        for event in pygame.event.get():
-            if event.type == pygame.QUIT:
-                pygame.quit()
-                sys.exit(0)
-
-        # Check if controller is still connected
-        if pygame.joystick.get_count() == 0:
-            print("Gamepad disconnected. Exiting...")
-            # Send one last zero control message
-            input = CoreControl()
-            input.left_stick = 0
-            input.right_stick = 0
-            input.max_speed = 0
-            self.publisher.publish(input)
-            self.get_logger().info("Final stop command sent. Shutting down.")
-            # Clean up
-            pygame.quit()
-            sys.exit(0)
-
-        input = CoreControl()
-        input.max_speed = max_speed
-        input.right_stick = -1 * round(self.gamepad.get_axis(4), 2)  # right y-axis
-        if self.gamepad.get_axis(5) > 0:
-            input.left_stick = input.right_stick
-        else:
-            input.left_stick = -1 * round(self.gamepad.get_axis(1), 2)  # left y-axis
-
-        output = f"L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}"
-        self.get_logger().info(f"[Ctrl] {output}")
-        self.publisher.publish(input)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = Headless()
-    rclpy.spin(node)
-    rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

src/core_pkg/package.xml

@@ -3,15 +3,17 @@
 <package format="3">
   <name>core_pkg</name>
   <version>1.0.0</version>
-  <description>Core rover control package to handle command interpretation and embedded interfacing.</description>
+  <description>Relays topics related to Core between VicCAN (through Anchor) and basestation.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>python3-scipy</depend>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>python-crccheck-pip</depend>
+  <exec_depend>python3-scipy</exec_depend>
-  <depend>astra_msgs</depend>
+  <exec_depend>python-crccheck-pip</exec_depend>
+
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/core_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/core_pkg
 [install]
 install_scripts=$base/lib/core_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/core_pkg/setup.py

@@ -4,7 +4,7 @@ package_name = "core_pkg"
 
 setup(
     name=package_name,
-    version="0.0.0",
+    version="1.0.0",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
@@ -12,14 +12,13 @@ setup(
     ],
     install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="tristan",
+    maintainer="David Sharpe",
-    maintainer_email="tristanmcginnis26@gmail.com",
+    maintainer_email="ds0196@uah.edu",
-    description="Core rover control package to handle command interpretation and embedded interfacing.",
+    description="Relays topics related to Core between VicCAN (through Anchor) and basestation.",
-    license="All Rights Reserved",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": [
             "core = core_pkg.core_node:main",
-            "headless = core_pkg.core_headless:main",
             "ptz = core_pkg.core_ptz:main",
         ],
     },

src/headless_pkg/package.xml

@@ -3,14 +3,15 @@
 <package format="3">
   <name>headless_pkg</name>
   <version>1.0.0</version>
-  <description>Headless rover control package to handle command interpretation and embedded interfacing.</description>
+  <description>Provides headless rover control, similar to Basestation.</description>
   <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>python3-pygame</depend>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>astra_msgs</depend>
+  <exec_depend>python3-pygame</exec_depend>
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/headless_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/headless_pkg
 [install]
 install_scripts=$base/lib/headless_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/headless_pkg/setup.py

@@ -14,11 +14,9 @@ setup(
     zip_safe=True,
     maintainer="David Sharpe",
     maintainer_email="ds0196@uah.edu",
-    description="Headless rover control package to handle command interpretation and embedded interfacing.",
+    description="Provides headless rover control, similar to Basestation.",
-    license="All Rights Reserved",
+    license="AGPL-3.0-only",
     entry_points={
-        "console_scripts": [
+        "console_scripts": ["headless_full = src.headless_node:main"],
-            "headless_full = src.headless_node:main",
-        ],
     },
 )

src/headless_pkg/src/headless_node.py

@@ -18,7 +18,7 @@ from std_msgs.msg import Header
 from geometry_msgs.msg import Twist, TwistStamped
 from control_msgs.msg import JointJog
 from astra_msgs.msg import CoreControl, ArmManual, BioControl
-from astra_msgs.msg import CoreCtrlState
+from astra_msgs.msg import CoreCtrlState, ArmCtrlState
 
 import warnings
 
@@ -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)
@@ -166,6 +177,8 @@ class Headless(Node):
         self.ctrl_mode = "core"  # Start in core mode
         self.core_brake_mode = False
         self.core_max_duty = 0.5  # Default max duty cycle (walking speed)
+        self.arm_brake_mode = False
+        self.arm_laser = False
 
         ##################################################
         # Old Topics
@@ -184,12 +197,18 @@ 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
             )
 
             self.arm_manual_pub_ = self.create_publisher(
-                JointJog, "/arm/manual_new", qos_profile=control_qos
+                JointJog, "/arm/control/joint_jog", qos_profile=control_qos
+            )
+            self.arm_state_pub_ = self.create_publisher(
+                ArmCtrlState, "/arm/control/state", qos_profile=control_qos
             )
 
             self.arm_ik_twist_publisher = self.create_publisher(
@@ -231,13 +250,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 +357,32 @@ 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:
+                # These scaling factors convert raw stick inputs to absolute m/s and rad/s
+                # values that DiffDriveController will convert to motor RPM, rather than
+                # the plain Twist, which just sends the stick values as duty cycle and
+                # sends that scaled to the motors.
+                twist.linear.x *= 1.0
+                twist.angular.z *= 1.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
@@ -552,13 +591,26 @@ class Headless(Node):
             )
 
             # A: brake
-            # TODO: Brake mode
+            new_brake_mode = button_a
 
-            # Y: linear actuator
+            # X: laser
-            # TODO: linear actuator
+            new_laser = button_x
 
             self.arm_manual_pub_.publish(arm_input)
 
+            # Only publish state if needed
+            if new_brake_mode != self.arm_brake_mode or new_laser != self.arm_laser:
+                self.arm_brake_mode = new_brake_mode
+                self.arm_laser = new_laser
+                state_msg = ArmCtrlState()
+                state_msg.brake_mode = bool(self.arm_brake_mode)
+                state_msg.laser = bool(self.arm_laser)
+
+                self.arm_state_pub_.publish(state_msg)
+                self.get_logger().info(
+                    f"[Arm State] Brake: {self.arm_brake_mode}, Laser: {self.arm_laser}"
+                )
+
         # IK (ONLY NEW)
         # =============
 
@@ -643,6 +695,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 +1,8 @@
-{ ... }:
+{ pkgs, ... }:
 {
   projectRootFile = "flake.nix";
   programs = {
     nixfmt.enable = true;
     black.enable = true;
-    shfmt.enable = true;
   };
 }