252 lines
11 KiB
Bash
Executable File
252 lines
11 KiB
Bash
Executable File
#!/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.*。"
|