fix a handful of oversights in the anchor refactor. add a testing script

This commit is contained in:
ryleu
2026-04-10 19:50:57 -05:00
parent 8404999369
commit 760f6ddd19
5 changed files with 529 additions and 35 deletions

View File

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

38
scripts/reset-repo.bash Executable file
View File

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

447
scripts/test-connectors.bash Executable file
View File

@@ -0,0 +1,447 @@
#!/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"
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
}
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"
# relay -> debug
log "testing relay -> debug"
local debug_output
debug_output=$(timeout 5 bash -c '
ros2 topic echo --once /anchor/to_vic/debug &
ECHO_PID=$!
sleep 0.5
ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 50, data: [1.0, 2.0, 3.0, 4.0]}" >/dev/null 2>&1
wait $ECHO_PID
' 2>/dev/null) || true
if [[ -n "$debug_output" ]] && echo "$debug_output" | grep -q "can_relay_tovic,core,50"; then
pass "mock connector: relay -> debug"
else
fail "mock connector: relay -> debug"
fi
# mock_mcu -> from_vic/core
log "testing mock_mcu (core) -> from_vic/core"
local core_output=""
core_output=$(timeout 5 bash -c '
ros2 topic echo --once /anchor/from_vic/core &
ECHO_PID=$!
sleep 0.5
ros2 topic pub --once /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 10, data: [100.0, 200.0]}" >/dev/null 2>&1 || true
wait $ECHO_PID || true
' 2>/dev/null || true) || core_output=""
if [[ -n "$core_output" ]] && echo "$core_output" | grep -q "mcu_name: core" && echo "$core_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
# mock_mcu -> from_vic/arm
log "testing mock_mcu (arm) -> from_vic/arm"
local arm_output
arm_output=$(timeout 5 bash -c '
ros2 topic echo --once /anchor/from_vic/arm &
ECHO_PID=$!
sleep 0.5
ros2 topic pub --once /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN "{mcu_name: \"arm\", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}" >/dev/null 2>&1
wait $ECHO_PID
' 2>/dev/null) || true
if [[ -n "$arm_output" ]] && echo "$arm_output" | grep -q "mcu_name: arm" && echo "$arm_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
# mock_mcu -> from_vic/bio
log "testing mock_mcu (citadel) -> from_vic/bio"
local bio_output
bio_output=$(timeout 5 bash -c '
ros2 topic echo --once /anchor/from_vic/bio &
ECHO_PID=$!
sleep 0.5
ros2 topic pub --once /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN "{mcu_name: \"citadel\", command_id: 20, data: [5.0]}" >/dev/null 2>&1
wait $ECHO_PID
' 2>/dev/null) || true
if echo "$bio_output" | grep -q "mcu_name: citadel" && echo "$bio_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
# relay_string -> debug
log "testing relay_string -> debug"
local relay_string_output
relay_string_output=$(timeout 5 bash -c '
ros2 topic echo --once /anchor/to_vic/debug &
ECHO_PID=$!
sleep 0.5
ros2 topic pub --once /anchor/to_vic/relay_string std_msgs/msg/String "{data: \"test_raw_string_data\"}" >/dev/null 2>&1
wait $ECHO_PID
' 2>/dev/null) || true
if [[ -n "$relay_string_output" ]] && echo "$relay_string_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"
# relay_string -> debug
log "testing relay_string -> debug"
local relay_string_output
relay_string_output=$(timeout 5 bash -c '
ros2 topic echo --once /anchor/to_vic/debug &
ECHO_PID=$!
sleep 0.5
ros2 topic pub --once /anchor/to_vic/relay_string std_msgs/msg/String "{data: \"serial_test_string\"}" >/dev/null 2>&1
wait $ECHO_PID
' 2>/dev/null) || true
if [[ -n "$relay_string_output" ]] && echo "$relay_string_output" | grep -q "serial_test_string"; then
pass "serial connector: relay_string -> debug"
else
fail "serial connector: relay_string -> debug"
fi
log "serial data transfer tests skipped (virtual pty limitation)"
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
# relay -> CAN bus
log "testing relay -> CAN bus"
local can_output
can_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
# core=1, int16x4=2, cmd=30 -> id = (1<<8)|(2<<6)|30 = 0x19E
if echo "$can_output" | grep -qi "19E"; then
pass "CAN connector: relay -> CAN bus"
else
fail "CAN connector: relay -> CAN bus (got: $can_output)"
fi
# CAN -> from_vic/core
log "testing CAN bus -> from_vic/core"
local core_output
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 "$core_output" | grep -q "mcu_name: core" && echo "$core_output" | grep -q "command_id: 15"; then
pass "CAN connector: CAN -> from_vic/core"
else
fail "CAN connector: CAN -> from_vic/core"
fi
# CAN -> from_vic/arm
log "testing CAN bus -> from_vic/arm"
local arm_output
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 "$arm_output" | grep -q "mcu_name: arm" && echo "$arm_output" | grep -q "command_id: 20"; then
pass "CAN connector: CAN -> from_vic/arm"
else
fail "CAN connector: CAN -> from_vic/arm"
fi
# double data type
log "testing CAN double data type"
local double_output
double_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 "$double_output" | grep -q "mcu_name: core" && echo "$double_output" | grep -q "command_id: 5"; then
pass "CAN connector: double data type"
else
fail "CAN connector: double data type"
fi
# float32x2 data type
log "testing CAN float32x2 data type"
local float_output
float_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 "$float_output" | grep -q "mcu_name: core" && echo "$float_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' 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 "$@"

View File

@@ -1,7 +1,7 @@
from warnings import deprecated
import rclpy
from rclpy.node import Node
from rclpy.executors import ExternalShutdownException
from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
from rcl_interfaces.msg import ParameterDescriptor, ParameterType
import atexit
@@ -14,8 +14,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 +24,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 method. Takes String, converts to VicCAN, then sends to connector.
"""
connector: Connector
@@ -152,9 +150,9 @@ 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,
)
@@ -173,7 +171,7 @@ class Anchor(Node):
20,
)
self.mock_mcu_sub_ = self.create_subscription(
String,
VicCAN,
"/anchor/from_vic/mock_mcu",
self.relay_fromvic,
20,
@@ -181,10 +179,13 @@ 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,
)
# Timer to poll connector for incoming data at 100 Hz
self.read_timer_ = self.create_timer(0.01, self.read_connector)
# Close devices on exit
atexit.register(self.cleanup)
@@ -204,6 +205,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 +232,23 @@ 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 = MultiThreadedExecutor()
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:
executor.shutdown()
anchor_node.destroy_node()
rclpy.try_shutdown()

View File

@@ -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:
@@ -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