From 44e457bf76e0da8a5d52f848b35f71174e3a26f6 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Sat, 11 Apr 2026 14:45:12 -0500 Subject: [PATCH] some consistency stuff with comments and also remove timeout from serial reads --- scripts/reset-repo.bash | 2 +- scripts/test-connectors.bash | 284 ++++++++++++----------- src/anchor_pkg/anchor_pkg/anchor_node.py | 21 +- src/anchor_pkg/anchor_pkg/connector.py | 4 +- 4 files changed, 164 insertions(+), 147 deletions(-) diff --git a/scripts/reset-repo.bash b/scripts/reset-repo.bash index 36e3ed4..cc9e4e2 100755 --- a/scripts/reset-repo.bash +++ b/scripts/reset-repo.bash @@ -12,7 +12,7 @@ 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 +if [[ $okay != "y" ]]; then echo "you didn't say exactly 'y'. aborting." >&2 exit 2 fi diff --git a/scripts/test-connectors.bash b/scripts/test-connectors.bash index 0695ce1..df87d2e 100755 --- a/scripts/test-connectors.bash +++ b/scripts/test-connectors.bash @@ -13,6 +13,7 @@ fi cd "$repo_root" +# colors BOLD='\033[1m' RED='\033[1;31m' GREEN='\033[1;32m' @@ -69,6 +70,24 @@ wait_for_topic() { return 0 } +# run a ROS pub/echo test +# usage: ros_pubsub_test +# 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" @@ -90,91 +109,57 @@ test_mock_connector() { log "anchor started successfully" - # relay -> debug + # 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]}') - 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 + 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 - # mock_mcu -> from_vic/core + # 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]}') - 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 + 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 - # mock_mcu -> from_vic/arm + # 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]}') - 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 + 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 - # mock_mcu -> from_vic/bio + # 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]}') - 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 + 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 - # relay_string -> debug + # 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\"}') - 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 + 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" @@ -218,26 +203,61 @@ test_serial_connector() { pass "serial connector: anchor starts with virtual serial" - # relay_string -> debug + # 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\"}') - 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 + 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 - 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="" @@ -268,106 +288,102 @@ test_can_connector() { log "anchor started successfully" sleep 1 - # relay -> CAN bus + # test: relay -> CAN bus + # core=1, int16x4=2, cmd=30 -> id = (1<<8)|(2<<6)|30 = 0x19E 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 + 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 - # core=1, int16x4=2, cmd=30 -> id = (1<<8)|(2<<6)|30 = 0x19E - if echo "$can_output" | grep -qi "19E"; then + if echo "$output" | grep -qi "19E"; then pass "CAN connector: relay -> CAN bus" else - fail "CAN connector: relay -> CAN bus (got: $can_output)" + fail "CAN connector: relay -> CAN bus (got: $output)" fi - # CAN -> from_vic/core + # test: 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 + 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 + 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 - # CAN -> from_vic/arm + # test: 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 + 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 + 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 - # double data type + # test: CAN double data type (data_type_key=0) 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 + 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 + 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 - # float32x2 data type + # test: CAN float32x2 data type (data_type_key=1) 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 + 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 + 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" diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 9a1baac..50fa388 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -40,9 +40,9 @@ class Anchor(Node): * /anchor/to_vic/relay - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU * /anchor/to_vic/relay_string - - Send raw strings to connectors. Does not work for CANConnector. + - Send raw strings to connectors. Does not work for CANConnector * /anchor/relay - - (Deprecated) Legacy method. Takes String, converts to VicCAN, then sends to connector. + - (Deprecated) Legacy topic. Takes String, converts to VicCAN, then sends to connector """ connector: Connector @@ -52,7 +52,7 @@ class Anchor(Node): logger = self.get_logger() - # ROS2 Parameter Setup + # ROS2 parameter setup self.declare_parameter( "connector", @@ -87,7 +87,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 ) @@ -127,9 +127,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", @@ -150,14 +150,14 @@ class Anchor(Node): "/anchor/from_vic/bio", 20, ) - # Debug publisher for outgoing messages + # debug publisher for outgoing messages self.tovic_debug_pub_ = self.create_publisher( String, "/anchor/to_vic/debug", 20, ) - # Subscribers + # subscribers self.tovic_sub_ = self.create_subscription( VicCAN, "/anchor/to_vic/relay", @@ -183,10 +183,10 @@ class Anchor(Node): 20, ) - # Timer to poll connector for incoming data at 100 Hz + # poll at 100Hz for incoming data self.read_timer_ = self.create_timer(0.01, self.read_connector) - # Close devices on exit + # close devices on exit atexit.register(self.cleanup) def cleanup(self): @@ -249,6 +249,7 @@ def main(args=None): except (KeyboardInterrupt, ExternalShutdownException): pass finally: + # might not have to shut down everything manually, find out later ~riley 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 ed51987..276a9ae 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -154,7 +154,7 @@ class SerialConnector(Connector): serial_interface: serial.Serial try: self.logger.info(f"asking {port} for its name") - serial_interface = serial.Serial(port, BAUD_RATE, timeout=1) + serial_interface = serial.Serial(port, BAUD_RATE) serial_interface.write(b"can_relay_mode,on\n") @@ -429,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)