mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-04-20 11:51:16 -05:00
some consistency stuff with comments and also remove timeout from serial reads
This commit is contained in:
ryleu
@@ -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
|
||||
|
||||
@@ -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 <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"
|
||||
|
||||
@@ -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"
|
||||
|
||||
Reference in New Issue
Block a user