#!/usr/bin/env bash
# 无硬件冒烟测试：拉起 roscore + invicta_hardware（骨架）+ invicta_deploy，
# 检查话题、频率、参数是否按预期工作。需要先 catkin_make 并 source devel/setup.bash。
#
#   source /opt/ros/noetic/setup.bash && source devel/setup.bash
#   bash src/invicta_deploy/scripts/smoke_test.sh     # 或任意路径下调用本脚本

set -u
WS_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"  # scripts -> invicta_deploy -> src -> ws
cd "$WS_DIR" || exit 1

echo "=== 1. onnxruntime 动态库解析 ==="
ldd devel/lib/invicta_deploy/invicta_deploy | grep -i onnx || echo "!! libonnxruntime.so 未解析，检查 CMakeLists 的 RPATH"

echo "=== 2. 启动 roscore ==="
nohup roscore >/tmp/roscore.log 2>&1 &
sleep 8

echo "=== 3. 启动 invicta_hardware（require_can:=false：无 CAN 时只发零位状态）==="
nohup roslaunch invicta_hardware invicta_hardware.launch require_can:=false >/tmp/hw.log 2>&1 &
sleep 4

echo "--- 话题 ---"
rostopic list

echo "--- /joint_states 频率（期望 ~200 Hz）---"
timeout 5 rostopic hz /joint_states 2>&1 | tail -2

echo "--- /joint_states 样例 ---"
timeout 5 rostopic echo -n1 /joint_states 2>&1 | sed -n '1,12p'

echo "--- /imu/data 样例 ---"
timeout 5 rostopic echo -n1 /imu/data 2>&1 | sed -n '1,18p'

echo "=== 4. 启动 invicta_remote（协议节点）==="
nohup roslaunch invicta_remote invicta_remote.launch >/tmp/remote.log 2>&1 &
sleep 3
python3 "$WS_DIR/src/invicta_remote/scripts/protocol_udp_test.py" --port 30000 --steer 0.2,0,0.1 --motion 1 >/tmp/proto_test.log 2>&1
tail -4 /tmp/proto_test.log

echo "=== 5. 启动 invicta_deploy（RL 节点）==="
nohup roslaunch invicta_deploy invicta_deploy.launch >/tmp/dep.log 2>&1 &
sleep 8

echo "--- /joint_commands 频率（idle 状态期望 ~200 Hz）---"
timeout 5 rostopic hz /joint_commands 2>&1 | tail -2

echo "--- 配置是否读到（TOML）---"
grep -aE "^\[config\]" /tmp/dep.log | head -3

echo "--- 节点连接关系 ---"
rosnode info /invicta_deploy 2>&1 | sed -n '1,14p'

echo "--- /motion_state 频率（期望 ~20 Hz）---"
timeout 4 rostopic hz /motion_state 2>&1 | tail -2

echo "=== 6. 日志 ==="
echo "--- invicta_deploy ---"
tail -35 /tmp/dep.log
echo "--- invicta_remote ---"
tail -12 /tmp/remote.log
echo "--- invicta_hardware ---"
tail -8 /tmp/hw.log

pkill -f roslaunch >/dev/null 2>&1
pkill roscore >/dev/null 2>&1
sleep 1
echo "=== 冒烟测试结束 ==="