put the timeout back and figure out how shutdown works

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..."
-    sleep 1
+  echo "[INFO] Waiting for active network interface..."
+  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..."
-    sleep 1
+  echo "[INFO] Waiting for active network interface..."
+  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,4 +26,3 @@ source $SCRIPT_DIR/../install/setup.bash
 
 # Launch the ROS 2 node
 ros2 run headless_pkg headless_full
-

auto_start/start_rosbag.sh

@@ -1,26 +1,25 @@
 #!/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 "$AUTONOMY_WS" ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
+[[ -z $ANCHOR_WS ]] && ANCHOR_WS="$SCRIPT_DIR/.."
+[[ -z $AUTONOMY_WS ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
 BAG_LOCATION="$HOME/bags/autostart"
-[[ ! -d "$BAG_LOCATION" ]] && mkdir -p "$BAG_LOCATION"
+[[ ! -d $BAG_LOCATION ]] && mkdir -p "$BAG_LOCATION"
 
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
-    echo "[INFO] Waiting for active network interface..."
-    sleep 1
+  echo "[INFO] Waiting for active network interface..."
+  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,6 +31,8 @@
           name = "ASTRA Anchor";
           packages = with pkgs; [
             colcon
+            socat
+            can-utils
             (python313.withPackages (
               p: with p; [
                 pyserial

scripts/reset-repo.bash

@@ -0,0 +1,38 @@
+#!/usr/bin/env bash
+
+repo_root="$(git rev-parse --show-toplevel)"
+
+if [[ -z $repo_root ]]; then
+  echo "script must be run from within 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

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

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,11 +1,9 @@
 from warnings import deprecated
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
+from rclpy.executors import ExternalShutdownException, SingleThreadedExecutor
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 
-import atexit
-
 from .connector import (
     Connector,
     MockConnector,
@@ -14,8 +12,7 @@ from .connector import (
     NoValidDeviceException,
     NoWorkingDeviceException,
 )
-from .convert import string_to_viccan
-import threading
+from .convert import string_to_viccan, viccan_to_string
 
 from astra_msgs.msg import VicCAN
 from std_msgs.msg import String
@@ -25,26 +22,25 @@ class Anchor(Node):
     """
     Publishers:
     * /anchor/from_vic/debug
-        - Every string received from the MCU is published here for debugging
+        - Every string received from the MCU is published here for debugging (String)
     * /anchor/from_vic/core
         - VicCAN messages for Core node
     * /anchor/from_vic/arm
-        - VicCAN messages for Arm node
+        - VicCAN messages for Arm node (also receives digit messages)
     * /anchor/from_vic/bio
-        - VicCAN messages for Bio node
+        - VicCAN messages for Bio node (also receives digit messages)
     * /anchor/to_vic/debug
-        - A string copy of the messages published to ./relay are published here
+        - String copy of all messages sent to the connector
 
     Subscribers:
     * /anchor/from_vic/mock_mcu
-        - For testing without an actual MCU, publish ViCAN messages here as if they came from an MCU
+        - For testing without an actual MCU, publish VicCAN messages here as if they came from an MCU
     * /anchor/to_vic/relay
         - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
     * /anchor/to_vic/relay_string
-        - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector)
+        - Send raw strings to connectors. Does not work for CANConnector
     * /anchor/relay
-        - Legacy method for talking to connectors. Takes String as input, but does not send the raw strings to connectors.
-          Instead, it converts them to VicCAN messages first.
+        - (Deprecated) Legacy topic. Takes String, converts to VicCAN, then sends to connector
     """
 
     connector: Connector
@@ -54,7 +50,7 @@ class Anchor(Node):
 
         logger = self.get_logger()
 
-        # ROS2 Parameter Setup
+        # ROS2 parameter setup
 
         self.declare_parameter(
             "connector",
@@ -89,7 +85,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
         )
@@ -129,9 +125,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",
@@ -152,14 +148,14 @@ class Anchor(Node):
             "/anchor/from_vic/bio",
             20,
         )
-        # Debug publisher
+        # debug publisher for outgoing messages
         self.tovic_debug_pub_ = self.create_publisher(
-            VicCAN,
+            String,
             "/anchor/to_vic/debug",
             20,
         )
 
-        # Subscribers
+        # subscribers
         self.tovic_sub_ = self.create_subscription(
             VicCAN,
             "/anchor/to_vic/relay",
@@ -173,7 +169,7 @@ class Anchor(Node):
             20,
         )
         self.mock_mcu_sub_ = self.create_subscription(
-            String,
+            VicCAN,
             "/anchor/from_vic/mock_mcu",
             self.relay_fromvic,
             20,
@@ -181,15 +177,17 @@ class Anchor(Node):
         self.tovic_string_sub_ = self.create_subscription(
             String,
             "/anchor/to_vic/relay_string",
-            self.connector.write_raw,
+            self.write_connector_raw,
             20,
         )
 
-        # Close devices on exit
-        atexit.register(self.cleanup)
+        # poll at 100Hz for incoming data
+        self.read_timer_ = self.create_timer(0.01, self.read_connector)
 
-    def cleanup(self):
+    def destroy_node(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"""
@@ -204,6 +202,11 @@ 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(
@@ -226,25 +229,26 @@ 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)
-        elif msg.mcu_name == "arm" or msg.mcu_name == "digit":
+        if msg.mcu_name == "arm" or msg.mcu_name == "digit":
             self.fromvic_arm_pub_.publish(msg)
-        elif msg.mcu_name == "citadel" or msg.mcu_name == "digit":
+        if 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:
-        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()
+        executor.spin()
     except (KeyboardInterrupt, ExternalShutdownException):
-        print("Caught shutdown signal, shutting down...")
+        pass
     finally:
+        # don't accept any more jobs
+        executor.shutdown()
+        # make the node quit processing things
+        anchor_node.destroy_node()
+        # shut down everything else
         rclpy.try_shutdown()

src/anchor_pkg/anchor_pkg/connector.py

@@ -1,5 +1,6 @@
 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
@@ -77,7 +78,7 @@ class Connector(ABC):
         pass
 
     @abstractmethod
-    def write_raw(self, msg: str):
+    def write_raw(self, msg: String):
         pass
 
     def cleanup(self):
@@ -199,10 +200,10 @@ class SerialConnector(Connector):
             return (None, None)  # pretty much no other error matters
 
     def write(self, msg: VicCAN):
-        self.write_raw(viccan_to_string(msg))
+        self.write_raw(String(data=viccan_to_string(msg)))
 
-    def write_raw(self, msg: str):
-        self.serial_interface.write(bytes(msg, "utf8"))
+    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}")
@@ -411,8 +412,10 @@ class CANConnector(Connector):
             self.logger.error(f"CAN error while sending: {e}")
             raise DeviceClosedException("CAN bus closed unexpectedly")
 
-    def write_raw(self, msg: str):
-        self.logger.warn(f"write_raw is not supported for CANConnector. msg: {msg}")
+    def write_raw(self, msg: String):
+        self.logger.warn(
+            f"write_raw is not supported for CANConnector. msg: {msg.data}"
+        )
 
     def cleanup(self):
         try:
@@ -426,7 +429,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)
@@ -434,5 +437,5 @@ class MockConnector(Connector):
     def write(self, msg: VicCAN):
         pass
 
-    def write_raw(self, msg: str):
+    def write_raw(self, msg: String):
         pass

src/anchor_pkg/launch/rover.launch.py

@@ -1,10 +1,8 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, Shutdown, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, Shutdown
 from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 def generate_launch_description():
@@ -48,24 +46,50 @@ 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",
@@ -84,91 +108,4 @@ 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,20 +3,14 @@
 <package format="3">
   <name>anchor_pkg</name>
   <version>0.0.0</version>
-  <description>ASTRA VicCAN driver package, using python-can and pyserial.</description>
+  <description>Anchor -- ROS and CAN relay node</description>
   <maintainer email="rjm0037@uah.edu">Riley</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-
-  <exec_depend>common_interfaces</exec_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>
+  <depend>common_interfaces</depend>
+  <depend>python3-serial</depend>
+  <depend>python3-can</depend>
 
   <build_depend>black</build_depend>
 

src/anchor_pkg/setup.cfg

@@ -2,5 +2,3 @@
 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="1.0.0",
+    version="0.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="ASTRA VicCAN driver package, using python-can and pyserial.",
-    license="AGPL-3.0-only",
+    description="Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN",
+    license="All Rights Reserved",
     entry_points={
         "console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
     },

src/arm_pkg/arm_pkg/arm_node.py

@@ -1,4 +1,5 @@
 import sys
+import threading
 import signal
 import math
 from warnings import deprecated
@@ -12,7 +13,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, ArmCtrlState, VicCAN, RevMotorState
+from astra_msgs.msg import ArmFeedback, VicCAN, RevMotorState
 
 control_qos = qos.QoSProfile(
     history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -25,6 +26,8 @@ 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."""
@@ -112,10 +115,10 @@ class ArmNode(Node):
 
         # Control
 
-        # Manual: /arm/control/joint_jog is published by Basestation or Headless
+        # Manual: /arm/manual/joint_jog is published by Basestation or Headless
         self.man_jointjog_sub_ = self.create_subscription(
             JointJog,
-            "/arm/control/joint_jog",
+            "/arm/manual/joint_jog",
             self.jointjog_callback,
             qos_profile=control_qos,
         )
@@ -126,13 +129,6 @@ 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
 
@@ -152,14 +148,9 @@ 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)
@@ -182,54 +173,10 @@ class ArmNode(Node):
         # Deadzone
         velocities = [vel if abs(vel) > 0.05 else 0.0 for vel in velocities]
 
-        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,
-            )
-        )
+        self.send_velocities(velocities, 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.")
@@ -255,12 +202,12 @@ class ArmNode(Node):
 
         # Send Axis 0-3
         self.anchor_tovic_pub_.publish(
-            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:4], header=header)
+            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:3], 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:6], header=header)
+            VicCAN(mcu_name="digit", command_id=43, data=velocities[4:5], header=header)
         )
         # Send End Effector Gripper
         # TODO: Verify m/s received correctly by embedded
@@ -343,8 +290,6 @@ 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])
@@ -431,14 +376,11 @@ 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,15 +3,14 @@
 <package format="3">
   <name>arm_pkg</name>
   <version>1.0.0</version>
-  <description>Relays topics related to Arm between VicCAN (through Anchor) and basestation.</description>
-  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
+  <description>Core arm package which handles ROS2 commnuication.</description>
+  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-
-  <exec_depend>common_interfaces</exec_depend>
-  <exec_depend>python3-numpy</exec_depend>
-  <exec_depend>astra_msgs</exec_depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-numpy</depend>
+  <depend>astra_msgs</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 Sharpe",
+    maintainer="David",
     maintainer_email="ds0196@uah.edu",
-    description="Relays topics related to Arm between VicCAN (through Anchor) and basestation.",
+    description="Relays topics related to the 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,16 +2,14 @@
 <?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>1.0.0</version>
-  <description>Biosensor package to handle command interpretation and embedded interfacing.</description>
-  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-
-  <exec_depend>common_interfaces</exec_depend>
-
-  <exec_depend>astra_msgs</exec_depend>
+  <depend>common_interfaces</depend>
+  <depend>astra_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/bio_pkg/setup.cfg

@@ -2,5 +2,3 @@
 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="Relays topics related to Biosensor between VicCAN (through Anchor) and basestation.",
-    license="AGPL-3.0-only",
+    description="TODO: Package description",
+    license="TODO: License declaration",
     entry_points={
         "console_scripts": ["bio = bio_pkg.bio_node:main"],
     },

src/core_pkg/core_pkg/core_headless.py

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

src/core_pkg/setup.cfg

@@ -2,5 +2,3 @@
 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="1.0.0",
+    version="0.0.0",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
@@ -12,13 +12,14 @@ setup(
     ],
     install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="David Sharpe",
-    maintainer_email="ds0196@uah.edu",
-    description="Relays topics related to Core between VicCAN (through Anchor) and basestation.",
-    license="AGPL-3.0-only",
+    maintainer="tristan",
+    maintainer_email="tristanmcginnis26@gmail.com",
+    description="Core rover control package to handle command interpretation and embedded interfacing.",
+    license="All Rights Reserved",
     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,15 +3,14 @@
 <package format="3">
   <name>headless_pkg</name>
   <version>1.0.0</version>
-  <description>Provides headless rover control, similar to Basestation.</description>
+  <description>Headless rover control package to handle command interpretation and embedded interfacing.</description>
   <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-
-  <exec_depend>common_interfaces</exec_depend>
-  <exec_depend>python3-pygame</exec_depend>
-  <exec_depend>astra_msgs</exec_depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-pygame</depend>
+  <depend>astra_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/headless_pkg/setup.cfg

@@ -2,5 +2,3 @@
 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,9 +14,11 @@ setup(
     zip_safe=True,
     maintainer="David Sharpe",
     maintainer_email="ds0196@uah.edu",
-    description="Provides headless rover control, similar to Basestation.",
-    license="AGPL-3.0-only",
+    description="Headless rover control package to handle command interpretation and embedded interfacing.",
+    license="All Rights Reserved",
     entry_points={
-        "console_scripts": ["headless_full = src.headless_node:main"],
+        "console_scripts": [
+            "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, ArmCtrlState
+from astra_msgs.msg import CoreCtrlState
 
 import warnings
 
@@ -138,11 +138,6 @@ 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
 
@@ -150,6 +145,7 @@ 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 = (
@@ -159,13 +155,6 @@ 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)
@@ -177,8 +166,6 @@ 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
@@ -197,18 +184,12 @@ 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/control/joint_jog", qos_profile=control_qos
-            )
-            self.arm_state_pub_ = self.create_publisher(
-                ArmCtrlState, "/arm/control/state", qos_profile=control_qos
+                JointJog, "/arm/manual_new", qos_profile=control_qos
             )
 
             self.arm_ik_twist_publisher = self.create_publisher(
@@ -250,19 +231,13 @@ 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:
-            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)
+            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:
@@ -357,32 +332,18 @@ class Headless(Node):
             self.core_publisher.publish(input)
 
         else:  # New topics
-            twist = Twist()
+            input = Twist()
 
             # Forward/back and Turn
-            twist.linear.x = -1.0 * left_stick_y
-            twist.angular.z = -1.0 * copysign(
+            input.linear.x = -1.0 * left_stick_y
+            input.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
-            if self.use_cmd_vel:
-                header = Header(stamp=self.get_clock().now().to_msg())
-                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)}"
+            self.core_twist_pub_.publish(input)
+            self.get_logger().info(
+                f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
             )
 
             # Brake mode
@@ -591,26 +552,13 @@ class Headless(Node):
             )
 
             # A: brake
-            new_brake_mode = button_a
+            # TODO: Brake mode
 
-            # X: laser
-            new_laser = button_x
+            # Y: linear actuator
+            # TODO: linear actuator
 
             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)
         # =============
 
@@ -695,11 +643,6 @@ 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,8 +1,9 @@
-{ pkgs, ... }:
+{ ... }:
 {
   projectRootFile = "flake.nix";
   programs = {
     nixfmt.enable = true;
     black.enable = true;
+    shfmt.enable = true;
   };
 }