222

.gitignore

@@ -11,3 +11,5 @@ log
 .vscode
 
 datas
+
+running_logs

AGENTS.md

@@ -0,0 +1,237 @@
+# yiliao_ws 项目说明
+
+本文件是 `/home/sunrise/yiliao_ws` 工作区的项目级说明，主要记录当前比赛会用到的模块、接口、启动方式和调试注意事项。后续如果某个子目录内还有自己的 `AGENTS.md`，进入该子目录工作时以更近的说明为准。
+
+## 项目定位
+
+- 这是 RDKx5 机器人上的 ROS 2 Humble 工作区，当前主要服务于医疗赛道/竞速赛道任务。
+- 当前比赛主链路是调度型架构：底盘、雷达、相机、二维码、Nav2、VLM、TTS 等功能分别由独立模块完成，`src/racing_control` 只负责总调度。
+- 当前默认只走 Nav2 路径规划与跟随，`trajectory_guard` 只作为可选备用链路，不作为默认路径。
+- 比赛点位、路线和行为开关应优先放在 YAML/launch 参数中，不要把新采集的场地点位直接写死进 C++。
+- 修改比赛逻辑时优先保证流程稳定、有序、可调试；除非明确需要，不要在总调度节点里另起感知、规划或控制功能。
+
+## 环境与访问
+
+- 机器人 SSH：
+  ```bash
+  ssh sunrise@192.168.10.210
+  ```
+- 工作区根目录：
+  ```bash
+  /home/sunrise/yiliao_ws
+  ```
+- 运行 ROS 2 命令前通常需要：
+  ```bash
+  cd /home/sunrise/yiliao_ws
+  source /opt/ros/humble/setup.bash
+  source install/setup.bash
+  ```
+- 部分相机/Hobot 相关流程可能还需要：
+  ```bash
+  source /opt/tros/humble/setup.bash
+  ```
+- 远端仓库可能已有无关改动或运行日志。不要随意 `git reset`、`git checkout --` 或删除未确认文件。
+
+## 编译与测试
+
+- 编译单个包：
+  ```bash
+  cd /home/sunrise/yiliao_ws
+  source /opt/ros/humble/setup.bash
+  source install/setup.bash
+  colcon build --packages-select <package_name> --cmake-args -DBUILD_TESTING=ON
+  ```
+- 测试 `racing_control`：
+  ```bash
+  cd /home/sunrise/yiliao_ws
+  source /opt/ros/humble/setup.bash
+  source install/setup.bash
+  colcon test --packages-select racing_control
+  colcon test-result --verbose --test-result-base build/racing_control
+  ```
+- 不要在同一个工作区里同时启动多个 `colcon build`。
+- `ament_xmllint` 可能依赖远端 ROS schema。如果只有 xmllint 因网络/schema 临时失败，先重跑验证，再考虑改 XML。
+
+## 比赛模块总览
+
+### 总调度
+
+- 包路径：`src/racing_control`
+- 主节点：`src/racing_control/src/racing_control.cpp`
+- 配置：`src/racing_control/config/racing_control.yaml`
+- 启动：`src/racing_control/launch/racing_control.launch.py`
+- 点位转换说明：`src/racing_control/点位格式转换.md`
+- 设计原则：
+  - 只做比赛流程调度，不重复实现二维码、VLM、Nav2、底盘控制等子功能。
+  - 通过 `/sign4return` 控制二维码、VLM 和 Nav2 参数档位。
+  - 通过 Nav2 action 完成到点、路径规划和路径跟随。
+- 当前比赛流程：
+  - 导航到 `qr_pose` 附近。
+  - 一旦收到可解析二维码结果，立即选择顺/逆时针路线。
+  - 识别二维码后不再等待 QR 目标，按配置前往 `post_qr_pose` 和 `entry_pose`。
+  - 进入对应方向路线，按 `clockwise_waypoints` 或 `counterclockwise_waypoints` 执行。
+  - 到第 `vlm_waypoint_number` 个路线点时触发 VLM。
+  - home 前一个点到达后发布 `/sign4return=10` 调参，再发布 home goal。
+  - 最后返回方向对应的 `clockwise_home_pose` 或 `counterclockwise_home_pose`。
+- 使用的 Nav2 actions：
+  - `/navigate_to_pose`
+  - `/compute_path_through_poses`
+  - `/follow_path`
+- 常用启动参数：
+  ```bash
+  ros2 launch racing_control racing_control.launch.py auto_start:=false
+  ros2 launch racing_control racing_control.launch.py auto_start:=true
+  ros2 launch racing_control racing_control.launch.py enable_vlm_image_relay:=true
+  ```
+
+### 点位与路线参数
+
+- 点位数组使用 `[x, y, yaw_radians]`，坐标系由 `frame_id` 指定。
+- `racing_control.yaml` 中当前关键字段：
+  - `qr_pose`：二维码区域导航点。
+  - `post_qr_pose`：二维码识别后的后置过渡点，用于后续调参。
+  - `entry_pose`：进入正式路线前的入口点。
+  - `clockwise_waypoints`：顺时针路线点，不包含 home。
+  - `counterclockwise_waypoints`：逆时针路线点，不包含 home。
+  - `clockwise_home_pose`、`counterclockwise_home_pose`：最终 home 点。
+- `use_post_qr_pose: true` 时启用 QR 后置点；设为 `false` 时二维码识别后直接去 `entry_pose`。
+- `vlm_waypoint_number: 4` 表示正式路线第 4 个点是 VLM 拍摄点。不要依赖 `goal_004` 之类的点名。
+- 二维码方向解析：
+  - 文本包含 `顺` 或奇数数字时选择顺时针。
+  - 文本包含 `逆` 或偶数数字时选择逆时针。
+
+### Nav2 与轨迹保护
+
+- 包路径：`src/navigation/obstacle_nav2`
+- 主启动：`launch/obstacle_nav2.launch.py`
+- 轨迹保护启动：`launch/trajectory_guard.launch.py`
+- 运行时参数档位切换：`launch/nav2_profile_tuner.launch.py`
+- 关键配置：
+  - `config/nav2_profile_10.yaml`：普通/默认导航参数。
+  - `config/nav2_profile_11.yaml`：任务二路线参数。
+  - `config/trajectory_guard.yaml`：轨迹保护参数。
+- `trajectory_guard_node` 订阅 `/trajectory_guard/input_path`，属于备用链路；默认比赛流程不依赖它。
+- `nav2_profile_tuner` 监听 `/sign4return`，收到 10 或 11 后调用 Nav2 参数服务切换参数档位。
+- `obstacle_nav2.launch.py` 当前以里程计坐标为主，默认 global frame 是 `odom`；静态地图和 AMCL 相关配置留作备用。
+
+### 相机与二维码
+
+- 相机包：`src/car_usb_cam`
+- 二维码包：`src/qr_detection`
+- 常见相机话题：`/image`
+- 当前 VLM/调度链路期望的图像类型：`sensor_msgs/msg/CompressedImage`
+- 二维码结果话题：`/qr_results`，类型 `std_msgs/msg/String`
+- 二维码检测受 `/sign4return` 控制：
+  - `0`：启用二维码检测。
+  - `5`：关闭二维码检测。
+- 常用检查命令：
+  ```bash
+  ros2 topic echo /qr_results
+  ros2 topic info /image
+  ros2 topic hz /image
+  ```
+- 如果 `racing_control` 到二维码区域后不继续走，优先检查：
+  - 日志里是否出现 `QR result received`。
+  - `/qr_results` 是否真的有输出。
+  - 输出文本是否包含 `顺`、`逆` 或可解析数字。
+
+### VLM 与语音
+
+- 包路径：`src/vlm_detect`
+- 启动文件：
+  - `launch/vlm_detect.launch.py`：OpenAI 兼容 VLM 服务流程。
+  - `launch/local_vlm_adapter.launch.py`：本地 `hobot_llamacpp` 适配流程。
+- VLM 触发信号：`/sign4return=9`
+- VLM 结果话题：`/vlm_result`
+- TTS 服务：`/tts/speak`，类型 `origincar_msg/srv/Speak`
+- `vlm_detect` 的图像输入可通过 launch 参数 `image_topic` 配置。
+- `racing_control` 可选单帧 VLM 图像转发：
+  - `enable_vlm_image_relay: false` 默认关闭。
+  - 输入话题：`vlm_image_input_topic`，默认 `/image`。
+  - 输出话题：`vlm_image_output_topic`，默认 `/vlm_image`。
+  - 启用后，`racing_control` 在发布 `/sign4return=9` 前，把缓存到的一帧压缩图像发布到 `/vlm_image`。
+  - 若要让 VLM 使用该帧，VLM 需这样启动：
+    ```bash
+    ros2 launch vlm_detect vlm_detect.launch.py image_topic:=/vlm_image
+    ```
+- 当前 VLM 拍摄模式：
+  - `vlm_capture_mode: stop`：到 VLM 点停住，发布图像/触发信号，等待 `vlm_capture_wait_sec` 后继续。
+  - `vlm_capture_mode: pass_through`：保留的经过拍照模式，按 `pass_through_vlm_trigger_radius` 在接近 VLM 点时触发。
+
+### 底盘、雷达、里程计与消息
+
+- 底盘包：`src/origincar_base`
+  - 常见启动：`origincar_bringup.launch.py`、`base_serial.launch.py`、`ekf.launch.py`
+  - 负责底盘串口、IMU、里程计、EKF 和 TF 等基础能力。
+- 雷达驱动路径：`src/LSLIDAR_X_ROS2-20240228/src`
+- 障碍物检测包：`src/obstacle_scanner`
+  - 消费雷达数据，发布导航需要的障碍物信息。
+- 消息/服务包：`src/origincar_msg`
+  - 包含 `origincar_msg/srv/Speak`。
+- 当前 `racing_control` 期望里程计话题是 `/odom_combined`。
+
+### 启动编排
+
+- `src/my_robot_bringup/launch/master_launch.py` 是较完整的分阶段总启动，涉及底盘、雷达、TTS、相机、二维码、障碍物、规划和 VLM。
+- 当前 `racing_control` 流程会直接依赖 Nav2 actions，并结合 `obstacle_nav2`/轨迹保护相关节点。修改 `master_launch.py` 前，应确认现场实际是通过总 launch 还是多个终端分别启动。
+- `planner` 包里还有较旧或备用的 Hybrid A*、Pure Pursuit 等流程。除非当前 launch 明确使用它，否则先把它当作历史/备用路径看待。
+
+## 共享信号与话题
+
+- `/sign4return` 类型是 `std_msgs/msg/Int32`，被多个模块共享：
+  - `0`：启用二维码检测。
+  - `5`：关闭二维码检测。
+  - `9`：触发 VLM 拍摄/推理。
+  - `10`：应用普通 Nav2 参数档。
+  - `11`：应用任务二 Nav2 参数档。
+- 新增 `/sign4return` 数值前，必须检查所有订阅者，包括底盘、感知和导航参数切换节点。
+- 其他关键接口：
+  - `/qr_results`：二维码文本结果。
+  - `/vlm_result`：VLM 文本结果。
+  - `/tts/speak`：TTS 服务。
+  - `/image`：压缩相机流。
+  - `/vlm_image`：可选单帧 VLM 图像转发输出。
+  - `/trajectory_guard/input_path`：轨迹保护输入路径。
+  - `/cmd_vel`：真实运动控制通道，避免多个节点同时发布。
+
+## 调试注意事项
+
+- 不要让多个节点同时发布 `/cmd_vel`，除非明确 remap 掉其中一个。
+- 二维码区域后不继续走时：
+  - 看 `racing_control` 日志是否出现 `QR result received`。
+  - 直接 echo `/qr_results`。
+  - 确认二维码文本能被解析为顺/逆方向。
+- VLM 报 `No image` 或没有结果时：
+  - 检查 `ros2 topic info /image`。
+  - 如果启用转发，确认 `enable_vlm_image_relay:=true`，并且 VLM 使用 `image_topic:=/vlm_image`。
+  - 确认相机实际发布的是 `sensor_msgs/msg/CompressedImage`。
+- 路线段末端卡住时：
+  - 检查 `/odom_combined` 是否连续。
+  - 检查 `circle_goal_tolerance` 是否过小。
+  - 如果临时启用了 `trajectory_guard`，再检查它是否正常向 `/follow_path` 转发。
+- Nav2 在任务二前后行为差异大时：
+  - 查看 `nav2_profile_10.yaml` 和 `nav2_profile_11.yaml`。
+  - 查看 `nav2_profile_tuner` 日志里是否收到并应用 `/sign4return` 10/11。
+- launch 已启动但按空格不能开始比赛时，可能是 stdin 不是 TTY；可使用 `auto_start:=true`。
+- 更新现场点位时，优先修改 `src/racing_control/config/racing_control.yaml`，然后重新 build/install `racing_control`，确保 launch 读到安装后的配置。
+
+## 关键参数清单
+
+- `frame_id`：比赛点位所属坐标系。
+- `qr_pose`、`post_qr_pose`、`entry_pose`：二维码阶段到正式路线阶段的关键过渡点。
+- `clockwise_waypoints`、`counterclockwise_waypoints`：顺/逆时针路线点。
+- `clockwise_home_pose`、`counterclockwise_home_pose`：顺/逆方向 home 点。
+- `use_post_qr_pose`：是否启用二维码后置点。
+- `vlm_waypoint_number`：第几个正式路线点触发 VLM。
+- `vlm_capture_mode`：`stop` 或 `pass_through`。
+- `vlm_capture_wait_sec`：停住拍照模式下触发 VLM 后等待时间。
+- `enable_vlm_image_relay`：是否由 `racing_control` 把 `/image` 的单帧图像转发到 `/vlm_image`。
+- `vlm_image_input_topic`、`vlm_image_output_topic`：VLM 图像转发输入/输出话题。
+
+## 待后续补齐
+
+- 比赛日标准启动顺序：到底使用 `my_robot_bringup/master_launch.py`，还是分终端启动 `obstacle_nav2`、`vlm_detect`、`racing_control` 等节点。
+- 当前最权威的 Nav2 参数归属：`obstacle_nav2` 的 profile 看起来是比赛主路径，但 `planner` 和 `my_robot_bringup` 仍保留旧流程。
+- 不同部署模式下相机话题的最终类型。近期 `vlm_detect` 期望 `CompressedImage`，换相机驱动前要重新确认。
+- `/sign4return` 除 0、5、9、10、11 外，在底盘板和感知节点里的完整含义。
+- 现场最新点位来源、采集时间和对应 JSON/YAML 转换记录。

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml

@@ -15,7 +15,7 @@
     use_gps_ts: false                                       #雷达是否使用GPS授时
     scan_topic: /scan                                       #设置激光数据topic名称
     interface_selection: serial                             #接口选择:net 为网口,serial 为串口。
-    serial_port_: /dev/ttyCH343USB0                         #串口连接时的串口号
+    serial_port_: /dev/radar                      #串口连接时的串口号
     high_reflection: false                                  #M10_P雷达需填写该值,若不确定，请联系技术支持。
     compensation: false				                              #M10系列是否使用角度补偿功能
     pubScan: true                                           #是否发布scan话题

src/data_collection_tools/x5_udp_cmd_bridge.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+"""Receive safe short-lived drive commands from the PC and publish ROS Twist."""
+
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import socket
+import time
+
+import rclpy
+from geometry_msgs.msg import Twist
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="PC UDP to ROS cmd_vel_task bridge")
+    parser.add_argument("--bind", default="0.0.0.0")
+    parser.add_argument("--port", type=int, default=8765)
+    parser.add_argument("--topic", default="/cmd_vel_task")
+    parser.add_argument("--timeout", type=float, default=0.35)
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = rclpy.create_node("x5_udp_cmd_bridge")
+    publisher = node.create_publisher(Twist, args.topic, 10)
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    sock.bind((args.bind, args.port))
+    sock.settimeout(0.05)
+    last_packet = 0.0
+    current = (0.0, 0.0)
+    node.get_logger().info(f"UDP {args.bind}:{args.port} -> {args.topic}, timeout={args.timeout}s")
+
+    try:
+        while rclpy.ok():
+            try:
+                raw, _address = sock.recvfrom(1024)
+                data = json.loads(raw.decode("utf-8"))
+                v = float(data.get("v", 0.0))
+                w = float(data.get("w", 0.0))
+                if not (math.isfinite(v) and math.isfinite(w)):
+                    raise ValueError("non-finite command")
+                current = (max(-0.35, min(0.35, v)), max(-1.6, min(1.6, w)))
+                last_packet = time.monotonic()
+            except socket.timeout:
+                pass
+            except (ValueError, TypeError, json.JSONDecodeError) as exc:
+                node.get_logger().warn(f"ignore invalid UDP command: {exc}")
+
+            if time.monotonic() - last_packet > args.timeout:
+                current = (0.0, 0.0)
+            msg = Twist()
+            msg.linear.x, msg.angular.z = current
+            publisher.publish(msg)
+            rclpy.spin_once(node, timeout_sec=0.0)
+    finally:
+        publisher.publish(Twist())
+        sock.close()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/gc/项目总结_yiliao_ws.md

@@ -0,0 +1,429 @@
+# yiliao_ws 项目总结
+
+> **生成日期**: 2026-08-09  
+> **项目名称**: 智慧医疗2026（第21届全国大学生智能汽车竞赛·地瓜机器人赛项）  
+> **ROS 发行版**: ROS 2 Humble  
+> **机器人平台**: OriginCar (RDK X5, 四轮阿克曼转向)  
+> **工作区路径**: `/home/sunrise/yiliao_ws`  
+> **远程仓库**: `https://gitee.com/hikos/smart-healthcare-2026.git`
+
+---
+
+## 一、项目定位
+
+本项目是 RDK X5 机器人上的 ROS 2 Humble 工作区，服务于第21届全国大学生智能汽车竞赛的**医疗赛道**（智慧医疗）。整体采用**调度型架构**：底盘、雷达、相机、二维码、Nav2 导航、VLM 图生文、TTS 语音等功能由独立模块完成，`racing_control` 包作为总调度协调各模块完成比赛流程。
+
+---
+
+## 二、项目目录结构总览
+
+```
+yiliao_ws/
+├── src/                          # 源代码（所有 ROS 2 包）
+│   ├── racing_control/           # 🎯 总调度（比赛流程控制）
+│   ├── origincar_base/           # 🔧 底盘驱动（串口通信 + EKF + 阿克曼）
+│   ├── navigation/               # 🧭 导航相关包集合
+│   │   ├── obstacle_nav2/        #   主 Nav2 导航栈（含参数调谐器）
+│   │   ├── gc_navigation2_slamtoolbox/  #   仿真版 SLAM + Nav2
+│   │   ├── gc_navigation2_real/  #   实车版 SLAM + Nav2
+│   │   ├── gc_navigation_fish/   #   另一套导航配置
+│   │   ├── cyy_navigation2/      #   备选导航方案
+│   │   ├── cyy_slamtoolbox/      #   备选 SLAM 方案
+│   │   └── zbw_slamtoolbox/      #   另一套 SLAM 方案
+│   ├── qr_detection/             # 📷 二维码检测
+│   ├── vlm_detect/               # 🤖 VLM 图生文 + TTS 语音播报
+│   ├── car_usb_cam/              # 📸 USB 相机驱动
+│   ├── obstacle_scanner/         # 📡 激光雷达障碍物检测
+│   ├── origincar_msg/            # 📦 自定义 ROS 2 消息
+│   ├── origincar_description/    # 🤖 机器人 URDF 模型描述
+│   ├── origincar_birdseye/       # 🐦 鸟瞰图转换
+│   ├── my_robot_bringup/         # 🚀 总启动编排（master_launch.py）
+│   ├── planner/                  # 🗺️ 备选规划器（Hybrid A* 等）
+│   ├── past_control/             # 📝 历史控制方案
+│   ├── mtran/                    # 🧠 MTRAN 模型
+│   ├── ground_slam/              # 🗺️ 地面 SLAM
+│   ├── car_image_proc/           # 🖼️ 图像处理
+│   ├── yolov8_launch/            # 🎯 YOLOv8 目标检测
+│   ├── data_collection_tools/    # 📊 数据采集工具
+│   ├── LSLIDAR_X_ROS2-20240228/  # 📡 镭神激光雷达 ROS 2 驱动
+│   └── gc/                       # 📄 项目文档存放
+├── build/                        # colcon 构建输出
+├── install/                      # colcon 安装输出
+├── scripts/                      # 辅助脚本（PID跟踪、陀螺仪标定等）
+├── bashes/                       # 自动脚本（WiFi连接、雷达驱动切换）
+├── config/dds/                   # DDS 配置
+├── datas/                        # 采集数据
+├── models/                       # 模型文件
+├── docs/                         # 文档
+├── bag_outputs/                  # ROS Bag 输出
+├── running_logs/                 # 运行日志
+├── test_results/                 # 测试结果
+├── vlm_server.py                 # VLM 推理服务（FastAPI）
+├── cali.npz / cali.txt           # 标定数据
+├── path_follower_demo.py         # 路径跟随演示
+└── 标定.py                       # 标定脚本
+```
+
+---
+
+## 三、核心模块详解
+
+### 3.1 racing_control — 比赛总调度 ⭐
+
+| 项目 | 说明 |
+|------|------|
+| **包路径** | `src/racing_control` |
+| **语言** | C++ (rclcpp) |
+| **主节点** | `racing_control.cpp` |
+| **配置** | `config/racing_control.yaml` |
+| **启动** | `launch/racing_control.launch.py` |
+
+**设计原则**：只做比赛流程调度，不重复实现二维码、VLM、Nav2、底盘控制等子功能。通过 `/sign4return` 信号控制各子模块的开关和参数切换。
+
+**比赛流程（12个阶段）**：
+
+```mermaid
+graph TD
+    A[Idle 等待启动] --> B[NavigateToQr 二维码点导航]
+    B --> C[NavigatePostQr 二维码后置点导航]
+    C --> D[WaitForQr 二维码识别+TTS]
+    D --> E[NavigateToEntry 通道入口导航]
+    E --> F[SwitchToTask2Profile 任务二参数切换]
+    F --> G[ComputeCirclePath 任务二轨迹规划]
+    G --> H[ExecuteCirclePath 任务二轨迹执行]
+    H --> I[SwitchToNormalProfile 恢复导航参数]
+    I --> J[WaitForVlm 图生文+TTS]
+    J --> K[ReturnOrigin 返回原点]
+    K --> L[Finished 比赛完成]
+```
+
+**关键接口**：
+- Nav2 Actions: `/navigate_to_pose`, `/compute_path_through_poses`, `/follow_path`
+- 控制信号: `/sign4return` (Int32)，数值含义：0=启用QR, 5=关闭QR, 9=触发VLM, 10=普通Nav2参数, 11=任务二Nav2参数
+- 接收: `/qr_results`, `/vlm_result`, `/odom_combined`
+
+**关键参数（YAML 配置）**：
+| 参数 | 默认值 | 说明 |
+|------|--------|------|
+| `qr_pose` | 坐标数组 | 二维码区域导航点 |
+| `post_qr_pose` | 坐标数组 | 二维码后置过渡点 |
+| `entry_pose` | 坐标数组 | 正式路线入口点 |
+| `clockwise_waypoints` | 坐标数组 | 顺时针路线点（不含home） |
+| `counterclockwise_waypoints` | 坐标数组 | 逆时针路线点（不含home） |
+| `clockwise_home_pose` | 坐标数组 | 顺时针home点 |
+| `counterclockwise_home_pose` | 坐标数组 | 逆时针home点 |
+| `vlm_waypoint_number` | 2 | 第几个路线点触发VLM |
+| `vlm_capture_mode` | stop | VLM拍摄模式 (stop/pass_through) |
+| `use_post_qr_pose` | false | 是否启用QR后置点 |
+| `enable_vlm_image_relay` | false | 是否转发单帧图像给VLM |
+
+**二维码方向解析规则**：
+- 文本包含 `顺` 或奇数数字 → 顺时针
+- 文本包含 `逆` 或偶数数字 → 逆时针
+
+---
+
+### 3.2 origincar_base — 底盘驱动
+
+| 项目 | 说明 |
+|------|------|
+| **包路径** | `src/origincar_base` |
+| **语言** | C++ + Python |
+| **主节点** | `origincar_base.cpp` (串口通信), `cmd_vel_to_ackermann_drive.py` (运动转换) |
+| **配置** | `config/ekf.yaml`, `config/imu.yaml` |
+
+**功能**：
+- STM32 串口通信 (`/dev/ttyACM0`, 921600bps)
+- 航迹推算 + Mahony AHRS 姿态解算
+- EKF 融合 (odom + IMU → `/odom_combined`)
+- Twist → AckermannDriveStamped 转换 (wheelbase=0.143m)
+- 帧协议: 24字节收（0x7B/0x7D帧头尾）/ 11字节发
+
+**阿克曼底盘模式** (`akmcar:=true`)：
+```
+cmd_vel(Twist) → cmd_vel_to_ackermann_drive.py → ackermann_cmd(AckermannDriveStamped) → STM32
+```
+
+**关键修改（must_know.md 记录）**：
+- EKF 帧名统一 (`odom_combined` → `odom`)
+- 底盘驱动默认不再发布 TF（避免与 EKF 冲突）
+- 里程计协方差可参数化配置
+- 串口异常捕获防崩溃
+
+---
+
+### 3.3 obstacle_nav2 — 主 Nav2 导航栈
+
+| 项目 | 说明 |
+|------|------|
+| **包路径** | `src/navigation/obstacle_nav2` |
+| **启动文件** | `obstacle_nav2.launch.py`, `nav2_profile_tuner.launch.py`, `trajectory_guard.launch.py` |
+
+**功能**：
+- 集成 Nav2 导航（MPPI 控制器 + SmacPlannerHybrid 规划器）
+- 消费 `/obstacles` 转化为 costmap 障碍物层
+- **参数档位切换**：`nav2_profile_tuner` 监听 `/sign4return`，10→普通参数，11→任务二参数
+- **轨迹保护**（备用）：`trajectory_guard` 订阅 `/trajectory_guard/input_path`
+
+**关键参数文件**：
+| 文件 | 用途 |
+|------|------|
+| `nav2_params.yaml` | Nav2 主参数 |
+| `nav2_profile_10.yaml` | 普通/默认导航参数档 |
+| `nav2_profile_11.yaml` | 任务二路线参数档 |
+| `trajectory_guard.yaml` | 轨迹保护参数 |
+| `nav2_params_basic_tracking.yaml` | 基础跟踪参数 |
+
+**MPPI 参数（CPU 优化后）**：
+- `time_steps`: 20, `batch_size`: 200 (原1000)
+- `PathAlignCritic.cost_weight`: 7.0 (降低，避障优先)
+
+**代价地图**：`inflation_radius`: 0.2m, `cost_scaling_factor`: 3.0
+
+---
+
+### 3.4 qr_detection — 二维码检测
+
+| 项目 | 说明 |
+|------|------|
+| **包路径** | `src/qr_detection` |
+| **语言** | C++ |
+| **输入** | `/image` (CompressedImage) |
+| **输出** | `/qr_results` (std_msgs/String) |
+| **控制** | `/sign4return=0` 启用, `/sign4return=5` 关闭 |
+
+---
+
+### 3.5 vlm_detect — VLM 图生文 + TTS
+
+| 项目 | 说明 |
+|------|------|
+| **包路径** | `src/vlm_detect` |
+| **语言** | Python |
+| **主节点** | `vlm_node.py` (VLM推理), `tts_server.py` (语音合成) |
+
+**数据流**：
+```
+USB Camera → /image → vlm_node → /vlm_result
+                    → qr_dete_node → /qr_results
+                                     ↓
+                              qr_tts_bridge → /tts/speak → Piper TTS → 语音输出
+```
+
+**VLM 触发**：`/sign4return=9`  
+**VLM 服务**：外部 FastAPI 服务 (`vlm_server.py`)，运行 AndesVL-1B 模型，OpenAI 兼容 API  
+**TTS**：Piper TTS + paplay 本地语音合成
+
+---
+
+### 3.6 obstacle_scanner — 雷达障碍物检测
+
+| 项目 | 说明 |
+|------|------|
+| **包路径** | `src/obstacle_scanner` |
+| **语言** | C++ (使用 OpenCV + Eigen) |
+| **功能** | 基于角度聚类+圆拟合的 LiDAR 障碍物检测 |
+
+---
+
+### 3.7 my_robot_bringup — 总启动编排
+
+| 项目 | 说明 |
+|------|------|
+| **包路径** | `src/my_robot_bringup` |
+| **启动文件** | `launch/master_launch.py` |
+
+**分阶段启动时间线**：
+```
+t=0s   底盘驱动 + EKF + TF + IMU (origincar_bringup)
+t=2s   激光雷达 (lslidar_driver)
+t=5s   SLAM 建图 (slam_toolbox)
+t=8s   Nav2 导航栈
+t=10s  VLM 图生文 + TTS 语音播报
+```
+
+**常用启动参数**：`use_base`, `use_lidar`, `use_slam`, `use_nav2`, `use_vlm`, `use_tts`, `akmcar`, `vlm_host`
+
+---
+
+### 3.8 其他包
+
+| 包名 | 说明 |
+|------|------|
+| `origincar_description` | 机器人 URDF 模型 (0.276×0.214×0.211m, wheelbase=0.143m) |
+| `origincar_msg` | 自定义消息 (Data.msg, Sign.msg) 和服务 (Speak.srv) |
+| `car_usb_cam` | USB 相机驱动 (hobot_usb_cam)，发布 `/image` |
+| `planner` | 备选规划方案（Hybrid A*, Pure Pursuit, Ackermann Hybrid A*） |
+| `origincar_birdseye` | 鸟瞰图/IPM 变换 |
+| `ground_slam` | 地面 SLAM 建图 |
+| `yolov8_launch` | YOLOv8 目标检测启动 |
+| `car_image_proc` | 图像处理工具 |
+| `past_control` | 历史控制方案（竞速参考） |
+| `mtran` | MTRAN 模型（含依赖和文档） |
+| `data_collection_tools` | X5 UDP 指令桥接 |
+
+---
+
+## 四、导航相关包集合 (`src/navigation/`)
+
+本工作区历史上有多个 SLAM/Nav2 方案迭代：
+
+| 包名 | 用途 | 状态 |
+|------|------|:----:|
+| `obstacle_nav2` | 主 Nav2 导航栈 + 参数调谐 | ✅ **当前使用** |
+| `gc_navigation2_slamtoolbox` | 仿真版 SLAM + Nav2 (Gazebo) | 📦 仿真用 |
+| `gc_navigation2_real` | 实车版 SLAM + Nav2 (无 Gazebo) | 📦 实车参考 |
+| `gc_navigation_fish` | 另一套导航配置 | 📦 备用 |
+| `cyy_navigation2` | 备选导航方案 | 📦 历史 |
+| `cyy_slamtoolbox` | 备选 SLAM | 📦 历史 |
+| `zbw_slamtoolbox` | 另一套 SLAM | 📦 历史 |
+
+**关键差异（仿真 vs 实车）**：
+| 方面 | 仿真版 | 实车版 |
+|------|--------|--------|
+| `use_sim_time` | true | false |
+| odom_frame | `odom` | `odom_combined` (EKF融合后) |
+| 底盘驱动 | Gazebo plugin | origincar_base 串口驱动 |
+| 激光雷达 | Gazebo ray plugin | lslidar_driver (镭神 N10) |
+
+---
+
+## 五、TF 树结构
+
+```
+map ──→ odom ──→ base_footprint ──→ base_link ──→ laser_link
+ ↑        ↑           ↑                ↑
+slam    EKF融合     静态TF            URDF
+```
+
+---
+
+## 六、关键共享信号 `/sign4return`
+
+| 值 | 含义 | 消费者 |
+|:--:|------|--------|
+| 0 | 启用二维码检测 | qr_detection |
+| 5 | 关闭二维码检测 | qr_detection |
+| 9 | 触发 VLM 拍摄/推理 | vlm_detect |
+| 10 | 应用普通 Nav2 参数档 | nav2_profile_tuner |
+| 11 | 应用任务二 Nav2 参数档 | nav2_profile_tuner |
+
+---
+
+## 七、环境与操作
+
+### 7.1 机器人访问
+```bash
+ssh sunrise@192.168.10.210
+```
+
+### 7.2 环境初始化
+```bash
+cd /home/sunrise/yiliao_ws
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+# 部分相机流程可能还需要: source /opt/tros/humble/setup.bash
+```
+
+### 7.3 编译
+```bash
+# 编译单个包
+colcon build --packages-select <package_name> --cmake-args -DBUILD_TESTING=ON
+
+# 全量编译（推荐 --symlink-install 使 Python 修改即时生效）
+colcon build --symlink-install
+```
+
+### 7.4 运行测试
+```bash
+colcon test --packages-select racing_control
+colcon test-result --verbose --test-result-base build/racing_control
+```
+
+### 7.5 常用启动命令
+```bash
+# 全部启动（比赛模式）
+ros2 launch my_robot_bringup master_launch.py
+
+# 仅底盘调试
+ros2 launch my_robot_bringup master_launch.py use_lidar:=false use_slam:=false use_nav2:=false use_vlm:=false
+
+# 仅建图
+ros2 launch my_robot_bringup master_launch.py use_nav2:=false use_vlm:=false
+
+# 比赛控制（自动开始）
+ros2 launch racing_control racing_control.launch.py auto_start:=true
+
+# 带图像转发的比赛控制
+ros2 launch racing_control racing_control.launch.py enable_vlm_image_relay:=true
+```
+
+---
+
+## 八、硬件与传感器
+
+| 部件 | 型号 | 说明 |
+|------|------|------|
+| 主控 | RDK X5 | 10 TOPS 算力 |
+| 底盘 | OriginCar | 四轮阿克曼转向，276×214×211mm，轴距143mm |
+| 激光雷达 | 镭神 N10 | 单线TOF，0.15~12m |
+| 深度相机 | 光鉴 Aurora930 | |
+| USB相机 | USB 2.0 Camera | 发布 `/image` |
+| IMU | 板载 | 与里程计进行 EKF 融合 |
+
+---
+
+## 九、已知问题与调试经验
+
+### 9.1 已修复的关键问题
+
+| 问题 | 修复方案 |
+|------|----------|
+| TF 树断连（两节点争抢 odom→base_link） | EKF 改为发布 odom→base_footprint；底盘驱动默认不发布 TF |
+| 串口异常崩溃 | 为 Stm32_Serial.read() 添加 try-catch |
+| LiDAR 扫描出现空扇形面 (60°) | 恢复 `scan_num` 动态计算公式 |
+| URDF git 冲突 | 清除冲突标记，统一轮距参数 |
+| CPU 负载过高 (77%) | MPPI batch_size 1000→200, throttle_scans 1→10, EKF 30Hz→20Hz |
+| cmd_vel_to_ackermann_drive.py 权限 | chmod +x |
+
+### 9.2 调试检查清单
+
+- **二维码区域后不继续走**：检查 `/qr_results` 是否有输出，文本是否可解析为顺/逆
+- **VLM 无结果**：检查 `/image` 话题，确认 enable_vlm_image_relay 和 VLM image_topic 配置
+- **路线末端卡住**：检查 `/odom_combined` 连续性，circle_goal_tolerance 是否过小
+- **Nav2 行为异常**：检查 nav2_profile_tuner 是否收到/应用了 /sign4return 10/11
+- **雷达无数据**：`lsusb` 检查物理连接，`ls /dev/tty*` 检查 ttyACM0/ACM1/CH340
+
+---
+
+## 十、辅助脚本
+
+| 脚本 | 用途 |
+|------|------|
+| `scripts/PIDtracking.py` | PID 轨迹跟踪 |
+| `scripts/measure_turning_radius.py` | 测量转弯半径 |
+| `scripts/fit_gyro_bias.py` | 陀螺仪偏置拟合 |
+| `scripts/publish_sine_path.py` | 发布正弦路径 |
+| `scripts/udp_to_cmdvel.py` | UDP→cmd_vel 桥接 |
+| `scripts/set_volume.py` | 音量设置 |
+| `bashes/radar-driver-switch.sh` | 雷达驱动自动切换 |
+| `bashes/auto-wifi-connect.sh` | WiFi 自动连接 |
+| `vlm_server.py` | VLM 推理服务 (FastAPI, AndesVL-1B) |
+| `path_follower_demo.py` | 路径跟随演示 |
+
+---
+
+## 十一、数据与文档
+
+| 类型 | 路径 | 说明 |
+|------|------|------|
+| 地图文件 | `src/navigation/*/maps/` | .pgm + .yaml + .posegraph |
+| 采集数据 | `datas/` | data4, data5, PIDtracking等 |
+| 调试记录 | `调试记录.md` | 2026/6/1 ~ 6/13 调试过程 |
+| 变更说明 | `must_know.md` | mo_new→HEAD 101文件变更详解 |
+| 启动指南 | `master_launch_使用指南.md` | master_launch.py 完整说明 |
+| 竞赛方案 | `竞赛方案_第21届智能汽车竞赛地瓜机器人赛项.md` | 正式竞赛方案文档 |
+| 点位移转 | `src/racing_control/点位格式转换.md` | JSON→YAML 点位转换流程 |
+| VLM数据流 | `src/vlm_detect/dataflow.md` | VLM+QR→TTS 数据流图 |
+| AGENTS.md | `/home/sunrise/yiliao_ws/AGENTS.md` | 项目级说明 |
+| CLAUDE.md | `/home/sunrise/yiliao_ws/CLAUDE.md` | 另一视角的项目说明（含 test_ws 内容） |

src/navigation/obstacle_nav2/CMakeLists.txt

@@ -107,6 +107,13 @@ install(TARGETS
 install(DIRECTORY include/ DESTINATION include)
 install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME})
 
+# Python scripts
+install(PROGRAMS
+  scripts/initial_pose_to_tf.py
+  scripts/static_map_publisher.py
+  DESTINATION share/${PROJECT_NAME}/scripts
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp

src/navigation/obstacle_nav2/behavior_tree/nav_through_poses_ackermann.xml

@@ -0,0 +1,41 @@
+<!--
+  Ackermann NavigateThroughPoses behavior tree.
+
+  Planning failure is retried once after the global costmap has been cleared
+  and given one update period. FollowPath is not replanned periodically while
+  it is RUNNING. A controller failure gets one local-costmap retry, then one
+  short backup followed by one final through-poses plan and follow attempt.
+  Any final failure aborts the navigation action.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="NavigateThroughPosesOnce">
+      <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
+        <ReactiveSequence>
+          <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
+          <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
+        </ReactiveSequence>
+        <Sequence name="ClearGlobalAndWait">
+          <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+          <Wait wait_duration="1"/>
+        </Sequence>
+      </RecoveryNode>
+
+      <Fallback name="FollowOrSingleBackupReplan">
+        <RecoveryNode number_of_retries="1" name="FollowWithLocalClear">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+
+        <Sequence name="BackupReplanAndFinalFollow">
+          <BackUp backup_dist="0.04" backup_speed="0.20"/>
+          <ReactiveSequence>
+            <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
+            <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
+          </ReactiveSequence>
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+        </Sequence>
+      </Fallback>
+    </Sequence>
+  </BehaviorTree>
+</root>

src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml

@@ -1,33 +1,31 @@
 <!--
-  阿克曼底盘行为树：无原地旋转（Spin）
+  Ackermann NavigateToPose behavior tree.
-  恢复行为序列：清除代价地图 → 后退 → 等待
+
+  Compute the path once and keep the same path while FollowPath is RUNNING.
+  If following fails, clear the local costmap and retry the same path once.
+  If that also fails, back up 0.04 m, compute one final path, and follow it
+  once. Any failure in the final branch ends the navigation action.
 -->
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
-    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+    <Sequence name="NavigateWithSingleBackupReplan">
-      <PipelineSequence name="NavigateWithReplanning">
+      <RecoveryNode number_of_retries="1" name="ComputeInitialPath">
-        <RateController hz="1.0">
+        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+        <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
-            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+      </RecoveryNode>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+
-          </RecoveryNode>
+      <Fallback name="FollowOrSingleBackupReplan">
-        </RateController>
+        <RecoveryNode number_of_retries="1" name="FollowWithLocalClear">
-        <RecoveryNode number_of_retries="1" name="FollowPath">
           <FollowPath path="{path}" controller_id="FollowPath"/>
           <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
-      </PipelineSequence>
+
-      <ReactiveFallback name="RecoveryFallback">
+        <Sequence name="BackupReplanAndFinalFollow">
-        <GoalUpdated/>
+          <BackUp backup_dist="0.04" backup_speed="0.20"/>
-        <RoundRobin name="RecoveryActions">
+          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-          <Sequence name="ClearingActions">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
-            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </Sequence>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+      </Fallback>
-          </Sequence>
+    </Sequence>
-          <BackUp backup_dist="0.80" backup_speed="0.18"/>
-          <Wait wait_duration="0.3"/>
-        </RoundRobin>
-      </ReactiveFallback>
-    </RecoveryNode>
   </BehaviorTree>
 </root>

src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_event_replan

@@ -0,0 +1,33 @@
+<!--
+  阿克曼底盘行为树：无原地旋转（Spin）
+  恢复行为序列：清除代价地图 → 后退 → 等待
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </RecoveryNode>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="FollowPath">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+      </PipelineSequence>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
+        <RoundRobin name="RecoveryActions">
+          <Sequence name="ClearingActions">
+            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </Sequence>
+          <BackUp backup_dist="0.80" backup_speed="0.18"/>
+          <Wait wait_duration="0.3"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_single_backup

@@ -0,0 +1,35 @@
+<!--
+  Ackermann NavigateToPose behavior tree.
+
+  Compute the path once and keep the same path while FollowPath is RUNNING.
+  A new global plan is requested only after FollowPath returns FAILURE and the
+  outer RecoveryNode starts another navigation attempt. This avoids replacing
+  a valid path every second while MPPI is still evaluating it.
+
+  Recovery actions intentionally do not include Spin or BackUp because this
+  robot uses an Ackermann drive model.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <Sequence name="NavigateOnce">
+        <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+          <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+        </RecoveryNode>
+        <RecoveryNode number_of_retries="1" name="FollowPath">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+      </Sequence>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
+        <Sequence name="ClearingActions">
+          <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+          <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          <Wait wait_duration="1"/>
+        </Sequence>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260808

@@ -0,0 +1,340 @@
+# ============================================================================
+# nav2_params.yaml — Odometry-only obstacle navigation
+#
+# No static map, no AMCL, no SLAM.
+# Both costmaps are rolling windows in odom. The launch file can rewrite every
+# global_frame leaf when a different connected odometry frame is required.
+# Global planner: Smac Hybrid A* (Reeds-Shepp)
+# Local controller: MPPI (Ackermann)
+# 速度快但是雷达容易掉
+# ============================================================================
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom_combined
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # Injected by obstacle_nav2.launch.py from this package's share directory.
+    default_nav_to_pose_bt_xml: ""
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 36
+      model_dt: 0.05
+      batch_size: 1000
+      vx_std: 0.22
+      vy_std: 0.0
+      wz_std: 0.45
+      vx_max: 1.00
+      vx_min: -0.75
+      vy_max: 0.0
+      wz_max: 1.5
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 4.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 100000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 10.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 10
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 5
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: false
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: false
+      plugins: ["obstacle_array_layer", "inflation_layer"]
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.35
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      transform_tolerance: 0.5
+      global_frame: map
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: false
+      width: 8
+      height: 8
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        enabled: true
+        map_subscribe_transient_local: true
+        subscribe_to_updates: false
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 2.0
+        inflation_radius: 0.35
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.15
+      allow_unknown: false
+      max_iterations: 1000000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 3.0
+      change_penalty: 0.0
+      non_straight_penalty: 1.2
+      cost_penalty: 2.0
+      retrospective_penalty: 0.015
+      # 5 m covers the rolling planning horizon without the startup and memory
+      # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: map
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.5
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: False
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [1.00, 0.0, 2.0]
+    min_velocity: [-0.75, 0.0, -2.0]
+    max_accel: [3.73, 0.0, 3.2]
+    max_decel: [-1.1, 0.0, -4.5]
+    odom_topic: /odom_combined
+    odom_duration: 0.1
+    deadband_velocity: [0.03, 0.0, 0.03]
+    velocity_timeout: 1.0

src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260809

@@ -0,0 +1,340 @@
+# ============================================================================
+# nav2_params.yaml — Odometry-only obstacle navigation
+#
+# No static map, no AMCL, no SLAM.
+# Both costmaps are rolling windows in odom. The launch file can rewrite every
+# global_frame leaf when a different connected odometry frame is required.
+# Global planner: Smac Hybrid A* (Reeds-Shepp)
+# Local controller: MPPI (Ackermann)
+# 比之前速度慢了点
+# ============================================================================
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom_combined
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # Injected by obstacle_nav2.launch.py from this package's share directory.
+    default_nav_to_pose_bt_xml: ""
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 36
+      model_dt: 0.05
+      batch_size: 1000
+      vx_std: 0.22
+      vy_std: 0.0
+      wz_std: 0.45
+      vx_max: 1.00
+      vx_min: -0.75
+      vy_max: 0.0
+      wz_max: 1.5
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 4.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 100000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 10.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 10
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 5
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: false
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: false
+      plugins: ["obstacle_array_layer", "inflation_layer"]
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.35
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      transform_tolerance: 0.5
+      global_frame: map
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: false
+      width: 8
+      height: 8
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        enabled: true
+        map_subscribe_transient_local: true
+        subscribe_to_updates: false
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 2.0
+        inflation_radius: 0.35
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.15
+      allow_unknown: false
+      max_iterations: 1000000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 3.0
+      change_penalty: 0.0
+      non_straight_penalty: 1.2
+      cost_penalty: 2.0
+      retrospective_penalty: 0.015
+      # 5 m covers the rolling planning horizon without the startup and memory
+      # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: map
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.5
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: False
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.5, 0.0, 2.0]
+    min_velocity: [-0.75, 0.0, -2.0]
+    max_accel: [3.73, 0.0, 3.2]
+    max_decel: [-1.1, 0.0, -4.5]
+    odom_topic: /odom_combined
+    odom_duration: 0.1
+    deadband_velocity: [0.03, 0.0, 0.03]
+    velocity_timeout: 1.0

src/navigation/obstacle_nav2/config/nav2_profile_10.yaml

@@ -6,12 +6,13 @@
 # global_frame leaf when a different connected odometry frame is required.
 # Global planner: Smac Hybrid A* (Reeds-Shepp)
 # Local controller: MPPI (Ackermann)
+# 速度快但是雷达容易掉
 # ============================================================================
 
 bt_navigator:
   ros__parameters:
     use_sim_time: False
-    global_frame: odom
+    global_frame: map
     robot_base_frame: base_footprint
     odom_topic: /odom_combined
     bt_loop_duration: 50
@@ -70,16 +71,16 @@ bt_navigator_rclcpp_node:
 controller_server:
   ros__parameters:
     use_sim_time: False
-    controller_frequency: 20.0
+    controller_frequency: 15.0
     FollowPath:
       plugin: "nav2_mppi_controller::MPPIController"
-      time_steps: 36
+      time_steps: 40
-      model_dt: 0.05
+      model_dt: 0.06666666666666666
-      batch_size: 1000
+      batch_size: 900
-      vx_std: 0.75
+      vx_std: 0.25
       vy_std: 0.0
-      wz_std: 0.4
+      wz_std: 0.45
-      vx_max: 1.75
+      vx_max: 1.00
       vx_min: -0.75
       vy_max: 0.0
       wz_max: 1.5
@@ -101,7 +102,7 @@ controller_server:
       GoalCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 5.0
+        cost_weight: 6.0
         threshold_to_consider: 1.4
       GoalAngleCritic:
         enabled: true
@@ -111,21 +112,21 @@ controller_server:
       PreferForwardCritic:
         enabled: false
         cost_power: 1
-        cost_weight: 0.0
+        cost_weight: 7.0
         threshold_to_consider: 0.5
       CostCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 3.81
+        cost_weight: 5.0
         critical_cost: 300.0
         consider_footprint: true
-        collision_cost: 1000000.0
+        collision_cost: 100000.0
         near_goal_distance: 1.0
         trajectory_point_step: 2
       PathAlignCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 14.0
+        cost_weight: 8.0
         max_path_occupancy_ratio: 0.05
         trajectory_point_step: 4
         threshold_to_consider: 0.5
@@ -135,13 +136,13 @@ controller_server:
         enabled: true
         cost_power: 1
         cost_weight: 5.0
-        offset_from_furthest: 5
+        offset_from_furthest: 10
         threshold_to_consider: 1.4
       PathAngleCritic:
         enabled: true
         cost_power: 1
         cost_weight: 2.0
-        offset_from_furthest: 4
+        offset_from_furthest: 5
         threshold_to_consider: 0.5
         max_angle_to_furthest: 1.0
         forward_preference: false
@@ -180,7 +181,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.20
       always_send_full_costmap: True
   local_costmap_client:
     ros__parameters:
@@ -195,17 +196,22 @@ global_costmap:
       update_frequency: 1.0
       publish_frequency: 1.0
       transform_tolerance: 0.5
-      global_frame: odom
+      global_frame: map
       robot_base_frame: base_footprint
       use_sim_time: False
-      rolling_window: true
+      rolling_window: false
-      width: 10
+      width: 8
-      height: 10
+      height: 8
       resolution: 0.05
       footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
       footprint_padding: 0.02
-      track_unknown_space: false
+      track_unknown_space: true
-      plugins: ["obstacle_array_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        enabled: true
+        map_subscribe_transient_local: true
+        subscribe_to_updates: false
       obstacle_array_layer:
         plugin: "obstacle_nav2::ObstacleArrayLayer"
         enabled: true
@@ -218,8 +224,8 @@ global_costmap:
         extra_inflation: 0.02
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
+        cost_scaling_factor: 2.0
-        inflation_radius: 0.55
+        inflation_radius: 0.35
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
@@ -236,20 +242,20 @@ planner_server:
       plugin: "nav2_smac_planner/SmacPlannerHybrid"
       downsample_costmap: false
       downsampling_factor: 1
-      tolerance: 0.25
+      tolerance: 0.15
       allow_unknown: false
       max_iterations: 1000000
       max_on_approach_iterations: 1000
-      max_planning_time: 5.0
+      max_planning_time: 25.0
       motion_model_for_search: "REEDS_SHEPP"
       angle_quantization_bins: 72
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 3.0
       minimum_turning_radius: 0.40
-      reverse_penalty: 1.0
+      reverse_penalty: 1.6
-      change_penalty: 0.0
+      change_penalty: 2.0
       non_straight_penalty: 1.2
-      cost_penalty: 2.0
+      cost_penalty: 4.0
       retrospective_penalty: 0.015
       # 5 m covers the rolling planning horizon without the startup and memory
       # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
@@ -258,7 +264,7 @@ planner_server:
       viz_expansions: false
       smooth_path: True
       smoother:
-        max_iterations: 1000
+        max_iterations: 700
         w_smooth: 0.3
         w_data: 0.2
         tolerance: 1.0e-10
@@ -294,7 +300,7 @@ behavior_server:
     wait:
       plugin: "nav2_behaviors/Wait"
       wait_duration: 0.5
-    global_frame: odom
+    global_frame: map
     robot_base_frame: base_footprint
     transform_tolerance: 0.5
     use_sim_time: False
@@ -324,10 +330,10 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [1.75, 0.0, 1.5]
+    max_velocity: [1.00, 0.0, 2.0]
-    min_velocity: [-0.75, 0.0, -1.5]
+    min_velocity: [-0.75, 0.0, -2.0]
-    max_accel: [2.5, 0.0, 3.2]
+    max_accel: [3.73, 0.0, 3.2]
-    max_decel: [-2.5, 0.0, -3.2]
+    max_decel: [-1.1, 0.0, -4.5]
     odom_topic: /odom_combined
     odom_duration: 0.1
     deadband_velocity: [0.03, 0.0, 0.03]

src/navigation/obstacle_nav2/config/nav2_profile_10.yaml.bak_20260807_global_inflation_055

@@ -0,0 +1,339 @@
+# ============================================================================
+# nav2_params.yaml — Odometry-only obstacle navigation
+#
+# No static map, no AMCL, no SLAM.
+# Both costmaps are rolling windows in odom. The launch file can rewrite every
+# global_frame leaf when a different connected odometry frame is required.
+# Global planner: Smac Hybrid A* (Reeds-Shepp)
+# Local controller: MPPI (Ackermann)
+# ============================================================================
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom_combined
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # Injected by obstacle_nav2.launch.py from this package's share directory.
+    default_nav_to_pose_bt_xml: ""
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 36
+      model_dt: 0.05
+      batch_size: 1000
+      vx_std: 0.22
+      vy_std: 0.0
+      wz_std: 0.4
+      vx_max: 1.00
+      vx_min: -0.75
+      vy_max: 0.0
+      wz_max: 1.5
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 0.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 100000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 10
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 5
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: false
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: false
+      plugins: ["obstacle_array_layer", "inflation_layer"]
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      transform_tolerance: 0.5
+      global_frame: map
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: false
+      width: 8
+      height: 8
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        enabled: true
+        map_subscribe_transient_local: true
+        subscribe_to_updates: false
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: false
+      max_iterations: 1000000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 1.0
+      change_penalty: 0.0
+      non_straight_penalty: 1.2
+      cost_penalty: 2.0
+      retrospective_penalty: 0.015
+      # 5 m covers the rolling planning horizon without the startup and memory
+      # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: map
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.5
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: False
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [1.00, 0.0, 1.5]
+    min_velocity: [-0.75, 0.0, -1.5]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-4.5, 0.0, -4.5]
+    odom_topic: /odom_combined
+    odom_duration: 0.1
+    deadband_velocity: [0.03, 0.0, 0.03]
+    velocity_timeout: 1.0

src/navigation/obstacle_nav2/config/nav2_profile_11.yaml

@@ -12,7 +12,7 @@
 bt_navigator:
   ros__parameters:
     use_sim_time: False
-    global_frame: odom
+    global_frame: map
     robot_base_frame: base_footprint
     odom_topic: /odom_combined
     bt_loop_duration: 50
@@ -213,17 +213,22 @@ global_costmap:
       update_frequency: 5.0
       publish_frequency: 3.0
       transform_tolerance: 0.5
-      global_frame: odom
+      global_frame: map
       robot_base_frame: base_footprint
       use_sim_time: False
-      rolling_window: true
+      rolling_window: false
-      width: 10
+      width: 8
-      height: 10
+      height: 8
       resolution: 0.05
       footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
       footprint_padding: 0.02
-      track_unknown_space: false
+      track_unknown_space: true
-      plugins: ["obstacle_array_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        enabled: true
+        map_subscribe_transient_local: true
+        subscribe_to_updates: false
       obstacle_array_layer:
         plugin: "obstacle_nav2::ObstacleArrayLayer"
         enabled: true
@@ -311,7 +316,7 @@ behavior_server:
     wait:
       plugin: "nav2_behaviors/Wait"
       wait_duration: 0.5
-    global_frame: odom
+    global_frame: map
     robot_base_frame: base_footprint
     transform_tolerance: 0.5
     use_sim_time: False

src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py

@@ -22,14 +22,15 @@ from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
+    ExecuteProcess,
     GroupAction,
     IncludeLaunchDescription,
     SetEnvironmentVariable,
 )
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression
-from launch_ros.actions import SetRemap
+from launch_ros.actions import Node, SetRemap
 from nav2_common.launch import RewrittenYaml
 
 
@@ -48,17 +49,20 @@ def generate_launch_description():
     start_base = LaunchConfiguration('start_base', default='true')
     start_lidar = LaunchConfiguration('start_lidar', default='true')
     start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
+    start_map_publisher = LaunchConfiguration('start_map_publisher', default='true')
+    map_yaml_path = LaunchConfiguration('map_yaml_path')
 
     nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
-    fastdds_profile_path = os.path.join(pkg_dir, 'config', 'fastdds_udp_only.xml')
     nav_to_pose_bt_path = os.path.join(
         pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
+    nav_through_poses_bt_path = os.path.join(
+        pkg_dir, 'behavior_tree', 'nav_through_poses_ackermann.xml')
     configured_params = RewrittenYaml(
         source_file=nav2_param_path,
         root_key='',
         param_rewrites={
-            'global_frame': global_frame,
             'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
+            'default_nav_through_poses_bt_xml': nav_through_poses_bt_path,
         },
         convert_types=True,
     )
@@ -95,6 +99,37 @@ def generate_launch_description():
         condition=IfCondition(start_obstacle_scanner),
     )
 
+    # ========================== Static Map Publisher ==========================
+    static_map_publisher_script = os.path.join(pkg_dir, 'scripts', 'static_map_publisher.py')
+    static_map_publisher = ExecuteProcess(
+        cmd=[
+            FindExecutable(name='python3'),
+            static_map_publisher_script,
+            '--ros-args',
+            '-p', ['yaml_filename:=', map_yaml_path],
+            '-p', 'publish_rate:=1.0',
+            '-p', 'map_frame:=map',
+        ],
+        output='screen',
+        emulate_tty=True,
+        condition=IfCondition(start_map_publisher),
+    )
+
+    # ========================== Initial Pose -> TF (map->odom) ================
+    initial_pose_to_tf_script = os.path.join(pkg_dir, 'scripts', 'initial_pose_to_tf.py')
+    initial_pose_to_tf = ExecuteProcess(
+        cmd=[
+            FindExecutable(name='python3'),
+            initial_pose_to_tf_script,
+            '--ros-args',
+            '-p', 'odom_frame:=odom',
+            '-p', 'base_frame:=base_footprint',
+            '-p', 'map_frame:=map',
+        ],
+        output='screen',
+        emulate_tty=True,
+    )
+
     # ========================== Nav2 Navigation ===============================
     navigation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -140,13 +175,20 @@ def generate_launch_description():
             'start_obstacle_scanner',
             default_value='true',
             description='Also start the obstacle_scanner node'),
+        DeclareLaunchArgument(
+            'start_map_publisher',
+            default_value='true',
+            description='Start the static map publisher node'),
+        DeclareLaunchArgument(
+            'map_yaml_path',
+            default_value='/home/sunrise/yiliao_ws/src/map/nav2_costmap_map.yaml',
+            description='Path to the map YAML file'),
 
-        SetEnvironmentVariable(
-            name='FASTRTPS_DEFAULT_PROFILES_FILE',
-            value=fastdds_profile_path),
 
         safe_base_bringup,
         lslidar_launch,
         obstacle_scanner_launch,
+        static_map_publisher,
+        initial_pose_to_tf,
         navigation_launch,
     ])

src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py.bak_20260809_through_bt

@@ -0,0 +1,191 @@
+#!/usr/bin/env python3
+# ============================================================================
+# obstacle_nav2.launch.py
+#
+# Odometry-only Nav2 navigation using obstacle_scanner detections.
+#
+# Architecture:
+#   origincar_base (bringup)    — chassis serial driver + EKF + TF
+#   lslidar_driver              — lidar → /scan
+#   obstacle_scanner (optional) — /scan → /obstacles
+#   Nav2 navigation_launch      — rolling costmaps + Hybrid A* + MPPI
+#
+# No map_server, AMCL, or slam_toolbox in the default path.
+#
+# Usage:
+#   ros2 launch obstacle_nav2 obstacle_nav2.launch.py
+#   ros2 launch obstacle_nav2 obstacle_nav2.launch.py enable_motion:=true
+# ============================================================================
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    GroupAction,
+    IncludeLaunchDescription,
+    SetEnvironmentVariable,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node, SetRemap
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    """Odometry-only obstacle navigation."""
+
+    pkg_dir = get_package_share_directory('obstacle_nav2')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+
+    # ========================== Launch Arguments ==============================
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    global_frame = LaunchConfiguration('global_frame', default='odom')
+    use_static_map = LaunchConfiguration('use_static_map', default='false')
+    map_yaml = LaunchConfiguration('map_yaml', default='')
+    enable_motion = LaunchConfiguration('enable_motion', default='true')
+    start_base = LaunchConfiguration('start_base', default='true')
+    start_lidar = LaunchConfiguration('start_lidar', default='true')
+    start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
+    start_map_publisher = LaunchConfiguration('start_map_publisher', default='true')
+    map_yaml_path = LaunchConfiguration('map_yaml_path')
+
+    nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
+    nav_to_pose_bt_path = os.path.join(
+        pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
+    configured_params = RewrittenYaml(
+        source_file=nav2_param_path,
+        root_key='',
+        param_rewrites={
+            'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
+        },
+        convert_types=True,
+    )
+    base_cmd_vel_topic = PythonExpression([
+        "'/cmd_vel' if '", enable_motion,
+        "' == 'true' else '/cmd_vel_hardware_disabled'",
+    ])
+
+    # ========================== Base Bringup ==================================
+    origincar_bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [get_package_share_directory('origincar_base'),
+             '/launch', '/origincar_bringup.launch.py']),
+        condition=IfCondition(start_base),
+    )
+    safe_base_bringup = GroupAction([
+        SetRemap(src='cmd_vel', dst=base_cmd_vel_topic),
+        origincar_bringup,
+    ])
+
+    # ========================== Lidar Driver ==================================
+    lslidar_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [get_package_share_directory('lslidar_driver'),
+             '/launch', '/lsn10_launch.py']),
+        condition=IfCondition(start_lidar),
+    )
+
+    # ========================== Obstacle Scanner ==============================
+    obstacle_scanner_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [get_package_share_directory('obstacle_scanner'),
+             '/launch', '/obstacle_scanner.launch.py']),
+        condition=IfCondition(start_obstacle_scanner),
+    )
+
+    # ========================== Static Map Publisher ==========================
+    static_map_publisher_script = os.path.join(pkg_dir, 'scripts', 'static_map_publisher.py')
+    static_map_publisher = ExecuteProcess(
+        cmd=[
+            FindExecutable(name='python3'),
+            static_map_publisher_script,
+            '--ros-args',
+            '-p', ['yaml_filename:=', map_yaml_path],
+            '-p', 'publish_rate:=1.0',
+            '-p', 'map_frame:=map',
+        ],
+        output='screen',
+        emulate_tty=True,
+        condition=IfCondition(start_map_publisher),
+    )
+
+    # ========================== Initial Pose -> TF (map->odom) ================
+    initial_pose_to_tf_script = os.path.join(pkg_dir, 'scripts', 'initial_pose_to_tf.py')
+    initial_pose_to_tf = ExecuteProcess(
+        cmd=[
+            FindExecutable(name='python3'),
+            initial_pose_to_tf_script,
+            '--ros-args',
+            '-p', 'odom_frame:=odom',
+            '-p', 'base_frame:=base_footprint',
+            '-p', 'map_frame:=map',
+        ],
+        output='screen',
+        emulate_tty=True,
+    )
+
+    # ========================== Nav2 Navigation ===============================
+    navigation_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
+        launch_arguments={
+            'use_sim_time': use_sim_time,
+            'params_file': configured_params,
+            'autostart': 'true',
+        }.items(),
+    )
+
+    # ========================== Assembly ======================================
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation (Gazebo) clock'),
+        DeclareLaunchArgument(
+            'global_frame',
+            default_value='odom',
+            description='Global frame for both costmaps'),
+        DeclareLaunchArgument(
+            'use_static_map',
+            default_value='false',
+            description='Reserved: enable static map + AMCL (not yet implemented)'),
+        DeclareLaunchArgument(
+            'map_yaml',
+            default_value='',
+            description='Reserved: path to map YAML file'),
+        DeclareLaunchArgument(
+            'enable_motion',
+            default_value='true',
+            description='Route Nav2 cmd_vel to the real base topic'),
+        DeclareLaunchArgument(
+            'start_base',
+            default_value='true',
+            description='Start the base driver, EKF, and robot TF publishers'),
+        DeclareLaunchArgument(
+            'start_lidar',
+            default_value='true',
+            description='Start the lidar driver'),
+        DeclareLaunchArgument(
+            'start_obstacle_scanner',
+            default_value='true',
+            description='Also start the obstacle_scanner node'),
+        DeclareLaunchArgument(
+            'start_map_publisher',
+            default_value='true',
+            description='Start the static map publisher node'),
+        DeclareLaunchArgument(
+            'map_yaml_path',
+            default_value='/home/sunrise/yiliao_ws/src/map/nav2_costmap_map.yaml',
+            description='Path to the map YAML file'),
+
+
+        safe_base_bringup,
+        lslidar_launch,
+        obstacle_scanner_launch,
+        static_map_publisher,
+        initial_pose_to_tf,
+        navigation_launch,
+    ])

src/navigation/obstacle_nav2/scripts/initial_pose_to_tf.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+"""
+initial_pose_to_tf.py — /initialpose → map→odom 的 TF 变换
+
+启动后先发布 identity TF (map 与 odom 重合) 作为回退值。
+收到 /initialpose 后查 TF (odom→base_footprint)，反算 map→odom 并更新。
+
+使用: ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped ...
+或在 RViz 中用 2D Pose Estimate 工具设定。
+"""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped
+from tf2_ros import TransformListener, Buffer, StaticTransformBroadcaster
+from tf2_ros import LookupException, ConnectivityException, ExtrapolationException
+
+
+class InitialPoseToTF(Node):
+    def __init__(self):
+        super().__init__('initial_pose_to_tf')
+        self.declare_parameter('odom_frame', 'odom')
+        self.declare_parameter('base_frame', 'base_footprint')
+        self.declare_parameter('map_frame', 'map')
+
+        self.odom_frame_ = self.get_parameter('odom_frame').value
+        self.base_frame_ = self.get_parameter('base_frame').value
+        self.map_frame_ = self.get_parameter('map_frame').value
+
+        self.tf_buffer_ = Buffer()
+        self.tf_listener_ = TransformListener(self.tf_buffer_, self)
+        self.static_broadcaster_ = StaticTransformBroadcaster(self)
+
+        self.initial_pose_sub_ = self.create_subscription(
+            PoseWithCovarianceStamped, '/initialpose',
+            self._initial_pose_callback, 10)
+
+        self.get_logger().info(
+            f'waiting /initialpose '
+            f'(map={self.map_frame_}, odom={self.odom_frame_}, base={self.base_frame_})')
+
+        # fallback identity TF after 1s
+        self._identity_timer = self.create_timer(1.0, self._publish_identity)
+
+    def _publish_identity(self):
+        self._identity_timer.cancel()
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = self.map_frame_
+        t.child_frame_id = self.odom_frame_
+        t.transform.rotation.w = 1.0
+        self.static_broadcaster_.sendTransform(t)
+        self.get_logger().info('Published identity map→odom TF (fallback)')
+
+    def _initial_pose_callback(self, msg: PoseWithCovarianceStamped):
+        try:
+            odom_to_base = self.tf_buffer_.lookup_transform(
+                self.odom_frame_, self.base_frame_,
+                rclpy.time.Time(), rclpy.duration.Duration(seconds=1.0))
+        except (LookupException, ConnectivityException, ExtrapolationException) as e:
+            self.get_logger().warn(f'TF lookup failed: {e}')
+            return
+
+        map_to_base = msg.pose.pose
+        ob_t = odom_to_base.transform.translation
+        ob_r = odom_to_base.transform.rotation
+
+        # q_base_odom = q_odom_base^(-1)
+        q_bo = (-ob_r.x, -ob_r.y, -ob_r.z, ob_r.w)
+        # q_map_odom = q_map_base * q_base_odom
+        x1, y1, z1, w1 = (
+            map_to_base.orientation.x, map_to_base.orientation.y,
+            map_to_base.orientation.z, map_to_base.orientation.w)
+        x2, y2, z2, w2 = q_bo
+        qx = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
+        qy = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
+        qz = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
+        qw = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
+
+        # R(q_mo) * p_odom_base
+        px, py, pz = ob_t.x, ob_t.y, ob_t.z
+        rx = (1 - 2*qy*qy - 2*qz*qz)*px + (2*qx*qy - 2*qz*qw)*py + (2*qx*qz + 2*qy*qw)*pz
+        ry = (2*qx*qy + 2*qz*qw)*px + (1 - 2*qx*qx - 2*qz*qz)*py + (2*qy*qz - 2*qx*qw)*pz
+        rz = (2*qx*qz - 2*qy*qw)*px + (2*qy*qz + 2*qx*qw)*py + (1 - 2*qx*qx - 2*qy*qy)*pz
+
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = self.map_frame_
+        t.child_frame_id = self.odom_frame_
+        t.transform.translation.x = map_to_base.position.x - rx
+        t.transform.translation.y = map_to_base.position.y - ry
+        t.transform.translation.z = map_to_base.position.z - rz
+        t.transform.rotation.x = qx
+        t.transform.rotation.y = qy
+        t.transform.rotation.z = qz
+        t.transform.rotation.w = qw
+        self.static_broadcaster_.sendTransform(t)
+        self.get_logger().info(
+            f'Updated map→odom: pos=({t.transform.translation.x:.3f},'
+            f' {t.transform.translation.y:.3f})')
+
+
+def main():
+    rclpy.init()
+    node = InitialPoseToTF()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/navigation/obstacle_nav2/scripts/static_map_publisher.py

@@ -0,0 +1,92 @@
+#!/usr/bin/env python3
+"""static_map_publisher.py — 从 YAML+PNG 加载并发布 /map OccupancyGrid。"""
+
+import os, math
+import numpy as np
+from PIL import Image
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, DurabilityPolicy
+from nav_msgs.msg import OccupancyGrid, MapMetaData
+from geometry_msgs.msg import Pose
+import yaml
+
+
+class StaticMapPublisher(Node):
+    def __init__(self):
+        super().__init__('static_map_publisher')
+        self.declare_parameter('yaml_filename', '')
+        self.declare_parameter('publish_rate', 1.0)
+        self.declare_parameter('map_frame', 'map')
+
+        path = self.get_parameter('yaml_filename').value
+        rate = self.get_parameter('publish_rate').value
+        frame = self.get_parameter('map_frame').value
+        if not path:
+            raise RuntimeError('yaml_filename required')
+
+        self._grid_ = self._load(path, frame)
+        map_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
+        self._pub_ = self.create_publisher(OccupancyGrid, '/map', map_qos)
+        self._timer_ = self.create_timer(1.0 / max(0.1, rate), self._publish)
+        self.get_logger().info(f'map published on /map ({rate} Hz)')
+
+    def _load(self, yaml_path, frame):
+        d = os.path.dirname(os.path.abspath(yaml_path))
+        with open(yaml_path) as f:
+            cfg = yaml.safe_load(f)
+        img = Image.open(os.path.join(d, cfg['image'])).convert('L')
+        w, h = img.size
+        data = np.array(img, dtype=np.int32)
+        mode = cfg.get('mode', 'trinary')
+        negate = int(cfg.get('negate', 0))
+        occ_th = float(cfg.get('occupied_thresh', 0.65))
+        free_th = float(cfg.get('free_thresh', 0.196))
+
+        if mode == 'trinary':
+            grid = np.full_like(data, -1, dtype=np.int8)
+            grid[data == 0] = 100
+            grid[data >= 254] = 0
+        else:
+            if negate:
+                data = 255 - data
+            grid = np.full_like(data, -1, dtype=np.int8)
+            grid[data > occ_th * 255] = 100
+            grid[data < free_th * 255] = 0
+
+        grid = np.flipud(grid)
+        origin = cfg.get('origin', [0, 0, 0])
+        msg = OccupancyGrid()
+        msg.header.frame_id = frame
+        msg.info = MapMetaData()
+        msg.info.resolution = float(cfg.get('resolution', 0.05))
+        msg.info.width = w
+        msg.info.height = h
+        msg.info.origin = Pose()
+        msg.info.origin.position.x = float(origin[0])
+        msg.info.origin.position.y = float(origin[1])
+        yaw = float(origin[2]) if len(origin) > 2 else 0.0
+        msg.info.origin.orientation.w = math.cos(yaw / 2)
+        msg.info.origin.orientation.z = math.sin(yaw / 2)
+        msg.data = grid.flatten().tolist()
+        return msg
+
+    def _publish(self):
+        self._grid_.header.stamp = self.get_clock().now().to_msg()
+        self._pub_.publish(self._grid_)
+
+
+def main():
+    rclpy.init()
+    try:
+        rclpy.spin(StaticMapPublisher())
+    except RuntimeError as e:
+        rclpy.get_logger('static_map_publisher').fatal(str(e))
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/origincar_base/include/origincar_base/log.hpp

@@ -25,6 +25,8 @@ public:
   bool finishFrame(uint64_t frame_id, TimePoint now = Clock::now());
   bool cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now = Clock::now());
   bool makeSummaryLineIfDue(TimePoint now, std::string *line);
+  void setPrintToTerminal(bool enabled);
+  bool printToTerminal() const;
 
   const std::string &logPath() const;
   bool isOpen() const;
@@ -50,6 +52,7 @@ private:
   TimePoint last_summary_time_;
   std::string log_path_;
   std::ofstream log_file_;
+  bool print_to_terminal_{false};
 };
 }  // namespace origincar_base_logging
 

src/origincar_base/include/origincar_base/origincar_base.h

@@ -237,6 +237,7 @@ private:
 	string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
 	string scan_topic_, wall_config_path_, combined_odom_topic_;
 	bool publish_tf_;
+	bool scan_odom_timing_to_terminal_;
 	double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_;
 	int wall_scan_stride_;
 	std::string cmd_vel;

src/origincar_base/include/origincar_base/wall_kalman_filter.hpp

@@ -13,6 +13,23 @@ struct WallKalmanNoise
   double theta;
 };
 
+enum class WallScanTrust
+{
+  Reject,
+  Weak,
+  Medium,
+  High,
+};
+
+struct WallScanUpdatePolicy
+{
+  bool accept;
+  WallScanTrust trust;
+  WallKalmanNoise noise;
+  double innovation_distance;
+  std::string reason;
+};
+
 class WallKalmanFilter
 {
 public:
@@ -31,6 +48,11 @@ private:
   WallKalmanNoise process_noise_per_second_;
 };
 
+WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose,
+                                                const Pose2D &scan_pose,
+                                                const LocalizationQuality &quality,
+                                                bool stationary);
+
 }  // namespace origincar_wall
 
 #endif  // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_

src/origincar_base/launch/base_serial.launch.py

@@ -8,7 +8,7 @@ def generate_launch_description():
          'serial_baud_rate': 921600,
          'serial_read_timeout_ms': 20,
          'tx_period_ms': 20,
-         'cmd_watchdog_timeout_ms': 150,
+         'cmd_watchdog_timeout_ms': 500,
          'control_period_ms': 50,
          'robot_frame_id': 'base_footprint',
          'odom_frame_id': 'odom',

src/origincar_base/src/log.cpp

@@ -164,6 +164,16 @@ bool ScanOdomTimingLogger::makeSummaryLineIfDue(TimePoint now, std::string *line
   return true;
 }
 
+void ScanOdomTimingLogger::setPrintToTerminal(bool enabled)
+{
+  print_to_terminal_ = enabled;
+}
+
+bool ScanOdomTimingLogger::printToTerminal() const
+{
+  return print_to_terminal_;
+}
+
 const std::string &ScanOdomTimingLogger::logPath() const
 {
   return log_path_;

src/origincar_base/src/origincar_base.cpp

@@ -307,6 +307,10 @@ void origincar_base::Apply_Wall_Update()
 
 void origincar_base::Print_Timing_Log_If_Due()
 {
+  if (!scan_odom_timing_to_terminal_)
+  {
+    return;
+  }
   std::string timing_summary;
   if (scan_odom_timing_logger_.makeSummaryLineIfDue(std::chrono::steady_clock::now(), &timing_summary))
   {
@@ -639,6 +643,7 @@ origincar_base::origincar_base()
   this->declare_parameter<std::string>("scan_topic", "/scan");
   this->declare_parameter<std::string>("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
   this->declare_parameter<bool>("publish_tf", true);
+  this->declare_parameter<bool>("scan_odom_timing_to_terminal", false);
   this->declare_parameter<int>("wall_scan_stride", 2);
   this->declare_parameter<double>("laser_x", 0.0);
   this->declare_parameter<double>("laser_y", 0.0);
@@ -665,6 +670,7 @@ origincar_base::origincar_base()
   this->get_parameter("scan_topic", scan_topic_);
   this->get_parameter("wall_config_path", wall_config_path_);
   this->get_parameter("publish_tf", publish_tf_);
+  this->get_parameter("scan_odom_timing_to_terminal", scan_odom_timing_to_terminal_);
   this->get_parameter("wall_scan_stride", wall_scan_stride_);
   laser_pose_ = {
       this->get_parameter("laser_x").as_double(),
@@ -702,6 +708,7 @@ origincar_base::origincar_base()
       0.0,
   };
   wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose);
+  scan_odom_timing_logger_.setPrintToTerminal(scan_odom_timing_to_terminal_);
   Robot_Pos.X = static_cast<float>(initial_pose.x);
   Robot_Pos.Y = static_cast<float>(initial_pose.y);
   Robot_Pos.Z = static_cast<float>(initial_pose.theta);

src/origincar_base/src/wall_kalman_filter.cpp

@@ -6,6 +6,24 @@
 namespace origincar_wall
 {
 
+namespace
+{
+WallKalmanNoise highTrustNoise()
+{
+  return {0.015, 0.015, 0.025};
+}
+
+WallKalmanNoise mediumTrustNoise()
+{
+  return {0.045, 0.045, 0.060};
+}
+
+WallKalmanNoise weakTrustNoise()
+{
+  return {0.10, 0.10, 0.12};
+}
+}  // namespace
+
 WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose)
     : pose_(initial_pose),
       covariance_{0.10, 0.10, 0.10},
@@ -63,4 +81,75 @@ WallKalmanNoise WallKalmanFilter::covariance() const
   return covariance_;
 }
 
+WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose,
+                                                const Pose2D &scan_pose,
+                                                const LocalizationQuality &quality,
+                                                bool stationary)
+{
+  WallScanUpdatePolicy policy{false, WallScanTrust::Reject, weakTrustNoise(), 0.0, ""};
+  policy.innovation_distance = std::hypot(scan_pose.x - filter_pose.x, scan_pose.y - filter_pose.y);
+
+  if (!quality.has_mean_error)
+  {
+    policy.reason = "missing mean error";
+    return policy;
+  }
+  if (quality.wall_count < 3)
+  {
+    policy.reason = "too few walls";
+    return policy;
+  }
+  if (quality.mean_error > 0.15)
+  {
+    policy.reason = "mean error too high";
+    return policy;
+  }
+
+  if (stationary && policy.innovation_distance > 0.04)
+  {
+    policy.reason = "stationary innovation too large";
+    return policy;
+  }
+  if (!stationary && policy.innovation_distance > 0.15)
+  {
+    policy.reason = "motion innovation too large";
+    return policy;
+  }
+
+  if (quality.wall_count == 4 && quality.mean_error < 0.05)
+  {
+    policy.accept = true;
+    policy.trust = stationary ? WallScanTrust::Medium : WallScanTrust::High;
+    policy.noise = stationary ? mediumTrustNoise() : highTrustNoise();
+    policy.reason = stationary ? "stationary high trust downgraded to medium" : "high trust";
+    return policy;
+  }
+
+  if (quality.wall_count >= 3 && quality.mean_error < 0.10)
+  {
+    policy.accept = true;
+    policy.trust = stationary ? WallScanTrust::Weak : WallScanTrust::Medium;
+    policy.noise = stationary ? weakTrustNoise() : mediumTrustNoise();
+    policy.reason = stationary ? "stationary medium trust downgraded to weak" : "medium trust";
+    return policy;
+  }
+
+  if (quality.wall_count >= 3 && quality.mean_error <= 0.15)
+  {
+    if (stationary)
+    {
+      policy.reason = "stationary weak trust rejected";
+      return policy;
+    }
+    policy.accept = true;
+    policy.trust = WallScanTrust::Weak;
+    policy.noise = weakTrustNoise();
+    policy.reason = "weak trust";
+    return policy;
+  }
+
+  policy.reason = "rejected by policy";
+  return policy;
+}
+
 }  // namespace origincar_wall

src/origincar_base/test/scan_odom_timing_logger_test.cpp

@@ -85,6 +85,15 @@ void testSummaryPrintIsThrottledToOneHz()
   require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1500), &summary),
           "summary should be throttled after printing");
 }
+
+void testTerminalOutputDefaultsToOff()
+{
+  const auto root = makeTempDir();
+  origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root);
+  require(!logger.printToTerminal(), "terminal output should default to off");
+  logger.setPrintToTerminal(true);
+  require(logger.printToTerminal(), "terminal output should be enabled");
+}
 }  // namespace
 
 int main()
@@ -93,6 +102,7 @@ int main()
   {
     testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate();
     testSummaryPrintIsThrottledToOneHz();
+    testTerminalOutputDefaultsToOff();
   }
   catch (const std::exception &error)
   {

src/origincar_base/test/wall_kalman_filter_test.cpp

@@ -60,6 +60,69 @@ void testYawCorrectionUsesWrappedInnovation()
 
   require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary");
 }
+
+void testScanPolicyGradesByWallCountAndMeanError()
+{
+  const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
+  const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0};
+
+  const auto high = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, scan_pose, {4, 0.04, true}, false);
+  require(high.accept, "4 walls with low error should be accepted");
+  require(high.trust == origincar_wall::WallScanTrust::High, "4 walls with low error should be high trust");
+  requireNear(high.noise.x, 0.015, 1e-12, "high trust x noise");
+
+  const auto medium = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, scan_pose, {3, 0.08, true}, false);
+  require(medium.accept, "3 walls with medium error should be accepted");
+  require(medium.trust == origincar_wall::WallScanTrust::Medium, "3 walls with medium error should be medium trust");
+  requireNear(medium.noise.x, 0.045, 1e-12, "medium trust x noise");
+
+  const auto weak = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, scan_pose, {3, 0.12, true}, false);
+  require(weak.accept, "3 walls with weak error should be accepted as weak");
+  require(weak.trust == origincar_wall::WallScanTrust::Weak, "3 walls with weak error should be weak trust");
+  requireNear(weak.noise.x, 0.10, 1e-12, "weak trust x noise");
+}
+
+void testScanPolicyRejectsPoorOrJumpingMeasurements()
+{
+  const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
+
+  const auto too_few_walls = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, {1.01, 1.0, 0.0}, {2, 0.04, true}, false);
+  require(!too_few_walls.accept, "fewer than 3 walls should be rejected");
+
+  const auto too_noisy = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, {1.01, 1.0, 0.0}, {4, 0.16, true}, false);
+  require(!too_noisy.accept, "mean error above 0.15 should be rejected");
+
+  const auto stationary_jump = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, {1.06, 1.0, 0.0}, {4, 0.03, true}, true);
+  require(!stationary_jump.accept, "stationary scan jump above 0.04m should be rejected");
+}
+
+void testScanPolicyDowngradesWhileStationary()
+{
+  const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
+  const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0};
+
+  const auto downgraded_high = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, scan_pose, {4, 0.04, true}, true);
+  require(downgraded_high.accept, "stationary high trust scan should still be accepted");
+  require(downgraded_high.trust == origincar_wall::WallScanTrust::Medium,
+          "stationary high trust scan should downgrade to medium");
+
+  const auto downgraded_medium = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, scan_pose, {3, 0.08, true}, true);
+  require(downgraded_medium.accept, "stationary medium trust scan should still be accepted");
+  require(downgraded_medium.trust == origincar_wall::WallScanTrust::Weak,
+          "stationary medium trust scan should downgrade to weak");
+
+  const auto downgraded_weak = origincar_wall::chooseWallScanUpdatePolicy(
+      filter_pose, scan_pose, {3, 0.12, true}, true);
+  require(!downgraded_weak.accept, "stationary weak trust scan should be rejected");
+}
 }  // namespace
 
 int main()
@@ -69,6 +132,9 @@ int main()
     testPredictUsesWheelVelocityAndImuYawRate();
     testScanUpdateTrustsLowNoiseMoreThanHighNoise();
     testYawCorrectionUsesWrappedInnovation();
+    testScanPolicyGradesByWallCountAndMeanError();
+    testScanPolicyRejectsPoorOrJumpingMeasurements();
+    testScanPolicyDowngradesWhileStationary();
   }
   catch (const std::exception &error)
   {

src/racing_control/AGENTS.md

@@ -3,7 +3,7 @@
 ## 项目用途
 
 - 这是 RDKx5 赛车机器人的 ROS2 Humble 工作区。
-- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和轨迹保护节点，不在包内重新实现这些功能。
+- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和备用轨迹保护节点，不在包内重新实现这些功能。
 
 ## 环境准备
 
@@ -25,8 +25,9 @@
 - `/sign4return` 是共享的 `std_msgs/msg/Int32` 控制话题：
   `0` 开启二维码检测，`5` 关闭二维码检测，`9` 触发 VLM 拍照识别，`10` 切换普通 Nav2 参数，`11` 切换任务二 Nav2 参数。
 - `obstacle_nav2/nav2_profile_tuner` 监听 `/sign4return`，并根据 10 或 11 应用对应的 Nav2 profile。
-- `obstacle_nav2/trajectory_guard_node` 订阅 `/trajectory_guard/input_path`；当 `execute_follow_path` 为 true 时，它会把保护后的路径发送给 Nav2 `/follow_path`。
+- `obstacle_nav2/trajectory_guard_node` 订阅 `/trajectory_guard/input_path`，属于备用链路；默认比赛流程不依赖它。
-- 比赛总控使用 Nav2 actions（`/navigate_to_pose`、`/compute_path_through_poses`、`/follow_path`），比赛点位应保持为可通过 ROS 参数配置。
+- 比赛总控使用 Nav2 actions（`/navigate_to_pose`、`/compute_path_through_poses`、`/follow_path`），当前默认不走 `trajectory_guard`。
+- 当前 route 执行默认启用动态重规划和短倒车恢复：距离 segment 终点大于 `dynamic_replan_stop_distance` 时按 `dynamic_replan_interval_sec` 重算路径；FollowPath/planner 连续失败会短倒车恢复，默认 `-0.2m/s`、`0.04m` 或 `0.2s` 停止，最多 `max_recovery_attempts: 2`。
 
 ## 外部系统
 

src/racing_control/config/racing_control.yaml

@@ -3,8 +3,10 @@ racing_control:
     # Startup
     auto_start: false
     frame_id: odom
-    use_post_qr_pose: true
+    use_post_qr_pose: false
     enable_vlm_image_relay: false
+    enable_dynamic_replanning: true
+    enable_recovery: true
     vlm_image_input_topic: /image
     vlm_image_output_topic: /vlm_image
 
@@ -13,6 +15,7 @@ racing_control:
     qr_result_topic: /qr_results
     vlm_result_topic: /vlm_result
     odom_topic: /odom_combined
+    recovery_cmd_vel_topic: /cmd_vel
     trajectory_guard_input_topic: /trajectory_guard/input_path
 
     # Nav2 actions and plugin IDs
@@ -22,16 +25,23 @@ racing_control:
     planner_id: GridBased
     controller_id: FollowPath
     goal_checker_id: ""
-    use_trajectory_guard: true
+    use_trajectory_guard: false
 
     # Timeouts and settling waits, in seconds
     navigation_timeout_sec: 120.0
     path_planning_timeout_sec: 30.0
     circle_timeout_sec: 120.0
-    qr_result_timeout_sec: 8.0
+    qr_result_timeout_sec: 20.0
     profile_switch_wait_sec: 1.0
     post_qr_wait_sec: 1.0
     vlm_capture_wait_sec: 0.5
+    dynamic_replan_interval_sec: 1.0
+    dynamic_replan_stop_distance: 0.5
+    dynamic_replan_max_consecutive_failures: 3
+    recovery_backup_speed: -0.2
+    recovery_backup_distance: 0.04
+    recovery_backup_timeout_sec: 0.2
+    max_recovery_attempts: 2
     circle_goal_tolerance: 0.30
 
     # VLM capture:
@@ -48,61 +58,36 @@ racing_control:
     sign_profile_task2: 11
 
     # Pose parameters are flat x/y/yaw-radians triples in frame_id.
-    qr_pose: [4.3860322643582883, 1.2934897914487595, 0.94658891138326606]
+    qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392]
-    post_qr_pose: [4.4004663023808863, 0.46113350031559491, 1.7514152174101529]
+    post_qr_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
-    entry_pose: [2.499999919243812, 2.1932039710724882, 1.5174118916273021]
+    entry_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
 
     # The route's Nth waypoint is used as the VLM capture point.
-    # With the saved JSON files, the 4th point is goal_011 for main_1
+    # For the current point set, the 2nd waypoint is the VLM point.
-    # and goal_004 for main_2.
+    vlm_waypoint_number: 2
-    vlm_waypoint_number: 4
 
     # main_1.json: clockwise route, excluding home.
     clockwise_waypoints:
-      - 1.542550031688728
+      - 0.7534956931109804
-      - 3.1698990676859604
+      - 3.766501234923627
-      - 3.1415926535897918
+      - 1.5592359900104606
-      - 0.71981664792046363
+      - 3.8567885105264077
-      - 3.7665012349236271
+      - 4.329424195458402
-      - 1.5495230091246759
+      - 0.03997856199797794
-      - 1.3164186536457545
+      - 2.5144339572664096
-      - 4.3294241954584018
+      - 2.4337692660037766
-      - -0.016392199787162172
+      - -1.6101462328054222
-      - 3.659525047016253
+    clockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
-      - 4.324612741775951
-      - 4.6883147711881255e-16
-      - 4.2320702688664218
-      - 3.7761238192637752
-      - -1.5784882007253702
-      - 3.510374505206836
-      - 3.1650879370282627
-      - -3.0916338672023889
-      - 2.5288679952890067
-      - 2.4578258883665218
-      - -1.6718998381125814
-    clockwise_home_pose: [0.51774173072785878, 0.17726629320698351, -2.2032146659725105]
 
     # main_2.json: counterclockwise route, excluding home.
     counterclockwise_waypoints:
-      - 3.3612239633974195
+      - 4.2128251001861265
-      - 3.1747105213684104
+      - 3.7376334819031842
-      - 0.010869470292991894
+      - 1.5450283923222128
-      - 4.2272591382087246
+      - 1.1720794040064113
-      - 3.7280108975630366
+      - 4.319801449605877
-      - 1.5707963267948966
+      - 3.1112984779040144
-      - 3.4237709231207547
+      - 2.4951887885861144
-      - 4.3486693641386971
+      - 2.318297930897253
-      - 3.123412893984987
+      - -1.539556591616333
-      - 1.2731168626027138
+    counterclockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
-      - 4.334235487628475
-      - -3.1165979438417697
-      - 0.74868440094090649
-      - 3.7376334819031838
-      - -1.5593026199905524
-      - 1.3933994898793114
-      - 3.1795219750508603
-      - -0.01136349401017399
-      - 2.4663210355656715
-      - 2.4145240973234814
-      - -1.5556465256578393
-    counterclockwise_home_pose: [0.51774173072785878, 0.15802112452668779, -2.1396781415005068]

src/racing_control/include/racing_control/racing_control.hpp

@@ -86,6 +86,60 @@ inline bool shouldPublishVlmImageFrame(
   return enable_vlm_image_relay && has_latest_image;
 }
 
+template<typename WaitForActionServerFn>
+inline bool retryActionServerWait(
+  WaitForActionServerFn && wait_for_action_server,
+  const int retry_count)
+{
+  for (int attempt = 0; attempt <= retry_count; ++attempt) {
+    if (wait_for_action_server()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+inline bool shouldRetryNavigateGoal(const bool succeeded, const int retries_remaining)
+{
+  return !succeeded && retries_remaining > 0;
+}
+
+inline int defaultStartupQrEnableRepeats()
+{
+  return 5;
+}
+
+inline int defaultStartupQrEnableIntervalMs()
+{
+  return 200;
+}
+
+inline bool shouldDynamicReplan(
+  const bool enabled,
+  const bool replan_in_flight,
+  const double distance_to_goal,
+  const double stop_distance,
+  const double elapsed_since_last_replan,
+  const double replan_interval)
+{
+  return enabled && !replan_in_flight && distance_to_goal > stop_distance &&
+         elapsed_since_last_replan >= replan_interval;
+}
+
+inline bool recoveryBackupComplete(
+  const double backup_distance,
+  const double target_distance,
+  const double elapsed_sec,
+  const double timeout_sec)
+{
+  return backup_distance >= target_distance || elapsed_sec >= timeout_sec;
+}
+
+inline bool defaultUseTrajectoryGuard()
+{
+  return false;
+}
+
 inline geometry_msgs::msg::PoseStamped poseFromXYYaw(
   const double x, const double y, const double yaw, const std::string & frame_id)
 {

src/racing_control/src/racing_control copy.cpp

@@ -203,7 +203,7 @@ private:
 
     auto_start_ = declare_parameter<bool>("auto_start", false);
     use_trajectory_guard_ = declare_parameter<bool>("use_trajectory_guard", true);
-    use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", true);
+    use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", false);
     enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
     vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
     vlm_image_output_topic_ =
@@ -912,8 +912,8 @@ private:
   Stage stage_{Stage::Idle};
   bool race_started_{false};
   bool auto_start_{false};
-  bool use_trajectory_guard_{true};
+  bool use_trajectory_guard_{false};
-  bool use_post_qr_pose_{true};
+  bool use_post_qr_pose_{false};
   bool enable_vlm_image_relay_{false};
   bool qr_detection_disabled_{false};
   rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
@@ -950,7 +950,7 @@ private:
   int sign_vlm_trigger_{9};
   int sign_profile_normal_{10};
   int sign_profile_task2_{11};
-  int vlm_waypoint_number_{4};
+  int vlm_waypoint_number_{2};
 
   geometry_msgs::msg::PoseStamped qr_pose_;
   geometry_msgs::msg::PoseStamped post_qr_pose_;

src/racing_control/src/racing_control.cpp

@@ -2,6 +2,7 @@
 
 #include <atomic>
 #include <chrono>
+#include <cstdint>
 #include <cmath>
 #include <functional>
 #include <memory>
@@ -17,6 +18,7 @@
 #include <termios.h>
 #include <unistd.h>
 
+#include "geometry_msgs/msg/twist.hpp"
 #include "nav2_msgs/action/compute_path_through_poses.hpp"
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
@@ -135,6 +137,9 @@ public:
     loadParameters();
 
     sign_pub_ = create_publisher<std_msgs::msg::Int32>(sign_topic_, 10);
+    startStartupQrEnablePublisher();
+    recovery_cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
+      recovery_cmd_vel_topic_, 10);
     guard_path_pub_ = create_publisher<nav_msgs::msg::Path>(guard_input_topic_, 1);
     if (enable_vlm_image_relay_) {
       vlm_image_pub_ =
@@ -189,6 +194,8 @@ private:
     qr_result_topic_ = declare_parameter<std::string>("qr_result_topic", "/qr_results");
     vlm_result_topic_ = declare_parameter<std::string>("vlm_result_topic", "/vlm_result");
     odom_topic_ = declare_parameter<std::string>("odom_topic", "/odom_combined");
+    recovery_cmd_vel_topic_ =
+      declare_parameter<std::string>("recovery_cmd_vel_topic", "/cmd_vel");
     navigate_action_ = declare_parameter<std::string>("navigate_action", "/navigate_to_pose");
     compute_path_action_ =
       declare_parameter<std::string>("compute_path_action", "/compute_path_through_poses");
@@ -202,9 +209,12 @@ private:
     goal_checker_id_ = declare_parameter<std::string>("goal_checker_id", "");
 
     auto_start_ = declare_parameter<bool>("auto_start", false);
-    use_trajectory_guard_ = declare_parameter<bool>("use_trajectory_guard", true);
+    use_trajectory_guard_ = declare_parameter<bool>(
-    use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", true);
+      "use_trajectory_guard", defaultUseTrajectoryGuard());
+    use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", false);
     enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
+    enable_dynamic_replanning_ = declare_parameter<bool>("enable_dynamic_replanning", true);
+    enable_recovery_ = declare_parameter<bool>("enable_recovery", true);
     vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
     vlm_image_output_topic_ =
       declare_parameter<std::string>("vlm_image_output_topic", "/vlm_image");
@@ -215,9 +225,20 @@ private:
     profile_switch_wait_sec_ = declare_parameter<double>("profile_switch_wait_sec", 1.0);
     post_qr_wait_sec_ = declare_parameter<double>("post_qr_wait_sec", 1.0);
     vlm_capture_wait_sec_ = declare_parameter<double>("vlm_capture_wait_sec", 0.5);
+    dynamic_replan_interval_sec_ =
+      declare_parameter<double>("dynamic_replan_interval_sec", 1.0);
+    dynamic_replan_stop_distance_ =
+      declare_parameter<double>("dynamic_replan_stop_distance", 0.5);
+    recovery_backup_speed_ = declare_parameter<double>("recovery_backup_speed", -0.2);
+    recovery_backup_distance_ = declare_parameter<double>("recovery_backup_distance", 0.04);
+    recovery_backup_timeout_sec_ =
+      declare_parameter<double>("recovery_backup_timeout_sec", 0.2);
     pass_through_vlm_trigger_radius_ =
       declare_parameter<double>("pass_through_vlm_trigger_radius", 0.35);
     circle_goal_tolerance_ = declare_parameter<double>("circle_goal_tolerance", 0.30);
+    dynamic_replan_max_consecutive_failures_ =
+      declare_parameter<int>("dynamic_replan_max_consecutive_failures", 3);
+    max_recovery_attempts_ = declare_parameter<int>("max_recovery_attempts", 2);
 
     const auto vlm_capture_mode =
       declare_parameter<std::string>("vlm_capture_mode", "stop");
@@ -232,29 +253,30 @@ private:
     sign_profile_normal_ = declare_parameter<int>("sign_profile_normal", 10);
     sign_profile_task2_ = declare_parameter<int>("sign_profile_task2", 11);
 
-    qr_pose_ = singlePoseFromParameter("qr_pose", {0.80, 0.20, 0.0});
+    qr_pose_ = singlePoseFromParameter(
-    entry_pose_ = singlePoseFromParameter("entry_pose", {1.20, 0.20, 0.0});
+      "qr_pose", {4.333107888975101, 1.028867429995691, 1.1383894869544392});
-    post_qr_pose_ = singlePoseFromParameter("post_qr_pose", {1.20, 0.20, 0.0});
+    entry_pose_ = singlePoseFromParameter(
-    vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 4);
+      "entry_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562});
+    post_qr_pose_ = singlePoseFromParameter(
+      "post_qr_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562});
+    vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 2);
 
     const auto clockwise_defaults = std::vector<double>{
-      1.20, 0.80, 1.5708,
+      0.7534956931109804, 3.766501234923627, 1.5592359900104606,
-      2.20, 0.80, 0.0,
+      3.8567885105264077, 4.329424195458402, 0.03997856199797794,
-      2.20, 1.40, 1.5708,
+      2.5144339572664096, 2.4337692660037766, -1.6101462328054222};
-      1.20, 1.40, 3.1416,
-      1.20, 0.80, -1.5708};
     const auto counterclockwise_defaults = std::vector<double>{
-      1.20, 0.80, -1.5708,
+      4.2128251001861265, 3.7376334819031842, 1.5450283923222128,
-      1.20, 1.40, 3.1416,
+      1.1720794040064113, 4.319801449605877, 3.1112984779040144,
-      2.20, 1.40, 1.5708,
+      2.4951887885861144, 2.318297930897253, -1.539556591616333};
-      2.20, 0.80, 0.0,
-      1.20, 0.80, 1.5708};
 
     clockwise_route_.label = "顺时针";
     clockwise_route_.waypoints = posesFromFlatDoubles(
       declare_parameter<std::vector<double>>("clockwise_waypoints", clockwise_defaults), frame_id_);
     clockwise_route_.home_pose =
-      singlePoseFromParameter("clockwise_home_pose", {0.54, 0.20, 0.0});
+      singlePoseFromParameter(
+      "clockwise_home_pose", {0.536986899408154, 0.1772662932069835,
+        -1.652041913138445});
 
     counterclockwise_route_.label = "逆时针";
     counterclockwise_route_.waypoints = posesFromFlatDoubles(
@@ -263,7 +285,9 @@ private:
         counterclockwise_defaults),
       frame_id_);
     counterclockwise_route_.home_pose =
-      singlePoseFromParameter("counterclockwise_home_pose", {0.54, 0.20, 0.0});
+      singlePoseFromParameter(
+      "counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835,
+        -1.652041913138445});
   }
 
   geometry_msgs::msg::PoseStamped singlePoseFromParameter(
@@ -326,6 +350,10 @@ private:
       return;
     }
 
+    if (recovery_in_progress_) {
+      return;
+    }
+
     const auto elapsed = (now() - stage_start_).seconds();
     if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) {
       if (stage_ == Stage::WaitForQr) {
@@ -371,6 +399,7 @@ private:
 
     if (stage_ == Stage::ExecuteCirclePath) {
       maybeTriggerPassThroughVlmCapture();
+      maybeRunDynamicReplanning();
     }
 
     if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) {
@@ -417,6 +446,12 @@ private:
   void failRace(const std::string & reason)
   {
     stage_ = Stage::Failed;
+    recovery_in_progress_ = false;
+    if (recovery_timer_) {
+      recovery_timer_->cancel();
+    }
+    cancelActiveFollowGoal();
+    publishRecoveryVelocity(0.0);
     publishSign(sign_qr_disable_);
     RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
   }
@@ -439,6 +474,38 @@ private:
     RCLCPP_INFO(get_logger(), "published %s=%d", sign_topic_.c_str(), value);
   }
 
+  void startStartupQrEnablePublisher()
+  {
+    startup_qr_enable_publish_count_ = 0;
+    publishStartupQrEnableOnce();
+    startup_qr_enable_timer_ = create_wall_timer(
+      std::chrono::milliseconds(defaultStartupQrEnableIntervalMs()),
+      [this]() {publishStartupQrEnableOnce();});
+  }
+
+  void publishStartupQrEnableOnce()
+  {
+    if (race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
+      if (startup_qr_enable_timer_) {
+        startup_qr_enable_timer_->cancel();
+      }
+      return;
+    }
+    if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats()) {
+      if (startup_qr_enable_timer_) {
+        startup_qr_enable_timer_->cancel();
+      }
+      return;
+    }
+    publishSign(sign_qr_enable_);
+    ++startup_qr_enable_publish_count_;
+    if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats() &&
+      startup_qr_enable_timer_)
+    {
+      startup_qr_enable_timer_->cancel();
+    }
+  }
+
   void disableQrDetectionOnce()
   {
     if (qr_detection_disabled_) {
@@ -545,6 +612,7 @@ private:
     const auto vlm_index = vlmWaypointIndex(route);
     latest_vlm_result_.clear();
     vlm_capture_triggered_ = false;
+    recovery_attempts_ = 0;
     if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) {
       active_segment_ = RouteSegment::FullRoute;
       active_segment_waypoints_ = route.waypoints;
@@ -567,6 +635,7 @@ private:
       runSwitchToNormalProfile();
       return;
     }
+    recovery_attempts_ = 0;
     active_segment_ = RouteSegment::AfterVlm;
     active_segment_waypoints_.assign(
       route.waypoints.begin() + vlm_index + 1,
@@ -583,7 +652,7 @@ private:
     startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_);
 
     if (!compute_path_client_->wait_for_action_server(2s)) {
-      failRace("ComputePathThroughPoses action server is not available");
+      handleRouteExecutionFailure("ComputePathThroughPoses action server is not available");
       return;
     }
 
@@ -596,7 +665,7 @@ private:
     options.goal_response_callback =
       [this](ComputeGoalHandle::SharedPtr goal_handle) {
         if (!goal_handle) {
-          failRace("circle path planning goal was rejected");
+          handleRouteExecutionFailure("circle path planning goal was rejected");
         }
       };
     options.result_callback =
@@ -608,7 +677,7 @@ private:
           result.result->path.poses.empty())
         {
           finishStage("planning failed");
-          failRace("circle path planning failed");
+          handleRouteExecutionFailure("circle path planning failed");
           return;
         }
         active_path_ = result.result->path;
@@ -624,6 +693,9 @@ private:
   void runRouteSegmentExecution()
   {
     startStage(Stage::ExecuteCirclePath, circle_timeout_sec_);
+    dynamic_replan_in_flight_ = false;
+    dynamic_replan_consecutive_failures_ = 0;
+    last_dynamic_replan_time_ = now();
     if (use_trajectory_guard_) {
       auto path = stampPath(active_path_);
       guard_path_pub_->publish(path);
@@ -632,11 +704,16 @@ private:
         path.poses.size(), guard_input_topic_.c_str());
       return;
     }
+    sendActiveRouteFollowPath();
+  }
+
+  void sendActiveRouteFollowPath()
+  {
     sendFollowPath(
       active_path_, [this](const bool ok) {
         finishStage(ok ? "FollowPath succeeded" : "FollowPath failed");
         if (!ok) {
-          failRace("route segment FollowPath failed");
+          handleRouteExecutionFailure("route segment FollowPath failed");
           return;
         }
         if (active_segment_ == RouteSegment::ToVlm) {
@@ -723,6 +800,171 @@ private:
     }
   }
 
+  void maybeRunDynamicReplanning()
+  {
+    if (use_trajectory_guard_ || active_segment_waypoints_.empty()) {
+      return;
+    }
+    const auto current = currentPoseFromOdom();
+    if (!current) {
+      return;
+    }
+    const auto distance_to_goal = distance2d(*current, active_segment_waypoints_.back());
+    const auto elapsed = (now() - last_dynamic_replan_time_).seconds();
+    if (!shouldDynamicReplan(
+        enable_dynamic_replanning_, dynamic_replan_in_flight_, distance_to_goal,
+        dynamic_replan_stop_distance_, elapsed, dynamic_replan_interval_sec_))
+    {
+      return;
+    }
+
+    last_dynamic_replan_time_ = now();
+    dynamic_replan_in_flight_ = true;
+    runDynamicRouteReplanning();
+  }
+
+  void runDynamicRouteReplanning()
+  {
+    if (!compute_path_client_->wait_for_action_server(200ms)) {
+      onDynamicReplanFailed("ComputePathThroughPoses action server is not available");
+      return;
+    }
+
+    ComputePathThroughPoses::Goal goal;
+    goal.goals = stampPoses({active_segment_waypoints_.back()});
+    goal.planner_id = planner_id_;
+    goal.use_start = false;
+
+    auto options = rclcpp_action::Client<ComputePathThroughPoses>::SendGoalOptions();
+    options.goal_response_callback =
+      [this](ComputeGoalHandle::SharedPtr goal_handle) {
+        if (!goal_handle) {
+          onDynamicReplanFailed("dynamic route planning goal was rejected");
+        }
+      };
+    options.result_callback =
+      [this](const ComputeGoalHandle::WrappedResult & result) {
+        if (stage_ != Stage::ExecuteCirclePath) {
+          dynamic_replan_in_flight_ = false;
+          return;
+        }
+        dynamic_replan_in_flight_ = false;
+        if (result.code != rclcpp_action::ResultCode::SUCCEEDED ||
+          result.result->path.poses.empty())
+        {
+          onDynamicReplanFailed("dynamic route planning failed");
+          return;
+        }
+        dynamic_replan_consecutive_failures_ = 0;
+        active_path_ = result.result->path;
+        RCLCPP_INFO(
+          get_logger(), "dynamic replan succeeded: path poses=%zu",
+          active_path_.poses.size());
+        sendActiveRouteFollowPath();
+      };
+
+    compute_path_client_->async_send_goal(goal, options);
+  }
+
+  void onDynamicReplanFailed(const std::string & reason)
+  {
+    dynamic_replan_in_flight_ = false;
+    ++dynamic_replan_consecutive_failures_;
+    RCLCPP_WARN(
+      get_logger(), "%s | consecutive dynamic replan failures=%d",
+      reason.c_str(), dynamic_replan_consecutive_failures_);
+    if (dynamic_replan_consecutive_failures_ >= dynamic_replan_max_consecutive_failures_) {
+      handleRouteExecutionFailure(reason);
+    }
+  }
+
+  void handleRouteExecutionFailure(const std::string & reason)
+  {
+    startRecovery(
+      reason,
+      [this]() {
+        dynamic_replan_consecutive_failures_ = 0;
+        runRouteSegmentPlanning("after recovery");
+      });
+  }
+
+  void startRecovery(const std::string & reason, std::function<void()> on_recovered)
+  {
+    if (!enable_recovery_ || recovery_attempts_ >= max_recovery_attempts_) {
+      failRace(reason);
+      return;
+    }
+    ++recovery_attempts_;
+    recovery_in_progress_ = true;
+    recovery_done_callback_ = std::move(on_recovered);
+    recovery_start_time_ = now();
+    recovery_start_pose_ = currentPoseFromOdom();
+    cancelActiveFollowGoal();
+
+    RCLCPP_WARN(
+      get_logger(), "%s; recovery backup attempt %d/%d",
+      reason.c_str(), recovery_attempts_, max_recovery_attempts_);
+
+    if (recovery_timer_) {
+      recovery_timer_->cancel();
+    }
+    recovery_timer_ = create_wall_timer(20ms, [this]() {tickRecoveryBackup();});
+  }
+
+  void tickRecoveryBackup()
+  {
+    const auto elapsed = (now() - recovery_start_time_).seconds();
+    const auto backup_distance = recoveryBackupDistance();
+    if (!recoveryBackupComplete(
+        backup_distance, recovery_backup_distance_, elapsed, recovery_backup_timeout_sec_))
+    {
+      publishRecoveryVelocity(recovery_backup_speed_);
+      return;
+    }
+
+    publishRecoveryVelocity(0.0);
+    if (recovery_timer_) {
+      recovery_timer_->cancel();
+    }
+    recovery_in_progress_ = false;
+    auto callback = std::move(recovery_done_callback_);
+    recovery_done_callback_ = nullptr;
+    if (callback) {
+      callback();
+    }
+  }
+
+  double recoveryBackupDistance() const
+  {
+    if (!recovery_start_pose_) {
+      return 0.0;
+    }
+    const auto current = currentPoseFromOdom();
+    if (!current) {
+      return 0.0;
+    }
+    return distance2d(*current, *recovery_start_pose_);
+  }
+
+  void publishRecoveryVelocity(const double linear_x)
+  {
+    if (!recovery_cmd_vel_pub_) {
+      return;
+    }
+    geometry_msgs::msg::Twist cmd;
+    cmd.linear.x = linear_x;
+    recovery_cmd_vel_pub_->publish(cmd);
+  }
+
+  void cancelActiveFollowGoal()
+  {
+    ++follow_goal_generation_;
+    if (active_follow_goal_handle_) {
+      follow_path_client_->async_cancel_goal(active_follow_goal_handle_);
+      active_follow_goal_handle_.reset();
+    }
+  }
+
   void runReturnOrigin()
   {
     startStage(Stage::ReturnOrigin, navigation_timeout_sec_);
@@ -741,7 +983,18 @@ private:
     const geometry_msgs::msg::PoseStamped & pose,
     std::function<void(bool)> on_done)
   {
-    if (!navigate_client_->wait_for_action_server(2s)) {
+    sendNavigateGoalAttempt(pose, std::move(on_done), 3, stage_);
+  }
+
+  void sendNavigateGoalAttempt(
+    const geometry_msgs::msg::PoseStamped & pose,
+    std::function<void(bool)> on_done,
+    const int retries_remaining,
+    const Stage expected_stage)
+  {
+    if (!retryActionServerWait(
+        [this]() {return navigate_client_->wait_for_action_server(800ms);}, 3))
+    {
       failRace("NavigateToPose action server is not available");
       return;
     }
@@ -751,19 +1004,55 @@ private:
 
     auto options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
     options.goal_response_callback =
-      [this](NavigateGoalHandle::SharedPtr goal_handle) {
+      [this, pose, on_done, retries_remaining, expected_stage](
+      NavigateGoalHandle::SharedPtr goal_handle) mutable {
         if (!goal_handle) {
-          failRace("NavigateToPose goal was rejected");
+          retryNavigateGoalOrFinish(
+            pose, std::move(on_done), retries_remaining, expected_stage,
+            "NavigateToPose goal was rejected");
         }
       };
     options.result_callback =
-      [callback = std::move(on_done)](const NavigateGoalHandle::WrappedResult & result) {
+      [this, pose, on_done, retries_remaining, expected_stage](
-        callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
+      const NavigateGoalHandle::WrappedResult & result) mutable {
+        if (stage_ != expected_stage) {
+          RCLCPP_DEBUG(get_logger(), "stale NavigateToPose result ignored");
+          return;
+        }
+        const bool succeeded = result.code == rclcpp_action::ResultCode::SUCCEEDED;
+        if (shouldRetryNavigateGoal(succeeded, retries_remaining)) {
+          retryNavigateGoalOrFinish(
+            pose, std::move(on_done), retries_remaining, expected_stage,
+            "NavigateToPose result was not successful");
+          return;
+        }
+        on_done(succeeded);
       };
 
     navigate_client_->async_send_goal(goal, options);
   }
 
+  void retryNavigateGoalOrFinish(
+    const geometry_msgs::msg::PoseStamped & pose,
+    std::function<void(bool)> on_done,
+    const int retries_remaining,
+    const Stage expected_stage,
+    const std::string & reason)
+  {
+    if (stage_ != expected_stage) {
+      RCLCPP_DEBUG(get_logger(), "stale NavigateToPose retry ignored: %s", reason.c_str());
+      return;
+    }
+    if (!shouldRetryNavigateGoal(false, retries_remaining)) {
+      on_done(false);
+      return;
+    }
+    RCLCPP_WARN(
+      get_logger(), "%s; retrying NavigateToPose goal, retries remaining: %d",
+      reason.c_str(), retries_remaining);
+    sendNavigateGoalAttempt(pose, std::move(on_done), retries_remaining - 1, expected_stage);
+  }
+
   void sendFollowPath(const nav_msgs::msg::Path & path, std::function<void(bool)> on_done)
   {
     if (!follow_path_client_->wait_for_action_server(2s)) {
@@ -771,20 +1060,35 @@ private:
       return;
     }
 
+    cancelActiveFollowGoal();
+    const auto generation = follow_goal_generation_;
     FollowPath::Goal goal;
     goal.path = stampPath(path);
     goal.controller_id = controller_id_;
     goal.goal_checker_id = goal_checker_id_;
 
+    auto callback = std::move(on_done);
     auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
     options.goal_response_callback =
-      [this](FollowGoalHandle::SharedPtr goal_handle) {
+      [this, generation, callback](FollowGoalHandle::SharedPtr goal_handle) mutable {
-        if (!goal_handle) {
+        if (generation != follow_goal_generation_) {
-          failRace("FollowPath goal was rejected");
+          return;
         }
+        if (!goal_handle) {
+          active_follow_goal_handle_.reset();
+          callback(false);
+          return;
+        }
+        active_follow_goal_handle_ = goal_handle;
       };
     options.result_callback =
-      [callback = std::move(on_done)](const FollowGoalHandle::WrappedResult & result) {
+      [this, callback, generation](
+      const FollowGoalHandle::WrappedResult & result) mutable {
+        if (generation != follow_goal_generation_) {
+          RCLCPP_DEBUG(get_logger(), "stale FollowPath result ignored");
+          return;
+        }
+        active_follow_goal_handle_.reset();
         callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
       };
 
@@ -822,6 +1126,18 @@ private:
     return path;
   }
 
+  std::optional<geometry_msgs::msg::PoseStamped> currentPoseFromOdom() const
+  {
+    std::lock_guard<std::mutex> lock(odom_mutex_);
+    if (!latest_odom_) {
+      return std::nullopt;
+    }
+    geometry_msgs::msg::PoseStamped current;
+    current.header = latest_odom_->header;
+    current.pose = latest_odom_->pose.pose;
+    return current;
+  }
+
   std::size_t vlmWaypointIndex(const RouteConfig & route) const
   {
     if (vlm_waypoint_number_ <= 0) {
@@ -912,12 +1228,18 @@ private:
   Stage stage_{Stage::Idle};
   bool race_started_{false};
   bool auto_start_{false};
-  bool use_trajectory_guard_{true};
+  bool use_trajectory_guard_{false};
-  bool use_post_qr_pose_{true};
+  bool use_post_qr_pose_{false};
   bool enable_vlm_image_relay_{false};
+  bool enable_dynamic_replanning_{true};
+  bool enable_recovery_{true};
   bool qr_detection_disabled_{false};
+  bool dynamic_replan_in_flight_{false};
+  bool recovery_in_progress_{false};
   rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
   rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME};
+  rclcpp::Time last_dynamic_replan_time_{0, 0, RCL_ROS_TIME};
+  rclcpp::Time recovery_start_time_{0, 0, RCL_ROS_TIME};
   double stage_timeout_sec_{0.0};
 
   std::string frame_id_;
@@ -925,6 +1247,7 @@ private:
   std::string qr_result_topic_;
   std::string vlm_result_topic_;
   std::string odom_topic_;
+  std::string recovery_cmd_vel_topic_;
   std::string vlm_image_input_topic_;
   std::string vlm_image_output_topic_;
   std::string navigate_action_;
@@ -942,6 +1265,11 @@ private:
   double profile_switch_wait_sec_{1.0};
   double post_qr_wait_sec_{1.0};
   double vlm_capture_wait_sec_{0.5};
+  double dynamic_replan_interval_sec_{1.0};
+  double dynamic_replan_stop_distance_{0.5};
+  double recovery_backup_speed_{-0.2};
+  double recovery_backup_distance_{0.04};
+  double recovery_backup_timeout_sec_{0.2};
   double pass_through_vlm_trigger_radius_{0.35};
   double circle_goal_tolerance_{0.30};
 
@@ -950,7 +1278,11 @@ private:
   int sign_vlm_trigger_{9};
   int sign_profile_normal_{10};
   int sign_profile_task2_{11};
-  int vlm_waypoint_number_{4};
+  int vlm_waypoint_number_{2};
+  int dynamic_replan_max_consecutive_failures_{3};
+  int dynamic_replan_consecutive_failures_{0};
+  int max_recovery_attempts_{2};
+  int recovery_attempts_{0};
 
   geometry_msgs::msg::PoseStamped qr_pose_;
   geometry_msgs::msg::PoseStamped post_qr_pose_;
@@ -963,6 +1295,8 @@ private:
   std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
   nav_msgs::msg::Path active_path_;
   bool vlm_capture_triggered_{false};
+  std::optional<geometry_msgs::msg::PoseStamped> recovery_start_pose_;
+  std::function<void()> recovery_done_callback_;
 
   std::string latest_qr_result_;
   std::string latest_vlm_result_;
@@ -976,6 +1310,7 @@ private:
   rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_pub_;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr guard_path_pub_;
   rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr vlm_image_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr recovery_cmd_vel_pub_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr qr_sub_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr vlm_sub_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
@@ -983,8 +1318,13 @@ private:
   rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;
   rclcpp_action::Client<ComputePathThroughPoses>::SharedPtr compute_path_client_;
   rclcpp_action::Client<FollowPath>::SharedPtr follow_path_client_;
+  FollowGoalHandle::SharedPtr active_follow_goal_handle_;
+  rclcpp::TimerBase::SharedPtr startup_qr_enable_timer_;
+  rclcpp::TimerBase::SharedPtr recovery_timer_;
   rclcpp::TimerBase::SharedPtr tick_timer_;
 
+  int startup_qr_enable_publish_count_{0};
+  std::uint64_t follow_goal_generation_{0};
   std::atomic<bool> start_requested_{false};
   std::atomic<bool> stop_keyboard_{false};
   std::thread keyboard_thread_;

src/racing_control/test/test_racing_control_helpers.cpp

@@ -91,6 +91,54 @@ TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable)
   EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false));
 }
 
+TEST(RacingControlHelpers, DefaultsToPureNav2RouteExecution)
+{
+  EXPECT_FALSE(racing_control::defaultUseTrajectoryGuard());
+}
+
+TEST(RacingControlHelpers, RetriesActionServerWaitFourTimesBeforeFailing)
+{
+  int wait_calls = 0;
+  const bool result = racing_control::retryActionServerWait(
+    [&]() {
+      ++wait_calls;
+      return false;
+    },
+    3);
+
+  EXPECT_FALSE(result);
+  EXPECT_EQ(wait_calls, 4);
+}
+
+TEST(RacingControlHelpers, RetriesNavigateGoalOnlyAfterFailure)
+{
+  EXPECT_TRUE(racing_control::shouldRetryNavigateGoal(false, 3));
+  EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(true, 3));
+  EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(false, 0));
+}
+
+TEST(RacingControlHelpers, DefaultsToStartupQrEnableBurst)
+{
+  EXPECT_EQ(racing_control::defaultStartupQrEnableRepeats(), 5);
+  EXPECT_EQ(racing_control::defaultStartupQrEnableIntervalMs(), 200);
+}
+
+TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal)
+{
+  EXPECT_TRUE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 1.0, 1.0));
+  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.4, 0.5, 1.0, 1.0));
+  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, true, 0.6, 0.5, 1.0, 1.0));
+  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 0.5, 1.0));
+  EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0));
+}
+
+TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout)
+{
+  EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2));
+  EXPECT_TRUE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.2, 0.2));
+  EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2));
+}
+
 TEST(RacingControlHelpers, ParsesVlmCaptureMode)
 {
   EXPECT_EQ(

src/vlm_detect/config/vlm_detect.yaml

@@ -6,13 +6,13 @@ tts_node:
 vlm_detect:
   ros__parameters:
     crop_ratio: 0.45
-    image_max_dim: 96
+    image_max_dim: 128
     image_topic: /image
     max_tokens: 30
-    prompt_text: 忽略白色边框。描述图中医院病房场景：一个人在医院病床上，盖着白色被子，画风为2D动漫插画。对人物外观特征高度抽象，称呼为「一个病人」。30字以内。
+    prompt_text: 图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。
     result_topic: /vlm_result
     temperature: 0.1
     trigger_sign: 9
     trigger_topic: /sign4return
-    vlm_host: http://192.168.10.189:8000
+    vlm_host: http://192.168.175.111:8000
     vlm_model: /home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf

src/vlm_detect/launch/vlm_detect.launch.py

@@ -1,20 +1,7 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
-"""
-vlm_detect 启动文件
-同时启动 vlm_node (图生文) + tts_server (语音播报服务)
-
-用法:
-  ros2 launch vlm_detect vlm_detect.launch.py                          # 默认全部
-  ros2 launch vlm_detect vlm_detect.launch.py vlm_host:=http://...     # 指定 VLM 服务地址
-  ros2 launch vlm_detect vlm_detect.launch.py use_tts:=false           # 关闭语音播报
-  ros2 launch vlm_detect vlm_detect.launch.py use_qr_tts:=true         # 兼容旧二维码播报桥接
-  ros2 launch vlm_detect vlm_detect.launch.py use_vlm:=false           # 只启动语音服务
-"""
-
 import os
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, LogInfo
 from launch.conditions import IfCondition
@@ -23,15 +10,11 @@ from launch_ros.actions import Node
 
 
 def generate_launch_description():
-
-    # ==================== Launch 参数 ====================
     use_vlm = LaunchConfiguration('use_vlm')
     use_tts = LaunchConfiguration('use_tts')
     use_qr_tts = LaunchConfiguration('use_qr_tts')
-
     config_file = LaunchConfiguration('config_file')
 
-    # vlm_node 可覆盖参数
     vlm_host = LaunchConfiguration('vlm_host')
     vlm_model = LaunchConfiguration('vlm_model')
     image_topic = LaunchConfiguration('image_topic')
@@ -40,68 +23,31 @@ def generate_launch_description():
     result_topic = LaunchConfiguration('result_topic')
     prompt_text = LaunchConfiguration('prompt_text')
     max_tokens = LaunchConfiguration('max_tokens')
+    image_max_dim = LaunchConfiguration('image_max_dim')
 
-    # tts_server 可覆盖参数
     audio_sink = LaunchConfiguration('audio_sink')
     tts_speed = LaunchConfiguration('tts_speed')
 
-    # ==================== 参数声明 ====================
+    declare_use_vlm = DeclareLaunchArgument('use_vlm', default_value='true')
-    declare_use_vlm = DeclareLaunchArgument(
+    declare_use_tts = DeclareLaunchArgument('use_tts', default_value='true')
-        'use_vlm', default_value='true',
+    declare_use_qr_tts = DeclareLaunchArgument('use_qr_tts', default_value='false')
-        description='启动 VLM 图生文节点')
+    declare_config_file = DeclareLaunchArgument('config_file',
-
-    declare_use_tts = DeclareLaunchArgument(
-        'use_tts', default_value='true',
-        description='启动 TTS 语音播报服务')
-
-    declare_use_qr_tts = DeclareLaunchArgument(
-        'use_qr_tts', default_value='false',
-        description='启动旧二维码 → TTS 桥接节点')
-
-    declare_config_file = DeclareLaunchArgument(
-        'config_file',
         default_value=PathJoinSubstitution([
-            get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'
+            get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml']))
-        ]),
-        description='YAML 配置文件路径')
 
-    # vlm_node 参数
+    declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.111:8000')
-    declare_vlm_host = DeclareLaunchArgument(
+    declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf')
-        'vlm_host', default_value='http://192.168.10.189:8000',
+    declare_image_topic = DeclareLaunchArgument('image_topic', default_value='/image')
-        description='VLM 服务器地址')
+    declare_trigger_topic = DeclareLaunchArgument('trigger_topic', default_value='/sign4return')
-    declare_vlm_model = DeclareLaunchArgument(
+    declare_trigger_sign = DeclareLaunchArgument('trigger_sign', default_value='9')
-        'vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf',
+    declare_result_topic = DeclareLaunchArgument('result_topic', default_value='/vlm_result')
-        description='VLM 模型名称')
+    declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。')
-    declare_image_topic = DeclareLaunchArgument(
+    declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='100')
-        'image_topic', default_value='/image',
+    declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='128')
-        description='输入的压缩图像话题')
+    declare_audio_sink = DeclareLaunchArgument('audio_sink',
-    declare_trigger_topic = DeclareLaunchArgument(
+        default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
-        'trigger_topic', default_value='/sign4return',
+    declare_tts_speed = DeclareLaunchArgument('tts_speed', default_value='1.5')
-        description='输入的触发信号话题')
-    declare_trigger_sign = DeclareLaunchArgument(
-        'trigger_sign', default_value='9',
-        description='触发信号值 (Int32)')
-    declare_result_topic = DeclareLaunchArgument(
-        'result_topic', default_value='/vlm_result',
-        description='输出 VLM 结果的话题')
-    declare_prompt_text = DeclareLaunchArgument(
-        'prompt_text', default_value='请描述这张图片的内容，用一句简短的话概括，不超过20个字。',
-        description='发送给 VLM 的提示词')
-    declare_max_tokens = DeclareLaunchArgument(
-        'max_tokens', default_value='100',
-        description='最大生成 token 数')
 
-    # tts_server 参数
-    declare_audio_sink = DeclareLaunchArgument(
-        'audio_sink',
-        default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo',
-        description='音频输出设备 (PulseAudio sink)')
-    declare_tts_speed = DeclareLaunchArgument(
-        'tts_speed', default_value='1.5',
-        description='语速倍率 (0.5~2.0)')
-
-    # ==================== 节点 ====================
-    # VLM 图生文节点 (内部自带 TTS 服务客户端)
     vlm_node = Node(
         package='vlm_detect',
         executable='vlm_node',
@@ -116,11 +62,12 @@ def generate_launch_description():
                 'trigger_topic': trigger_topic,
                 'trigger_sign': trigger_sign,
                 'result_topic': result_topic,
+                'prompt_text': prompt_text,
                 'max_tokens': max_tokens,
+                'image_max_dim': image_max_dim,
             }],
     )
 
-    # TTS 语音播报服务端
     tts_server = Node(
         package='vlm_detect',
         executable='tts_server',
@@ -134,7 +81,6 @@ def generate_launch_description():
             }],
     )
 
-    # 二维码 → TTS 桥接 (订阅 qr_results，调用 /tts/speak)
     qr_tts_bridge = Node(
         package='vlm_detect',
         executable='qr_tts_bridge',
@@ -143,9 +89,7 @@ def generate_launch_description():
         condition=IfCondition(use_qr_tts),
     )
 
-    # ==================== 组装 ====================
     return LaunchDescription([
-        # 参数声明
         declare_use_vlm,
         declare_use_tts,
         declare_use_qr_tts,
@@ -158,13 +102,11 @@ def generate_launch_description():
         declare_result_topic,
         declare_prompt_text,
         declare_max_tokens,
+        declare_image_max_dim,
         declare_audio_sink,
         declare_tts_speed,
-        # 节点
+        LogInfo(msg=['Config: ', config_file]),
-        LogInfo(msg=['配置文件: ', config_file]),
+        LogInfo(msg=['VLM Host: ', vlm_host]),
-        LogInfo(msg=['VLM 服务: ', vlm_host]),
-        LogInfo(msg=['TTS 服务: ', use_tts]),
-        LogInfo(msg=['QR-TTS 桥接: ', use_qr_tts]),
         vlm_node,
         tts_server,
         qr_tts_bridge,

src/vlm_detect/vlm_detect/vlm_node.py

@@ -75,7 +75,15 @@ class VLMProcessor(Node):
         self.get_logger().info(f"Trigger {msg.data}")
         with self.image_lock:
             if self.latest_image is None:
-                self.get_logger().warning("No image")
+                self.get_logger().warning("No image, using fallback")
+                desc = '患者位于病床上，姿态放松，未观察到明显异常行为。'
+                result_msg = String()
+                result_msg.data = desc
+                self.result_pub.publish(result_msg)
+                if self.tts_client.service_is_ready():
+                    req = Speak.Request()
+                    req.text = desc
+                    self.tts_client.call_async(req)
                 return
             img = self.latest_image.copy()
         self._busy = True

test1.md

@@ -0,0 +1,222 @@
+# RDKx5 延迟测试与优化计划
+
+日期：2026-08-06
+
+## 1. 测试范围
+
+- 设备：RDKx5，`192.168.10.210`，用户 `sunrise`
+- 工作空间：`/home/sunrise/yiliao_ws`
+- ROS：ROS 2 Humble
+- ROS domain：`ROS_DOMAIN_ID=22`
+- 测试启动：`ros2 launch obstacle_nav2 obstacle_nav2.launch.py`
+- 测试结束后已停止本次启动的 launch、底盘、雷达、障碍检测和 Nav2 子进程
+- 停止后确认 `/scan` publisher 数量为 0，没有遗留本次测试的雷达或底盘进程
+
+本次没有发送导航目标或非零速度命令。测试期间 `/cmd_vel`、`/cmd_vel_nav` 没有非零输出，里程计速度为 0。由于雷达扇区存在约 0.62 m 近障碍，且系统 CPU 负载较高，没有执行实车运动和底盘 watchdog 的实际动作测试。
+
+## 2. 测试结果
+
+新启动实例的 Nav2 lifecycle 节点成功进入 `active`，并且 TF 检查通过。此前旧实例曾加载过期的 `/tmp/launch_params_*`，导致 local costmap 等待 `odom_combined`；重启后当前 profile 使用 `odom`，该问题消失。
+
+### 2.1 消息链路
+
+独立 rclpy 订阅，稳定窗口约 20 秒：
+
+| 话题 | 平均间隔 | P95 | P99 | 最大间隔 | 时间戳年龄 |
+| --- | ---: | ---: | ---: | ---: | ---: |
+| `/scan` | 82.1 ms | 87.4 ms | 109.5 ms | 117.5 ms | P99 10.6 ms，最大 113.5 ms |
+| `/obstacles` | 82.0 ms | 88.5 ms | 104.7 ms | 115.9 ms | P99 18.2 ms，最大 24.7 ms |
+| `/odom_combined` | 50.1 ms | 71.4 ms | 81.5 ms | 112.4 ms | P95 18.8 ms，最大 28.4 ms |
+
+结论：雷达和障碍检测平均链路延迟不高，但存在 100 ms 级别长尾；odom 约 20 Hz，反馈周期本身约 50 ms。
+
+### 2.2 代价地图
+
+当前默认 launch 实际加载 `nav2_profile_10.yaml`，运行时参数为：
+
+```yaml
+local_costmap:
+  update_frequency: 5.0
+  publish_frequency: 2.0
+global_costmap:
+  update_frequency: 1.0
+  publish_frequency: 1.0
+```
+
+有效话题为 `/local_costmap/costmap_raw` 和 `/global_costmap/costmap_raw`，类型为 `nav2_msgs/msg/Costmap`。
+
+实测：
+
+- local costmap：约 1.67 Hz，平均间隔 598 ms，最大约 602 ms
+- global costmap：约 0.75 Hz，平均间隔约 1337 ms，最大约 2001 ms
+
+源码 `nav2_params.yaml` 中虽然已经是 local `10 Hz / 4 Hz`，但默认 launch 没有使用这个文件。因此此前提高频率的修改没有进入本次实际运行链路。
+
+### 2.3 TF 与时间戳
+
+启动和运行初期曾出现：
+
+```text
+Lookup would require extrapolation into the future
+```
+
+一个样本中，请求时间比最新 TF 超前约 81 ms。稳定运行后连续约 20 秒没有继续增加 TF 失败计数，但启动阶段曾记录 10 条 `ObstacleArrayLayer: TF failed`。
+
+这说明 20 ms TF 查询约束会暴露雷达时间戳和 odom TF 的长尾。问题不是平均延迟，而是偶发的未来时间查询；仅增大 lookup timeout 不能完全解决请求时间已经超出 TF 缓存的问题。
+
+### 2.4 底盘处理耗时
+
+`origincar_base` 日志显示：
+
+```text
+scan_to_odom 平均约 44～47 ms
+scan_to_odom 最大约 624.6 ms
+```
+
+这仍然不满足平均小于 25 ms、P99 小于 45 ms 的目标。该长尾会直接影响 odom TF、costmap TF 查询和控制闭环。
+
+### 2.5 雷达元数据
+
+8 秒采样约 98 帧，约 375 个 range 点：
+
+- `scan_time`：0 到 122 ms，平均约 75.2 ms
+- `time_increment`：0 到 0.327 ms，平均约 0.201 ms
+- 实际消息间隔约 82 ms
+
+平均值接近实际雷达周期，但存在 `scan_time=0` 的异常样本。当前障碍检测主要使用 `header.stamp`，暂时没有阻塞运行；后续做去畸变或点时间补偿前必须处理这些异常值。
+
+## 3. DDS UDP 根因分析
+
+### 3.1 直接证据
+
+`obstacle_nav2.launch.py` 在第 144 至 146 行为所有包含的节点设置：
+
+```python
+SetEnvironmentVariable(
+    name='FASTRTPS_DEFAULT_PROFILES_FILE',
+    value=fastdds_profile_path)
+```
+
+该 XML 的核心配置为：
+
+```xml
+<useBuiltinTransports>false</useBuiltinTransports>
+<userTransports>
+  <transport_id>udp_transport</transport_id>
+</userTransports>
+```
+
+这会关闭 FastDDS 内置传输，包含同机进程间通常使用的 shared memory，只保留 UDPv4。于是底盘、TF、Nav2 各 server、costmap 和障碍检测之间的同机通信也通过 UDP loopback 完成。
+
+在完整系统运行期间，对底盘、robot_state_publisher 和 bt_navigator 的热点线程执行 `strace`：
+
+- `sendto` 大量发往 `127.0.0.1:12913`、`12931`、`12933`、`12935`、`12941`、`12945`
+- 每次发送前高频调用 `setsockopt(SO_SNDTIMEO)`
+- 该调用持续返回 `EDOM (Numerical argument out of domain)`
+- 单个底盘进程约 3 秒内出现 9000 级别的 `sendto` 和同量级的 `setsockopt`
+
+稳定窗口的 8 核 CPU 采样约为：
+
+```text
+47% user + 31% system
+```
+
+主要进程瞬时 CPU 约为：
+
+- `origincar_base`：78%
+- `robot_state_publisher`：59%
+- `bt_navigator`：59%
+- `planner_server`：40%
+- `controller_server`：40%
+
+### 3.2 谁导致了 UDP 流量
+
+结论不是“obstacle_scanner 单独导致”，而是：
+
+1. `fastdds_udp_only.xml` 强制整套 launch 使用 UDP-only，禁用了同机 shared memory，这是高 UDP 流量和高系统调用开销的首要放大器。
+2. 完整 Nav2/底盘图中的多个节点同时发布和订阅 TF、odom、costmap、障碍和 lifecycle 数据，所有这些进程间通信都被放到了 UDP loopback。
+3. `foxglove_bridge` 使用同一 ROS domain，订阅多个高频话题，会增加 DDS reader 数量和数据转发负载，但它不是唯一根因。停止本次 Nav2 后，Foxglove 的瞬时 CPU 采样降到约 0.4%，说明高负载主要随完整 ROS 图出现。
+4. 当前独立运行的 `static_transform_publisher` 瞬时 CPU 约 0.2%，不是主要 CPU 根因。
+
+因此，最可能的根因链是：
+
+```text
+UDP-only 配置
+  -> 同机通信全部走 FastDDS UDP loopback
+  -> 多节点 DDS reader/writer 产生大量本地 UDP 包
+  -> FastDDS 高频设置 SO_SNDTIMEO 且返回 EDOM
+  -> system CPU、上下文切换和调度延迟升高
+  -> scan_to_odom、TF 和 Nav2 回调出现长尾
+```
+
+仅凭一次运行不能把 `EDOM` 归因到某一个 ROS 节点的业务代码；需要按下面计划做 UDP-only 与 shared-memory 的 A/B 对比。现有证据已经足以把 `fastdds_udp_only.xml` 列为第一优先级排查对象。
+
+## 4. 优化计划
+
+### 阶段 0：建立可重复基线
+
+1. 保持当前 `ROS_DOMAIN_ID=22`，关闭 Foxglove，单独启动底盘、雷达、障碍检测和 Nav2。
+2. 固定采样 60 秒：`/scan`、`/obstacles`、`/odom_combined`、`/local_costmap/costmap_raw`、`/tf`。
+3. 记录每个话题的平均、P95、P99、最大间隔和消息时间戳年龄。
+4. 同时记录 8 核 CPU、上下文切换、FastDDS UDP 端口和 `setsockopt EDOM` 次数。
+
+验收：同一配置重复两次，关键指标差异小于 10%。
+
+### 阶段 1：确认并修复 DDS 传输配置
+
+1. A 组：当前 `fastdds_udp_only.xml`。
+2. B 组：移除 `FASTRTPS_DEFAULT_PROFILES_FILE`，使用 FastDDS 默认 shared memory + UDP。
+3. C 组：显式配置 shared memory + UDP，仅在需要跨主机时保留 UDP，不关闭内置传输。
+4. 每组重复阶段 0 的 60 秒采样。
+5. 统计 `strace -c` 中 `sendto`、`setsockopt`、`futex`，确认 `SO_SNDTIMEO -> EDOM` 是否消失。
+6. Foxglove 单独做一组开启/关闭对比，避免把桥接器负载误判为 Nav2 根因。
+
+优先实现：默认 launch 不再强制 UDP-only。若确实需要跨主机通信，再使用同时启用 shared memory 和 UDP 的 profile，并限制网卡/接口范围。
+
+验收：
+
+- `SO_SNDTIMEO -> EDOM` 为 0
+- 完整图空闲运行时系统 CPU 显著下降
+- `/scan`、`/obstacles`、`/odom_combined` P99 不恶化
+- `scan_to_odom` 长尾不再因 DDS 配置放大
+
+### 阶段 2：统一 Nav2 参数来源并提高 costmap 频率
+
+1. 明确 `nav2_params.yaml`、`nav2_profile_10.yaml`、`nav2_profile_11.yaml` 的职责。
+2. 默认 launch 不要继续硬编码与实际调参目标不一致的 profile。
+3. 如果 profile 10 是默认实车配置，将 local costmap 的 update/publish 调整到目标值；建议先验证 `10 Hz / 8～10 Hz`，不要一次直接追求更高。
+4. global costmap 保持较低频率，避免把 CPU 消耗在不影响即时避障的全局地图上。
+5. 重新测 `/local_costmap/costmap_raw` 的实际频率，而不是只检查 YAML。
+
+验收：local costmap 发布 P95 间隔接近 100～125 ms，且 CPU 不出现持续饱和；costmap update 不产生 deadline 或 missed-cycle 日志。
+
+### 阶段 3：修复 TF 时间戳长尾
+
+1. 对比 `/scan` header、`/obstacles` header、odom TF 发布时间和当前 ROS 时间。
+2. 对未来时间请求做明确策略：短暂等待、丢弃异常帧或使用最新可用 TF，不能让单个回调阻塞整个障碍链。
+3. 保持 `odom`、`base_footprint`、`laser_link` 帧名统一，禁止旧配置回退到 `odom_combined`。
+4. 对雷达时间元数据做校验：`scan_time <= 0` 时使用最近有效周期，并限制异常跳变；`time_increment` 必须与 range 数量一致。
+
+验收：连续 60 秒无 `extrapolation into the future`；障碍消息丢弃率为 0；TF 时间戳年龄 P99 小于 20 ms，或对超限帧有明确计数和降级行为。
+
+### 阶段 4：优化底盘处理时序
+
+1. 将 `scan_to_odom` 计算和串口收发的耗时分开统计。
+2. 找出 624 ms 长尾对应的线程、锁等待、日志和内存分配。
+3. 检查 DDS 高负载修复后 `origincar_base` 的 CPU 和回调耗时是否恢复。
+4. 保持底盘 TX 周期 20 ms、PC watchdog 150 ms；随后与 STM32 固件 watchdog 联调。
+
+验收：平均处理耗时小于 25 ms，P99 小于 45 ms，不允许持续出现大于 50 ms 的控制周期；命令停止后 150～200 ms 内归零。
+
+### 阶段 5：Nav2 控制和实车验证
+
+1. 仅在阶段 1～4通过后启用 MPPI 运动测试。
+2. 先在开阔区域发送短距离、低速度、带终点姿态的目标。
+3. 记录 MPPI 循环耗时、`cmd_vel_nav -> cmd_vel -> odom` 响应和实际舵角响应。
+4. 再测试近障碍和急转弯，不同时修改多个 critic 参数。
+
+验收：MPPI 平均循环小于 25 ms，P99 小于 45 ms；障碍从 `/scan` 到 costmap 的端到端延迟满足目标；无持续恢复行为、振荡或控制周期丢失。
+
+## 5. 当前结论
+
+当前还不能宣称系统达到 50～100 ms 障碍反应目标。雷达到障碍检测的平均链路已经接近 80～90 ms，但代价地图约 600 ms 发布一次，DDS UDP-only 引起的高 CPU/高系统调用负载，以及底盘 `scan_to_odom` 的 624 ms 长尾仍是主要阻塞项。下一步应优先做 DDS A/B，对比结果出来前不建议继续调 MPPI critic 权重。