#!/usr/bin/env bash
# 打印 RViz goal 到 planning trajectory 链路上的关键 topic/service 状态。

set -euo pipefail

GREEN='\033[0;32m'
YELLOW='\033[1;33m'
RED='\033[0;31m'
NC='\033[0m'

log_info() { echo -e "${GREEN}[INFO]${NC} $*"; }
log_warn() { echo -e "${YELLOW}[WARN]${NC} $*"; }
log_error() { echo -e "${RED}[ERROR]${NC} $*" >&2; }

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

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
  log_warn "未找到工作空间 setup 文件: ${workspace_root}/install/setup.bash"
  log_warn "如果当前 shell 没有手动 source 正确的 install，后续 ros2 查询可能看不到本工作空间的包和节点。"
fi
set -u

node_check() {
  local node="$1"
  echo
  log_info "检查 node: ${node}"
  if ros2 node list | grep -Fx "${node}" >/dev/null; then
    echo "存在"
  else
    log_warn "缺少 node: ${node}"
  fi
}

component_check() {
  local container="$1"
  echo
  log_info "检查 composable container: ${container}"
  if ! ros2 node list | grep -Fx "${container}" >/dev/null; then
    log_warn "容器不存在: ${container}"
    return
  fi

  if ! timeout 3s ros2 component list "${container}"; then
    log_warn "读取容器组件失败: ${container}"
  fi
}

topic_info() {
  local topic="$1"
  echo
  log_info "检查 topic 信息: ${topic}"
  if ! timeout 3s ros2 topic info -v "${topic}"; then
    log_warn "读取 topic 信息失败: ${topic}"
  fi
}

topic_once() {
  local topic="$1"
  echo
  log_info "读取一次 topic 消息: ${topic}"
  if ! timeout 3s ros2 topic echo --once "${topic}"; then
    log_warn "3 秒内没有收到消息: ${topic}"
  fi
}

topic_once_field() {
  local topic="$1"
  local field="$2"
  echo
  log_info "读取一次 topic 字段 ${field}: ${topic}"
  if ! timeout 3s ros2 topic echo --once --field "${field}" "${topic}"; then
    log_warn "3 秒内没有收到消息: ${topic}"
  fi
}

param_get() {
  local node="$1"
  local param="$2"
  echo
  log_info "读取参数: ${node} / ${param}"
  if ! timeout 3s ros2 param get "${node}" "${param}"; then
    log_warn "读取参数失败: ${node} / ${param}"
  fi
}

service_check() {
  local service="$1"
  echo
  log_info "检查 service: ${service}"
  if ros2 service list | grep -Fx "${service}" >/dev/null; then
    echo "存在"
  else
    log_warn "缺少 service: ${service}"
  fi
}

package_prefix_check() {
  local package="$1"
  echo
  log_info "检查 ROS 包安装路径: ${package}"
  local prefix
  if prefix="$(ros2 pkg prefix "${package}" 2>/dev/null)"; then
    echo "${prefix}"
  else
    log_warn "当前环境找不到 ROS 包: ${package}"
  fi
}

component_type_check() {
  local package="$1"
  local plugin="$2"
  echo
  log_info "检查 component plugin: ${package} / ${plugin}"
  if timeout 5s ros2 component types | grep -A20 -Fx "${package}" | grep -Fx "  ${plugin}" >/dev/null; then
    echo "存在"
  else
    log_warn "当前环境没有发现 plugin: ${package} / ${plugin}"
  fi
}

recent_behavior_container_log() {
  echo
  log_info "最近 behavior_planning_container 启动日志"

  local log_file
  local found="false"
  for log_file in $(ls -t "${HOME}/.ros/log"/component_container_mt_*.log 2>/dev/null | head -50); do
    if grep -q "behavior_planning_container" "${log_file}"; then
      echo "日志文件: ${log_file}"
      grep -E "behavior_planning_container|behavior_path_planner|Component constructor threw|outside_route|Load Library|Found class|Instantiate class|ERROR|FATAL|exception" "${log_file}" | tail -40
      found="true"
      break
    fi
  done

  if [[ "${found}" == "false" ]]; then
    log_warn "最近 50 个 component_container_mt 日志里没有找到 behavior_planning_container。"
  fi
}

recent_planning_failure_log() {
  echo
  log_info "最近 planning/component 失败日志"

  local log_root="${HOME}/.ros/log/latest"
  if [[ ! -d "${log_root}" ]]; then
    log_warn "未找到 ROS latest 日志目录: ${log_root}"
    return
  fi

  local pattern="behavior_planning_container|behavior_path_planner|behavior_velocity_planner|motion_planning_container|mission_planner_container|scenario_selector|glog_component|Component constructor threw|Failed to load|LoadLibrary|plugin|exception|parameter|must be initialized|not found|outside_route|FATAL|ERROR"
  if ! grep -R -n -E "${pattern}" "${log_root}" 2>/dev/null | tail -120; then
    log_warn "latest 日志里没有匹配到明显的 planning/component 失败。"
  fi
}

echo "ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-}"
echo "RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION:-}"
echo "CYCLONEDDS_URI=${CYCLONEDDS_URI:-}"

package_prefix_check "farui_launch"
package_prefix_check "autoware_behavior_path_planner"
package_prefix_check "autoware_behavior_velocity_planner"
package_prefix_check "autoware_behavior_path_lane_change_module"
package_prefix_check "autoware_behavior_path_external_request_lane_change_module"
package_prefix_check "autoware_behavior_path_static_obstacle_avoidance_module"
package_prefix_check "autoware_behavior_path_dynamic_obstacle_avoidance_module"
package_prefix_check "glog_component"
component_type_check "autoware_behavior_path_planner" "autoware::behavior_path_planner::BehaviorPathPlannerNode"
component_type_check "autoware_behavior_velocity_planner" "autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode"
component_type_check "glog_component" "GlogComponent"

echo
log_info "包含 routing/planning/scenario/operation/localization 的节点"
ros2 node list | grep -E 'routing|mission|scenario|planning|operation|localization|velocity_smoother' || true

node_check "/planning/scenario_planning/lane_driving/behavior_planning/behavior_planning_container"
node_check "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"
node_check "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner"
component_check "/planning/scenario_planning/lane_driving/behavior_planning/behavior_planning_container"
component_check "/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container"
component_check "/planning/mission_planning/mission_planner_container"

param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner" "launch_modules"
param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner" "slots"
param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner" "lane_change.prepare_duration"
param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner" "external_request_lane_change_right.enable_rtc"
param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner" "external_request_lane_change_left.enable_rtc"
param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner" "launch_modules"
param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner" "intersection.common.attention_area_length"
param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner" "merge_from_private.stopline_margin"
param_get "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner" "merge_from_private.stop_duration_sec"

service_check "/api/routing/set_route_points"
service_check "/planning/mission_planning/route_selector/main/set_waypoint_route"

topic_info "/map/vector_map"
topic_info "/map/vector_map_marker"
topic_info "/vehicle/status/velocity_status"
topic_info "/sensing/vehicle_velocity_converter/twist_with_covariance"
topic_info "/localization/kinematic_state"
topic_info "/system/operation_mode/state"
topic_info "/api/routing/state"
topic_info "/api/routing/route"
topic_info "/planning/mission_planning/route_selector/main/state"
topic_info "/planning/mission_planning/route"
topic_info "/planning/scenario_planning/scenario"
topic_info "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"
topic_info "/planning/scenario_planning/lane_driving/behavior_planning/path"
topic_info "/planning/scenario_planning/lane_driving/trajectory"
topic_info "/planning/scenario_planning/scenario_selector/trajectory"
topic_info "/planning/scenario_planning/velocity_smoother/trajectory"
topic_info "/planning/scenario_planning/trajectory"

topic_once_field "/map/vector_map" "header"
topic_once "/localization/kinematic_state"
topic_once "/system/operation_mode/state"
topic_once "/api/routing/state"
topic_once "/planning/mission_planning/route_selector/main/state"
topic_once "/planning/mission_planning/route"
topic_once "/planning/scenario_planning/scenario"
topic_once "/planning/scenario_planning/lane_driving/behavior_planning/path"
topic_once_field "/planning/scenario_planning/lane_driving/trajectory" "header"
topic_once "/planning/scenario_planning/lane_driving/trajectory"
topic_once_field "/planning/scenario_planning/trajectory" "header"
topic_once "/planning/scenario_planning/trajectory"

recent_behavior_container_log
recent_planning_failure_log

echo
log_info "结果解读"
echo "1. 如果 /map/vector_map 没有消息，定位仍可能正常，但 mission planning 无法生成 route。"
echo "2. 如果 /api/routing/state 缺失，或者不是 UNSET，RViz goal 不会真正设置新 route。"
echo "3. 如果点击 goal 后 /planning/mission_planning/route 缺失，检查 mission_planner 日志：goal 无效、route 为空、odometry 缺失或 lanelet map 缺失。"
echo "4. 如果 route 存在但 /planning/scenario_planning/scenario 缺失，优先检查 /system/operation_mode/state。"
echo "5. 如果 scenario 存在但 behavior/motion 输出缺失，按上面的 topic 链路找到第一个没有输出的 topic。"
echo "6. 如果 lane_driving trajectory 存在但最终 /planning/scenario_planning/trajectory 缺失，检查 operation mode 和 trajectory 时间戳延迟。"
echo "7. 如果 behavior_planning_container 或 behavior_path_planner 节点不存在，path 不会生成；先查 master 启动日志中的 component_container_mt 是否加载 autoware_behavior_path_planner 失败。"
echo "8. 如果日志出现 glog_component not found 或 GlogComponent plugin 缺失，说明当前运行环境没有正确构建/安装 glog_component，或 launch 中不应在该容器加载它。"
echo "9. 如果日志出现 must be initialized，说明组件参数 yaml 没有传入，或运行环境读到了旧版本/错误路径的 yaml。"
echo "10. external_request_lane_change 没有独立 yaml；它复用 lane_change.* 参数，并从 scene_module_manager 读取 external_request_lane_change_* 的 RTC/执行策略。"
echo "11. merge_from_private 没有独立 yaml；它的 merge_from_private.* 参数位于 behavior_velocity_planner/intersection.param.yaml，同时依赖 intersection.common.*。"