#!/usr/bin/env bash
# 诊断从板 control、system 状态、ECAR CAN driver 和 socketcan 链路。

set -euo pipefail

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
WORKSPACE_ROOT="$(cd "${SCRIPT_DIR}/.." && pwd)"
ROS_SETUP_FILE="${SLAVE_ROS_SETUP_FILE:-/opt/ros/${ROS_DISTRO:-humble}/setup.bash}"

info() { printf '\n\033[0;32m[INFO]\033[0m %s\n' "$*"; }
warn() { printf '\n\033[1;33m[WARN]\033[0m %s\n' "$*"; }
run() {
  printf '\n$ %s\n' "$*"
  "$@" || true
}

echo_once() {
  local topic="$1"
  local timeout_sec="${2:-2}"

  printf '\n$ timeout %ss ros2 topic echo --once %s\n' "${timeout_sec}" "${topic}"
  timeout "${timeout_sec}"s ros2 topic echo --once "${topic}" || true
}

set +u
if [[ -f "${ROS_SETUP_FILE}" ]]; then
  source "${ROS_SETUP_FILE}"
fi
if [[ -f "${WORKSPACE_ROOT}/install/setup.bash" ]]; then
  source "${WORKSPACE_ROOT}/install/setup.bash"
else
  warn "Workspace setup not found: ${WORKSPACE_ROOT}/install/setup.bash"
fi
set -u

info "环境变量"
printf 'ROS_DOMAIN_ID=%s\n' "${ROS_DOMAIN_ID:-}"
printf 'RMW_IMPLEMENTATION=%s\n' "${RMW_IMPLEMENTATION:-}"
printf 'CYCLONEDDS_URI=%s\n' "${CYCLONEDDS_URI:-}"

info "相关节点"
run ros2 node list

info "Control/system 服务"
run ros2 service list
run ros2 service type /control/control_mode_request
run ros2 service type /system/operation_mode/change_autoware_control
run ros2 service type /system/operation_mode/change_operation_mode

info "Topic 端点检查"
topics=(
  /socket_can/from_can_bus
  /socket_can/to_can_bus
  /vehicle/status/control_mode
  /vehicle/status/velocity_status
  /vehicle/status/steering_status
  /vehicle/status/gear_status
  /vehicle/status/turn_indicators_status
  /vehicle/status/hazard_lights_status
  /vehicle/status/actuation_status
  /vehicle/status/steering_wheel_status
  /vehicle/status/door_status
  /localization/slip_angle
  /sensing/vehicle_velocity_converter/twist_with_covariance
  /planning/scenario_planning/trajectory
  /control/trajectory_follower/control_cmd
  /control/command/control_cmd
  /control/command/gear_cmd
  /control/vehicle_cmd_gate/operation_mode
  /system/operation_mode/state
  /system/operation_mode/availability
  /system/fail_safe/mrm_state
  /system/emergency/hazard_status
  /ecar_can_sender/command/adcu_drive_cmd
  /ecar_can_sender/command/adcu_brake_cmd
  /ecar_can_sender/command/adcu_steer_cmd
)

for topic in "${topics[@]}"; do
  run ros2 topic info -v "${topic}"
done

info "单次消息采样"
echo_once /vehicle/status/control_mode 2
echo_once /vehicle/status/velocity_status 2
echo_once /vehicle/status/steering_status 2
echo_once /vehicle/status/gear_status 2
echo_once /vehicle/status/actuation_status 2
echo_once /vehicle/status/steering_wheel_status 2
echo_once /localization/slip_angle 2
echo_once /control/vehicle_cmd_gate/operation_mode 2
echo_once /system/operation_mode/state 2
echo_once /system/operation_mode/availability 2
echo_once /system/fail_safe/mrm_state 2
echo_once /system/emergency/hazard_status 2
echo_once /control/command/control_cmd 2
echo_once /ecar_can_sender/command/adcu_drive_cmd 2
echo_once /socket_can/to_can_bus 2

info "结果解读"
cat <<'EOF'
从板状态链路:
  /socket_can/from_can_bus
    -> /ecar_can_receiver
    -> /vehicle/status/control_mode、/vehicle/status/velocity_status 等 Autoware 车辆状态
    -> /autoware_operation_mode_transition_manager + /vehicle_cmd_gate
    -> /system/operation_mode/state
    -> /system/operation_mode/availability、/system/fail_safe/mrm_state
    -> 主板 planning scenario_selector 和 velocity_smoother

控制命令链路:
  /planning/scenario_planning/trajectory
    -> /control/trajectory_follower/control_cmd
    -> /control/command/control_cmd
    -> /ecar_can_sender/command/adcu_drive_cmd
    -> /socket_can/to_can_bus

如果 /vehicle/status/control_mode 或 /control/vehicle_cmd_gate/operation_mode 缺失，
/system/operation_mode/state 就可能不发布。主板 planning 会卡在
"Waiting for operation mode state"，最终不输出 trajectory。

如果 /control/command/control_cmd 有消息，但 /ecar_can_sender/command/adcu_drive_cmd
不变化，说明驱动命令输入或命令超时路径仍然有问题。
EOF