diff --git a/flake.nix b/flake.nix index baf6853..4e49781 100644 --- a/flake.nix +++ b/flake.nix @@ -31,6 +31,8 @@ name = "ASTRA Anchor"; packages = with pkgs; [ colcon + socat + can-utils (python313.withPackages ( p: with p; [ pyserial diff --git a/scripts/reset-repo.bash b/scripts/reset-repo.bash new file mode 100755 index 0000000..dc7d418 --- /dev/null +++ b/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" +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 + diff --git a/scripts/test-connectors.bash b/scripts/test-connectors.bash new file mode 100755 index 0000000..a739243 --- /dev/null +++ b/scripts/test-connectors.bash @@ -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 "$@" diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 7c475a6..9a1baac 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -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() diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 0d4790b..ed51987 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/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: @@ -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