aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-04-20 11:51:16 -05:00
Code Issues Packages Projects Releases Wiki Activity

some consistency stuff with comments and also remove timeout from serial reads

Browse Source
This commit is contained in:
ryleu
2026-04-11 14:45:12 -05:00
parent 6eb4f0041b
commit 44e457bf76
4 changed files with 164 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
scripts/reset-repo.bash
View File

@@ -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)" 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 read okay
if [[ ! $okay == "y" ]]; then if [[ $okay != "y" ]]; then
echo "you didn't say exactly 'y'. aborting." >&2 echo "you didn't say exactly 'y'. aborting." >&2
exit 2 exit 2
fi fi

188
scripts/test-connectors.bash
View File

@@ -13,6 +13,7 @@ fi
cd "$repo_root" cd "$repo_root"
# colors
BOLD='\033[1m' BOLD='\033[1m'
RED='\033[1;31m' RED='\033[1;31m'
GREEN='\033[1;32m' GREEN='\033[1;32m'
@@ -69,6 +70,24 @@ wait_for_topic() {
return 0 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() { test_mock_connector() {
log "testing mock connector" log "testing mock connector"
@@ -90,91 +109,57 @@ test_mock_connector() {
log "anchor started successfully" log "anchor started successfully"
# relay -> debug # test: relay -> debug
log "testing 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 if [[ -n $output ]] && echo "$output" | grep -q "can_relay_tovic,core,50"; then
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" pass "mock connector: relay -> debug"
else else
fail "mock connector: relay -> debug" fail "mock connector: relay -> debug"
fi fi
# mock_mcu -> from_vic/core # test: mock_mcu -> from_vic/core
log "testing mock_mcu (core) -> 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="" if [[ -n $output ]] && echo "$output" | grep -q "mcu_name: core" && echo "$output" | grep -q "command_id: 10"; then
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" pass "mock connector: mock_mcu -> from_vic/core"
else else
fail "mock connector: mock_mcu -> from_vic/core" fail "mock connector: mock_mcu -> from_vic/core"
fi fi
# mock_mcu -> from_vic/arm # test: mock_mcu -> from_vic/arm
log "testing mock_mcu (arm) -> 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 if [[ -n $output ]] && echo "$output" | grep -q "mcu_name: arm" && echo "$output" | grep -q "command_id: 55"; then
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" pass "mock connector: mock_mcu -> from_vic/arm"
else else
fail "mock connector: mock_mcu -> from_vic/arm" fail "mock connector: mock_mcu -> from_vic/arm"
fi fi
# mock_mcu -> from_vic/bio # test: mock_mcu -> from_vic/bio
log "testing mock_mcu (citadel) -> 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 if echo "$output" | grep -q "mcu_name: citadel" && echo "$output" | grep -q "command_id: 20"; then
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" pass "mock connector: mock_mcu -> from_vic/bio"
else else
fail "mock connector: mock_mcu -> from_vic/bio" fail "mock connector: mock_mcu -> from_vic/bio"
fi fi
# relay_string -> debug # test: relay_string -> debug
log "testing 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 if [[ -n $output ]] && echo "$output" | grep -q "test_raw_string_data"; then
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" pass "mock connector: relay_string -> debug"
else else
fail "mock connector: relay_string -> debug" fail "mock connector: relay_string -> debug"
@@ -218,26 +203,61 @@ test_serial_connector() {
pass "serial connector: anchor starts with virtual serial" pass "serial connector: anchor starts with virtual serial"
# relay_string -> debug # test: relay -> serial output (VicCAN encoding)
log "testing relay_string -> debug" log "testing relay -> serial output"
local relay_string_output local serial_out_file
relay_string_output=$(timeout 5 bash -c ' serial_out_file=$(mktemp)
ros2 topic echo --once /anchor/to_vic/debug &
# 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=$! ECHO_PID=$!
sleep 0.5 sleep 0.5
ros2 topic pub --once /anchor/to_vic/relay_string std_msgs/msg/String "{data: \"serial_test_string\"}" >/dev/null 2>&1 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 wait $ECHO_PID
' 2>/dev/null) || true ' 2>/dev/null) || true
if [[ -n $relay_string_output ]] && echo "$relay_string_output" | grep -q "serial_test_string"; then 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" pass "serial connector: relay_string -> debug"
else else
fail "serial connector: relay_string -> debug" fail "serial connector: relay_string -> debug"
fi fi
log "serial data transfer tests skipped (virtual pty limitation)"
kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
wait "$ANCHOR_PID" 2>/dev/null || true wait "$ANCHOR_PID" 2>/dev/null || true
ANCHOR_PID="" ANCHOR_PID=""
@@ -268,11 +288,12 @@ test_can_connector() {
log "anchor started successfully" log "anchor started successfully"
sleep 1 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" log "testing relay -> CAN bus"
local can_output local output
can_output=$(timeout 8 bash -c ' output=$(timeout 8 bash -c '
candump -n 1 vcan0 & candump -n 1 vcan0 &
DUMP_PID=$! DUMP_PID=$!
sleep 1 sleep 1
@@ -282,18 +303,16 @@ test_can_connector() {
wait $DUMP_PID wait $DUMP_PID
' 2>/dev/null) || true ' 2>/dev/null) || true
# core=1, int16x4=2, cmd=30 -> id = (1<<8)|(2<<6)|30 = 0x19E if echo "$output" | grep -qi "19E"; then
if echo "$can_output" | grep -qi "19E"; then
pass "CAN connector: relay -> CAN bus" pass "CAN connector: relay -> CAN bus"
else else
fail "CAN connector: relay -> CAN bus (got: $can_output)" fail "CAN connector: relay -> CAN bus (got: $output)"
fi fi
# CAN -> from_vic/core # test: CAN -> from_vic/core
log "testing CAN bus -> from_vic/core" log "testing CAN bus -> from_vic/core"
local core_output output=$(timeout 5 bash -c '
core_output=$(timeout 5 bash -c '
ros2 topic echo --once /anchor/from_vic/core & ros2 topic echo --once /anchor/from_vic/core &
ECHO_PID=$! ECHO_PID=$!
sleep 1 sleep 1
@@ -303,17 +322,16 @@ test_can_connector() {
wait $ECHO_PID wait $ECHO_PID
' 2>/dev/null) || true ' 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" pass "CAN connector: CAN -> from_vic/core"
else else
fail "CAN connector: CAN -> from_vic/core" fail "CAN connector: CAN -> from_vic/core"
fi fi
# CAN -> from_vic/arm # test: CAN -> from_vic/arm
log "testing CAN bus -> from_vic/arm" log "testing CAN bus -> from_vic/arm"
local arm_output output=$(timeout 5 bash -c '
arm_output=$(timeout 5 bash -c '
ros2 topic echo --once /anchor/from_vic/arm & ros2 topic echo --once /anchor/from_vic/arm &
ECHO_PID=$! ECHO_PID=$!
sleep 1 sleep 1
@@ -323,17 +341,16 @@ test_can_connector() {
wait $ECHO_PID wait $ECHO_PID
' 2>/dev/null) || true ' 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" pass "CAN connector: CAN -> from_vic/arm"
else else
fail "CAN connector: CAN -> from_vic/arm" fail "CAN connector: CAN -> from_vic/arm"
fi fi
# double data type # test: CAN double data type (data_type_key=0)
log "testing CAN double data type" log "testing CAN double data type"
local double_output output=$(timeout 8 bash -c '
double_output=$(timeout 8 bash -c '
ros2 topic echo --once /anchor/from_vic/core & ros2 topic echo --once /anchor/from_vic/core &
ECHO_PID=$! ECHO_PID=$!
sleep 1 sleep 1
@@ -345,17 +362,16 @@ test_can_connector() {
wait $ECHO_PID wait $ECHO_PID
' 2>/dev/null) || true ' 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" pass "CAN connector: double data type"
else else
fail "CAN connector: double data type" fail "CAN connector: double data type"
fi fi
# float32x2 data type # test: CAN float32x2 data type (data_type_key=1)
log "testing CAN float32x2 data type" log "testing CAN float32x2 data type"
local float_output output=$(timeout 8 bash -c '
float_output=$(timeout 8 bash -c '
ros2 topic echo --once /anchor/from_vic/core & ros2 topic echo --once /anchor/from_vic/core &
ECHO_PID=$! ECHO_PID=$!
sleep 1 sleep 1
@@ -367,7 +383,7 @@ test_can_connector() {
wait $ECHO_PID wait $ECHO_PID
' 2>/dev/null) || true ' 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" pass "CAN connector: float32x2 data type"
else else
fail "CAN connector: float32x2 data type" fail "CAN connector: float32x2 data type"

21
src/anchor_pkg/anchor_pkg/anchor_node.py
View File

@@ -40,9 +40,9 @@ class Anchor(Node):
* /anchor/to_vic/relay * /anchor/to_vic/relay
- Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
* /anchor/to_vic/relay_string * /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 * /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 connector: Connector
@@ -52,7 +52,7 @@ class Anchor(Node):
logger = self.get_logger() logger = self.get_logger()
# ROS2 Parameter Setup # ROS2 parameter setup
self.declare_parameter( self.declare_parameter(
"connector", "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 = ( connector_select = (
self.get_parameter("connector").get_parameter_value().string_value self.get_parameter("connector").get_parameter_value().string_value
) )
@@ -127,9 +127,9 @@ class Anchor(Node):
) )
exit(1) exit(1)
# ROS2 Topic Setup # ROS2 topic setup
# Publishers # publishers
self.fromvic_debug_pub_ = self.create_publisher( # only used by serial self.fromvic_debug_pub_ = self.create_publisher( # only used by serial
String, String,
"/anchor/from_vic/debug", "/anchor/from_vic/debug",
@@ -150,14 +150,14 @@ class Anchor(Node):
"/anchor/from_vic/bio", "/anchor/from_vic/bio",
20, 20,
) )
# Debug publisher for outgoing messages # debug publisher for outgoing messages
self.tovic_debug_pub_ = self.create_publisher( self.tovic_debug_pub_ = self.create_publisher(
String, String,
"/anchor/to_vic/debug", "/anchor/to_vic/debug",
20, 20,
) )
# Subscribers # subscribers
self.tovic_sub_ = self.create_subscription( self.tovic_sub_ = self.create_subscription(
VicCAN, VicCAN,
"/anchor/to_vic/relay", "/anchor/to_vic/relay",
@@ -183,10 +183,10 @@ class Anchor(Node):
20, 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) self.read_timer_ = self.create_timer(0.01, self.read_connector)
# Close devices on exit # close devices on exit
atexit.register(self.cleanup) atexit.register(self.cleanup)
def cleanup(self): def cleanup(self):
@@ -249,6 +249,7 @@ def main(args=None):
except (KeyboardInterrupt, ExternalShutdownException): except (KeyboardInterrupt, ExternalShutdownException):
pass pass
finally: finally:
# might not have to shut down everything manually, find out later ~riley
executor.shutdown() executor.shutdown()
anchor_node.destroy_node() anchor_node.destroy_node()
rclpy.try_shutdown() rclpy.try_shutdown()

4
src/anchor_pkg/anchor_pkg/connector.py
View File

@@ -154,7 +154,7 @@ class SerialConnector(Connector):
serial_interface: serial.Serial serial_interface: serial.Serial
try: try:
self.logger.info(f"asking {port} for its name") 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") serial_interface.write(b"can_relay_mode,on\n")
@@ -429,7 +429,7 @@ class CANConnector(Connector):
class MockConnector(Connector): class MockConnector(Connector):
def __init__(self, logger: RcutilsLogger, clock: Clock): def __init__(self, logger: RcutilsLogger, clock: Clock):
super().__init__(logger, 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]: def read(self) -> tuple[VicCAN | None, str | None]:
return (None, None) return (None, None)