Initial import of FaRui Autoware Dual-Orin Stack
This commit is contained in:
li-shihao-code
Executable
+251
@@ -0,0 +1,251 @@
|
||||
#!/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.*。"
|
||||
+118
@@ -0,0 +1,118 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
from collections import deque
|
||||
|
||||
import rclpy
|
||||
from autoware_adapi_v1_msgs.msg import OperationModeState
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
|
||||
|
||||
|
||||
class OperationModeStateFallback(Node):
|
||||
def __init__(self):
|
||||
super().__init__("planning_debug_operation_mode_fallback")
|
||||
|
||||
self.topic = (
|
||||
self.declare_parameter("topic", "/system/operation_mode/state")
|
||||
.get_parameter_value()
|
||||
.string_value
|
||||
)
|
||||
self.period_sec = (
|
||||
self.declare_parameter("period_sec", 0.5).get_parameter_value().double_value
|
||||
)
|
||||
self.timeout_sec = (
|
||||
self.declare_parameter("timeout_sec", 1.5).get_parameter_value().double_value
|
||||
)
|
||||
|
||||
qos = QoSProfile(depth=1)
|
||||
qos.reliability = ReliabilityPolicy.RELIABLE
|
||||
qos.durability = DurabilityPolicy.TRANSIENT_LOCAL
|
||||
|
||||
self.pub = self.create_publisher(OperationModeState, self.topic, qos)
|
||||
|
||||
sub_qos = QoSProfile(depth=1)
|
||||
sub_qos.reliability = ReliabilityPolicy.RELIABLE
|
||||
sub_qos.durability = DurabilityPolicy.VOLATILE
|
||||
self.sub = self.create_subscription(
|
||||
OperationModeState, self.topic, self.on_state, sub_qos
|
||||
)
|
||||
|
||||
self.timer = self.create_timer(self.period_sec, self.on_timer)
|
||||
self.publishing_fallback = False
|
||||
self.last_msg_time = None
|
||||
self.own_stamps = deque(maxlen=32)
|
||||
|
||||
self.get_logger().info(
|
||||
f"Monitoring {self.topic}; publishing STOP fallback when no fresh message exists."
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def stamp_key(stamp):
|
||||
return stamp.sec, stamp.nanosec
|
||||
|
||||
def on_state(self, msg):
|
||||
if self.stamp_key(msg.stamp) in self.own_stamps:
|
||||
return
|
||||
self.last_msg_time = self.get_clock().now()
|
||||
|
||||
def has_external_publisher(self):
|
||||
own_name = self.get_name()
|
||||
own_namespace = self.get_namespace()
|
||||
|
||||
for info in self.get_publishers_info_by_topic(self.topic):
|
||||
if info.node_name != own_name or info.node_namespace != own_namespace:
|
||||
return True
|
||||
return False
|
||||
|
||||
def has_fresh_message(self):
|
||||
if self.last_msg_time is None:
|
||||
return False
|
||||
|
||||
age_sec = (self.get_clock().now() - self.last_msg_time).nanoseconds * 1e-9
|
||||
return age_sec <= self.timeout_sec
|
||||
|
||||
def on_timer(self):
|
||||
if self.has_fresh_message():
|
||||
if self.publishing_fallback:
|
||||
self.get_logger().info(
|
||||
f"Detected fresh {self.topic}; fallback is idle."
|
||||
)
|
||||
self.publishing_fallback = False
|
||||
return
|
||||
|
||||
msg = OperationModeState()
|
||||
msg.stamp = self.get_clock().now().to_msg()
|
||||
msg.mode = OperationModeState.STOP
|
||||
msg.is_autoware_control_enabled = False
|
||||
msg.is_in_transition = False
|
||||
msg.is_stop_mode_available = True
|
||||
msg.is_autonomous_mode_available = False
|
||||
msg.is_local_mode_available = False
|
||||
msg.is_remote_mode_available = False
|
||||
self.own_stamps.append(self.stamp_key(msg.stamp))
|
||||
self.pub.publish(msg)
|
||||
|
||||
if not self.publishing_fallback:
|
||||
reason = (
|
||||
"no external publisher"
|
||||
if not self.has_external_publisher()
|
||||
else f"no fresh message within {self.timeout_sec:.1f}s"
|
||||
)
|
||||
self.get_logger().warn(
|
||||
f"{reason} on {self.topic}; publishing STOP fallback for planning."
|
||||
)
|
||||
self.publishing_fallback = True
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = OperationModeStateFallback()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user