good

test for gc
改了下地图和部分规划参数
2026-08-12 21:07:21 +08:00 · 2026-08-11 19:38:10 +08:00 · 2026-08-10 19:46:09 +08:00 · 2026-08-10 15:48:25 +08:00 · 2026-08-09 21:02:42 +08:00 · 2026-08-06 18:57:54 +08:00
138 changed files with 19157 additions and 573 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -11,3 +11,5 @@ log
 .vscode

 datas
+
+running_logs
--- a/AGENTS.md
+++ b/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 转换记录。
--- a/CLAUDE.md
+++ b/CLAUDE.md
@@ -151,7 +151,7 @@ World 文件在 `src/origincar_description/world/`:

 ## 实车底盘驱动包 (origincar_base)

- `origincar_base_node`（C++）: 串口读写（/dev/ttyACM0, 115200bps）、航迹推算、四元数姿态解算（Mahony AHRS）
+- `origincar_base_node`（C++）: 串口读写（/dev/ttyACM0, 921600bps）、航迹推算、四元数姿态解算（Mahony AHRS）
 - `cmd_vel_to_ackermann_drive.py`（Python）: Twist → AckermannDriveStamped 转换，wheelbase=0.143
 - 帧协议: 24 字节收（帧头0x7B/帧尾0x7D）/ 11 字节发
 - EKF 融合: `/odom` + IMU → `/odom_combined`，`two_d_mode=true`
--- a/docs/superpowers/plans/2026-08-12-racing-control-final-home-fallback.md
+++ b/docs/superpowers/plans/2026-08-12-racing-control-final-home-fallback.md
@@ -0,0 +1,631 @@
+# Racing Control 最终回家保护实现计划
+
+> **给 agentic workers:** 必须使用 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans` 按任务逐步实现本计划。所有步骤用 checkbox（`- [ ]`）跟踪。
+
+**目标:** 给 `racing_control` 增加最终兜底回家逻辑，让普通 Nav2/恢复全部失败后，小车仍然低速朝 home 前进并尝试完赛，而不是停在 `race failed`。
+
+**架构:** 核心控制算法单独放到 `final_home_fallback.hpp/.cpp`，主程序不直接写算法，只负责状态机、参数、odom/home 输入和 `/cmd_vel` 发布。`racing_control.cpp` 新增 `FinalHomeFallback` 阶段，在该阶段每个控制周期调用算法生成速度，同时按固定间隔尝试 Nav2 回家；Nav2 成功则切回正常完成，Nav2 不成功则继续直控。
+
+**技术栈:** ROS2 Humble、C++17、`rclcpp`、`geometry_msgs/msg/Twist`、`nav2_msgs/action/NavigateToPose`、gtest、现有 `racing_control` 包。
+
+---
+
+### 文件结构
+
+- 新建: `/home/sunrise/yiliao_ws/src/racing_control/include/racing_control/final_home_fallback.hpp`
+  - 声明最终回家保护算法的数据结构和函数。
+- 新建: `/home/sunrise/yiliao_ws/src/racing_control/src/final_home_fallback.cpp`
+  - 实现 yaw 提取、角度归一化、home 方向误差计算、直控速度生成。
+- 修改: `/home/sunrise/yiliao_ws/src/racing_control/CMakeLists.txt`
+  - 把 `src/final_home_fallback.cpp` 加入 `racing_control_core`、`racing_control` 和 helper 测试目标。
+- 修改: `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp`
+  - 只接入最终保护阶段、参数、Nav2 重试和 `/cmd_vel` 发布，不写核心算法。
+- 修改: `/home/sunrise/yiliao_ws/src/racing_control/config/racing_control.yaml`
+  - 增加最终保护参数默认值。
+- 修改: `/home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.cpp`
+  - 增加算法单测，验证角度归一化、到达判断、转弯方向、前进控制。
+
+---
+
+### Task 1: 写最终保护算法的失败测试
+
+**Files:**
+- Modify: `/home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.cpp`
+- Create later: `/home/sunrise/yiliao_ws/src/racing_control/include/racing_control/final_home_fallback.hpp`
+- Create later: `/home/sunrise/yiliao_ws/src/racing_control/src/final_home_fallback.cpp`
+
+- [ ] **Step 1: 引入新头文件**
+
+在 `/home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.cpp` 的 include 区域加入：
+
+```cpp
+#include "racing_control/final_home_fallback.hpp"
+```
+
+- [ ] **Step 2: 添加失败测试**
+
+把下面测试追加到 `/home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.cpp`：
+
+```cpp
+TEST(FinalHomeFallback, NormalizesAnglesToShortestRotation)
+{
+  EXPECT_NEAR(racing_control::normalizeAngle(3.5), -2.7831853071795862, 1e-6);
+  EXPECT_NEAR(racing_control::normalizeAngle(-3.5), 2.7831853071795862, 1e-6);
+  EXPECT_NEAR(racing_control::normalizeAngle(0.25), 0.25, 1e-6);
+}
+
+TEST(FinalHomeFallback, StopsInsideDistanceTolerance)
+{
+  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
+  const auto home = racing_control::poseFromXYYaw(0.1, 0.1, 0.0, "odom");
+
+  const auto command = racing_control::computeFinalHomeFallbackCommand(
+    current, home, 0.2, 0.3, 0.12, 0.5, 0.7);
+
+  EXPECT_TRUE(command.reached);
+  EXPECT_DOUBLE_EQ(command.twist.linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(command.twist.angular.z, 0.0);
+}
+
+TEST(FinalHomeFallback, TurnsTowardHomeWhenYawErrorIsLarge)
+{
+  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
+  const auto home = racing_control::poseFromXYYaw(0.0, 1.0, 0.0, "odom");
+
+  const auto command = racing_control::computeFinalHomeFallbackCommand(
+    current, home, 0.2, 0.3, 0.12, 0.5, 0.7);
+
+  EXPECT_FALSE(command.reached);
+  EXPECT_NEAR(command.distance, 1.0, 1e-6);
+  EXPECT_NEAR(command.yaw_error, M_PI_2, 1e-6);
+  EXPECT_DOUBLE_EQ(command.twist.linear.x, 0.12);
+  EXPECT_DOUBLE_EQ(command.twist.angular.z, 0.7);
+}
+
+TEST(FinalHomeFallback, DrivesForwardWhenYawErrorIsSmall)
+{
+  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
+  const auto home = racing_control::poseFromXYYaw(1.0, 0.1, 0.0, "odom");
+
+  const auto command = racing_control::computeFinalHomeFallbackCommand(
+    current, home, 0.2, 0.3, 0.12, 0.5, 0.7);
+
+  EXPECT_FALSE(command.reached);
+  EXPECT_GT(command.twist.linear.x, 0.0);
+  EXPECT_NEAR(command.twist.angular.z, command.yaw_error * 0.5, 1e-6);
+}
+```
+
+- [ ] **Step 3: 运行测试并确认失败**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon test --packages-select racing_control --ctest-args -R test_racing_control_helpers --output-on-failure"'
+```
+
+预期：失败，原因是 `racing_control/final_home_fallback.hpp` 或 `computeFinalHomeFallbackCommand` 尚不存在。
+
+---
+
+### Task 2: 新建最终保护算法文件
+
+**Files:**
+- Create: `/home/sunrise/yiliao_ws/src/racing_control/include/racing_control/final_home_fallback.hpp`
+- Create: `/home/sunrise/yiliao_ws/src/racing_control/src/final_home_fallback.cpp`
+- Modify: `/home/sunrise/yiliao_ws/src/racing_control/CMakeLists.txt`
+
+- [ ] **Step 1: 创建算法头文件**
+
+创建 `/home/sunrise/yiliao_ws/src/racing_control/include/racing_control/final_home_fallback.hpp`：
+
+```cpp
+#ifndef RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_
+#define RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace racing_control
+{
+
+struct FinalHomeFallbackCommand
+{
+  geometry_msgs::msg::Twist twist;
+  double distance{0.0};
+  double yaw_error{0.0};
+  bool reached{false};
+};
+
+double normalizeAngle(double angle);
+
+double yawFromPose(const geometry_msgs::msg::PoseStamped & pose);
+
+FinalHomeFallbackCommand computeFinalHomeFallbackCommand(
+  const geometry_msgs::msg::PoseStamped & current,
+  const geometry_msgs::msg::PoseStamped & home,
+  double distance_tolerance,
+  double yaw_tolerance,
+  double forward_speed,
+  double yaw_gain,
+  double max_turn_speed);
+
+}  // namespace racing_control
+
+#endif  // RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_
+```
+
+- [ ] **Step 2: 创建算法实现文件**
+
+创建 `/home/sunrise/yiliao_ws/src/racing_control/src/final_home_fallback.cpp`：
+
+```cpp
+#include "racing_control/final_home_fallback.hpp"
+
+#include <algorithm>
+#include <cmath>
+
+namespace racing_control
+{
+
+double normalizeAngle(double angle)
+{
+  while (angle > M_PI) {
+    angle -= 2.0 * M_PI;
+  }
+  while (angle < -M_PI) {
+    angle += 2.0 * M_PI;
+  }
+  return angle;
+}
+
+double yawFromPose(const geometry_msgs::msg::PoseStamped & pose)
+{
+  const auto & q = pose.pose.orientation;
+  return std::atan2(
+    2.0 * (q.w * q.z + q.x * q.y),
+    1.0 - 2.0 * (q.y * q.y + q.z * q.z));
+}
+
+FinalHomeFallbackCommand computeFinalHomeFallbackCommand(
+  const geometry_msgs::msg::PoseStamped & current,
+  const geometry_msgs::msg::PoseStamped & home,
+  const double distance_tolerance,
+  const double yaw_tolerance,
+  const double forward_speed,
+  const double yaw_gain,
+  const double max_turn_speed)
+{
+  FinalHomeFallbackCommand command;
+  const auto dx = home.pose.position.x - current.pose.position.x;
+  const auto dy = home.pose.position.y - current.pose.position.y;
+  command.distance = std::hypot(dx, dy);
+
+  if (command.distance <= distance_tolerance) {
+    command.reached = true;
+    return command;
+  }
+
+  const auto target_yaw = std::atan2(dy, dx);
+  command.yaw_error = normalizeAngle(target_yaw - yawFromPose(current));
+  command.twist.linear.x = forward_speed;
+
+  if (std::abs(command.yaw_error) > yaw_tolerance) {
+    command.twist.angular.z = command.yaw_error > 0.0 ? max_turn_speed : -max_turn_speed;
+  } else {
+    const auto raw_turn = command.yaw_error * yaw_gain;
+    command.twist.angular.z = std::clamp(raw_turn, -max_turn_speed, max_turn_speed);
+  }
+
+  return command;
+}
+
+}  // namespace racing_control
+```
+
+- [ ] **Step 3: 修改 CMakeLists**
+
+在 `/home/sunrise/yiliao_ws/src/racing_control/CMakeLists.txt` 中：
+
+把 `racing_control_core` 改为：
+
+```cmake
+add_library(racing_control_core
+  src/candidate_waypoint_selector.cpp
+  src/final_home_fallback.cpp
+)
+```
+
+把 `add_executable(racing_control ...)` 改为包含新文件：
+
+```cmake
+add_executable(racing_control
+  src/racing_control.cpp
+  src/candidate_waypoint_selector.cpp
+  src/final_home_fallback.cpp
+)
+```
+
+把测试目标的 `target_sources(test_racing_control_helpers PRIVATE ...)` 改为：
+
+```cmake
+  target_sources(test_racing_control_helpers PRIVATE
+    src/candidate_waypoint_selector.cpp
+    src/final_home_fallback.cpp
+  )
+```
+
+- [ ] **Step 4: 运行 helper 测试**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon test --packages-select racing_control --ctest-args -R test_racing_control_helpers --output-on-failure"'
+```
+
+预期：`test_racing_control_helpers` 通过。
+
+---
+
+### Task 3: 增加最终保护参数和状态
+
+**Files:**
+- Modify: `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp`
+- Modify: `/home/sunrise/yiliao_ws/src/racing_control/config/racing_control.yaml`
+
+- [ ] **Step 1: 引入算法头文件**
+
+在 `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp` include 区域加入：
+
+```cpp
+#include "racing_control/final_home_fallback.hpp"
+```
+
+- [ ] **Step 2: 增加阶段枚举和名称**
+
+在 `enum class Stage` 中加入：
+
+```cpp
+  FinalHomeFallback,
+```
+
+在 `stageName(...)` 中加入：
+
+```cpp
+    case Stage::FinalHomeFallback:
+      return "最终回家保护";
+```
+
+- [ ] **Step 3: 加载参数**
+
+在 `loadParameters()` 里，恢复参数附近加入：
+
+```cpp
+    enable_final_home_fallback_ =
+      declare_parameter<bool>("enable_final_home_fallback", true);
+    final_home_distance_tolerance_ =
+      declare_parameter<double>("final_home_distance_tolerance", 0.2);
+    final_home_yaw_tolerance_ =
+      declare_parameter<double>("final_home_yaw_tolerance", 0.3);
+    final_home_forward_speed_ =
+      declare_parameter<double>("final_home_forward_speed", 0.12);
+    final_home_yaw_gain_ =
+      declare_parameter<double>("final_home_yaw_gain", 0.5);
+    final_home_max_turn_speed_ =
+      declare_parameter<double>("final_home_max_turn_speed", 0.7);
+    final_home_nav_retry_interval_sec_ =
+      declare_parameter<double>("final_home_nav_retry_interval_sec", 0.8);
+```
+
+- [ ] **Step 4: 增加成员变量**
+
+在 `RacingControl` 成员变量区加入：
+
+```cpp
+  bool enable_final_home_fallback_{true};
+  bool final_home_nav_retry_in_flight_{false};
+  double final_home_distance_tolerance_{0.2};
+  double final_home_yaw_tolerance_{0.3};
+  double final_home_forward_speed_{0.12};
+  double final_home_yaw_gain_{0.5};
+  double final_home_max_turn_speed_{0.7};
+  double final_home_nav_retry_interval_sec_{0.8};
+  rclcpp::Time last_final_home_nav_retry_time_{0, 0, RCL_ROS_TIME};
+```
+
+- [ ] **Step 5: 增加 YAML 默认值**
+
+在 `/home/sunrise/yiliao_ws/src/racing_control/config/racing_control.yaml` 的恢复参数附近加入：
+
+```yaml
+    # 最终回家保护：普通 Nav2/恢复失败后，低速直控回 home，同时定时探测 Nav2 是否恢复。
+    enable_final_home_fallback: true
+    final_home_distance_tolerance: 0.2
+    final_home_yaw_tolerance: 0.3
+    final_home_forward_speed: 0.12
+    final_home_yaw_gain: 0.5
+    final_home_max_turn_speed: 0.7
+    final_home_nav_retry_interval_sec: 0.8
+```
+
+- [ ] **Step 6: 编译**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON"'
+```
+
+预期：编译通过。
+
+---
+
+### Task 4: 把最终失败切入 FinalHomeFallback
+
+**Files:**
+- Modify: `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp`
+
+- [ ] **Step 1: 新增 `startFinalHomeFallback`**
+
+在 `failRace(...)` 附近加入：
+
+```cpp
+  void startFinalHomeFallback(const std::string & reason)
+  {
+    if (!enable_final_home_fallback_) {
+      stage_ = Stage::Failed;
+      recovery_in_progress_ = false;
+      cancelActiveFollowGoal();
+      publishRecoveryVelocity(0.0);
+      publishSign(sign_qr_disable_);
+      RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
+      return;
+    }
+
+    stage_ = Stage::FinalHomeFallback;
+    recovery_in_progress_ = false;
+    final_home_nav_retry_in_flight_ = false;
+    last_final_home_nav_retry_time_ = now() - rclcpp::Duration::from_seconds(
+      final_home_nav_retry_interval_sec_);
+    if (recovery_timer_) {
+      recovery_timer_->cancel();
+    }
+    if (recovery_clear_wait_timer_) {
+      recovery_clear_wait_timer_->cancel();
+    }
+    recovery_after_clear_callback_ = nullptr;
+    recovery_costmap_clear_pending_ = 0;
+    cancelActiveFollowGoal();
+    publishRecoveryVelocity(0.0);
+    publishSign(sign_qr_disable_);
+    startStage(Stage::FinalHomeFallback, 0.0);
+    RCLCPP_ERROR(
+      get_logger(), "entering final home fallback instead of race failed: %s", reason.c_str());
+  }
+```
+
+- [ ] **Step 2: 修改 `failRace`**
+
+把 `failRace(...)` 函数体替换为：
+
+```cpp
+  void failRace(const std::string & reason)
+  {
+    startFinalHomeFallback(reason);
+  }
+```
+
+- [ ] **Step 3: 修改 tick 入口**
+
+在 `tick()` 中，把终止保护后面加入最终保护 tick：
+
+```cpp
+    if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
+      return;
+    }
+
+    if (stage_ == Stage::FinalHomeFallback) {
+      tickFinalHomeFallback();
+      return;
+    }
+```
+
+- [ ] **Step 4: 编译确认缺口**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON"'
+```
+
+预期：如果还没写 `tickFinalHomeFallback()`，会因为函数缺失失败；继续 Task 5。
+
+---
+
+### Task 5: 接入算法、发布直控速度、定时重试 Nav2
+
+**Files:**
+- Modify: `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp`
+
+- [ ] **Step 1: 新增 home 选择 helper**
+
+在路线 helper 附近加入：
+
+```cpp
+  geometry_msgs::msg::PoseStamped fallbackHomePose() const
+  {
+    if (selected_direction_ == RouteDirection::Clockwise ||
+      selected_direction_ == RouteDirection::Counterclockwise)
+    {
+      return selectedRoute().home_pose;
+    }
+    return clockwise_route_.home_pose;
+  }
+```
+
+- [ ] **Step 2: 新增最终保护 tick**
+
+在恢复相关函数附近加入：
+
+```cpp
+  void tickFinalHomeFallback()
+  {
+    const auto current = currentPoseFromOdom();
+    if (!current) {
+      publishRecoveryVelocity(0.0);
+      RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 1000,
+        "final home fallback waiting for odom");
+      return;
+    }
+
+    const auto home = fallbackHomePose();
+    const auto command = computeFinalHomeFallbackCommand(
+      *current, home, final_home_distance_tolerance_, final_home_yaw_tolerance_,
+      final_home_forward_speed_, final_home_yaw_gain_, final_home_max_turn_speed_);
+
+    if (command.reached) {
+      publishRecoveryVelocity(0.0);
+      finishRace();
+      return;
+    }
+
+    if (recovery_cmd_vel_pub_) {
+      recovery_cmd_vel_pub_->publish(command.twist);
+    }
+
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 1000,
+      "final home fallback direct control: distance=%.2f yaw_error=%.2f vx=%.2f wz=%.2f",
+      command.distance, command.yaw_error, command.twist.linear.x, command.twist.angular.z);
+
+    maybeRetryFinalHomeNavigation(home);
+  }
+```
+
+- [ ] **Step 3: 新增 Nav2 定时重试**
+
+在 `sendNavigateGoalAttempt(...)` 附近加入：
+
+```cpp
+  void maybeRetryFinalHomeNavigation(const geometry_msgs::msg::PoseStamped & home)
+  {
+    if (final_home_nav_retry_in_flight_) {
+      return;
+    }
+    if ((now() - last_final_home_nav_retry_time_).seconds() <
+      final_home_nav_retry_interval_sec_)
+    {
+      return;
+    }
+    if (!navigate_client_->wait_for_action_server(10ms)) {
+      last_final_home_nav_retry_time_ = now();
+      return;
+    }
+
+    final_home_nav_retry_in_flight_ = true;
+    last_final_home_nav_retry_time_ = now();
+
+    NavigateToPose::Goal goal;
+    goal.pose = stampPose(home);
+
+    auto options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
+    options.goal_response_callback =
+      [this](NavigateGoalHandle::SharedPtr goal_handle) {
+        if (stage_ != Stage::FinalHomeFallback) {
+          return;
+        }
+        if (!goal_handle) {
+          final_home_nav_retry_in_flight_ = false;
+          RCLCPP_WARN(get_logger(), "final home fallback Nav2 retry rejected");
+        }
+      };
+    options.result_callback =
+      [this](const NavigateGoalHandle::WrappedResult & result) {
+        if (stage_ != Stage::FinalHomeFallback) {
+          return;
+        }
+        final_home_nav_retry_in_flight_ = false;
+        if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
+          publishRecoveryVelocity(0.0);
+          finishRace();
+          return;
+        }
+        RCLCPP_WARN(get_logger(), "final home fallback Nav2 retry failed; keeping direct control");
+      };
+
+    navigate_client_->async_send_goal(goal, options);
+    RCLCPP_INFO(get_logger(), "final home fallback sent Nav2 home retry");
+  }
+```
+
+- [ ] **Step 4: 编译**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON"'
+```
+
+预期：编译通过。
+
+---
+
+### Task 6: 验证和检查
+
+**Files:**
+- Test only.
+
+- [ ] **Step 1: 跑 helper 测试**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon test --packages-select racing_control --ctest-args -R test_racing_control_helpers --output-on-failure"'
+```
+
+预期：通过。
+
+- [ ] **Step 2: 查看测试结果**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'bash -lc "cd /home/sunrise/yiliao_ws && colcon test-result --test-result-base build/racing_control --verbose"'
+```
+
+预期：`racing_control` 无失败。
+
+- [ ] **Step 3: 检查改动范围**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'cd /home/sunrise/yiliao_ws && git status --short src/racing_control'
+```
+
+预期：只出现 `racing_control` 的源码、头文件、CMake、yaml、测试改动。
+
+- [ ] **Step 4: 审查最终 diff**
+
+运行：
+
+```bash
+ssh sunrise@192.168.10.210 'cd /home/sunrise/yiliao_ws && git diff -- src/racing_control | sed -n "1,280p"'
+```
+
+重点检查：
+- 核心算法只在 `final_home_fallback.hpp/.cpp`，不在 `racing_control.cpp`。
+- `racing_control.cpp` 只做阶段切换、参数读取、调用算法、发布速度、重试 Nav2。
+- `FinalHomeFallback` 阶段不会被 `tick()` 提前 return。
+- 进入距离阈值 `0.2m` 后一定发布 0 速度并 `finishRace()`。
+- Nav2 重试有 `final_home_nav_retry_in_flight_` 和 `final_home_nav_retry_interval_sec_`，不会每帧刷 action。
+
+---
+
+### 自检
+
+- 覆盖需求：已覆盖最终失败兜底、每帧按 odom-home 计算、0.3rad/0.2m 阈值、距离到达即停止、Nav2 周期性重试、成功后切回正常完成。
+- 文件边界：核心算法作为新文件 `final_home_fallback.hpp/.cpp`，主程序不承载算法。
+- 占位符检查：无 `TBD`、`TODO` 或“之后实现”。
+- 类型一致性：函数名 `computeFinalHomeFallbackCommand`、结构体 `FinalHomeFallbackCommand`、参数名和阶段名在计划内一致。
--- a/my_model/image.png
+++ b/my_model/image.png
--- a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml
+++ b/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话题
--- a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc
+++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc
@@ -1009,8 +1009,8 @@ namespace lslidar_driver
 					scan->ranges.assign(scan_num, std::numeric_limits<float>::infinity());
 					scan->intensities.reserve(scan_num);
 					scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
-					// scan->scan_time = scan_time;
-					// scan->time_increment = scan_time / (double)(count_num);
+					scan->scan_time = scan_time;
+					scan->time_increment = scan_time / static_cast<double>(scan_num);

 					for (int k = 0; k < scan_num; k++)
 					{
@@ -1168,8 +1168,8 @@ namespace lslidar_driver
 					scan->ranges.assign(scan_num, std::numeric_limits<float>::infinity());
 					scan->intensities.reserve(scan_num);
 					scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
-					scan->scan_time = 0.1;
-					scan->time_increment = 0.1 / (double)(scan_num - 1);
+					scan->scan_time = scan_time;
+					scan->time_increment = scan_time / static_cast<double>(scan_num - 1);

 					int start_num = floor(angle_able_min * count_num / 360);
 					int end_num = floor(angle_able_max * count_num / 360);
--- a/src/data_collection_tools/x5_udp_cmd_bridge.py
+++ b/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()
--- a/src/gc/项目总结_yiliao_ws.md
+++ b/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 内容） |
--- a/src/map/map06.png
+++ b/src/map/map06.png
--- a/src/map/nav2_costmap_binary
+++ b/src/map/nav2_costmap_binary
--- a/src/map/nav2_costmap_binary
+++ b/src/map/nav2_costmap_binary
--- a/src/map/nav2_costmap_binary
+++ b/src/map/nav2_costmap_binary
--- a/src/map/nav2_costmap_binary
+++ b/src/map/nav2_costmap_binary
--- a/src/map/nav2_costmap_binary_01.png
+++ b/src/map/nav2_costmap_binary_01.png
--- a/src/map/nav2_costmap_binary_06.png
+++ b/src/map/nav2_costmap_binary_06.png
--- a/src/map/nav2_costmap_map.yaml
+++ b/src/map/nav2_costmap_map.yaml
@@ -0,0 +1,7 @@
+image: map06.png
+mode: trinary
+resolution: 0.01
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
--- a/src/navigation/obstacle_nav2/CMakeLists.txt
+++ b/src/navigation/obstacle_nav2/CMakeLists.txt
@@ -16,6 +16,34 @@ find_package(obstacle_scanner REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(rclcpp_action REQUIRED)
+
+add_library(nav2_profile_loader
+  src/nav2_profile_loader.cpp
+)
+target_include_directories(nav2_profile_loader PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(nav2_profile_loader yaml-cpp)
+ament_target_dependencies(nav2_profile_loader
+  rclcpp
+)
+
+add_library(trajectory_guard_lib
+  src/trajectory_guard.cpp
+)
+target_include_directories(trajectory_guard_lib PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(trajectory_guard_lib
+  geometry_msgs
+  nav_msgs
+)

 add_library(obstacle_array_layer SHARED
  src/obstacle_array_layer.cpp
@@ -39,14 +67,53 @@ ament_target_dependencies(obstacle_array_layer

 pluginlib_export_plugin_description_file(nav2_costmap_2d obstacle_nav2_plugins.xml)

-install(TARGETS obstacle_array_layer
+add_executable(nav2_profile_tuner
+  src/nav2_profile_tuner.cpp
+)
+target_link_libraries(nav2_profile_tuner nav2_profile_loader)
+ament_target_dependencies(nav2_profile_tuner
+  rclcpp
+  rcl_interfaces
+  std_msgs
+)
+
+add_executable(trajectory_guard_node
+  src/trajectory_guard_node.cpp
+)
+target_include_directories(trajectory_guard_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(trajectory_guard_node trajectory_guard_lib)
+ament_target_dependencies(trajectory_guard_node
+  geometry_msgs
+  nav2_msgs
+  nav_msgs
+  rclcpp
+  rclcpp_action
+  tf2
+)
+
+install(TARGETS
+  nav2_profile_loader
+  trajectory_guard_lib
+  obstacle_array_layer
+  nav2_profile_tuner
+  trajectory_guard_node
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 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
@@ -67,6 +134,27 @@ if(BUILD_TESTING)
    nav_msgs
    std_msgs
  )
+
+  ament_add_gtest(test_nav2_profile_loader test/test_nav2_profile_loader.cpp
+    src/nav2_profile_loader.cpp
+  )
+  target_include_directories(test_nav2_profile_loader PRIVATE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  )
+  target_link_libraries(test_nav2_profile_loader yaml-cpp)
+  ament_target_dependencies(test_nav2_profile_loader
+    rclcpp
+  )
+
+  ament_add_gtest(test_trajectory_guard test/test_trajectory_guard.cpp)
+  target_include_directories(test_trajectory_guard PRIVATE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  )
+  target_link_libraries(test_trajectory_guard trajectory_guard_lib)
+  ament_target_dependencies(test_trajectory_guard
+    geometry_msgs
+    nav_msgs
+  )
 endif()

 install(DIRECTORY config launch behavior_tree
--- a/src/navigation/obstacle_nav2/behavior_tree/nav_through_poses_ackermann.xml
+++ b/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>
--- a/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml
+++ b/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">
-      <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">
+    <Sequence name="NavigateWithSingleBackupReplan">
+      <RecoveryNode number_of_retries="1" name="ComputeInitialPath">
+        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+        <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+      </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>
-      </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>
+
+        <Sequence name="BackupReplanAndFinalFollow">
+          <BackUp backup_dist="0.04" backup_speed="0.20"/>
+          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+        </Sequence>
+      </Fallback>
+    </Sequence>
  </BehaviorTree>
 </root>
--- a/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_event_replan
+++ b/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>
--- a/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_single_backup
+++ b/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>
--- a/src/navigation/obstacle_nav2/config/fastdds_udp_only.xml
+++ b/src/navigation/obstacle_nav2/config/fastdds_udp_only.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
+  <transport_descriptors>
+    <transport_descriptor>
+      <transport_id>udp_transport</transport_id>
+      <type>UDPv4</type>
+    </transport_descriptor>
+  </transport_descriptors>
+  <participant profile_name="udp_only_participant" is_default_profile="true">
+    <rtps>
+      <useBuiltinTransports>false</useBuiltinTransports>
+      <userTransports>
+        <transport_id>udp_transport</transport_id>
+      </userTransports>
+    </rtps>
+  </participant>
+</profiles>
--- a/src/navigation/obstacle_nav2/config/json/test1.json
+++ b/src/navigation/obstacle_nav2/config/json/test1.json
@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-07-24T12:43:47.020Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_001",
+      "captured_at": "2026-07-24T12:43:16.569Z",
+      "received_at": null,
+      "yaw_degrees": 4.398705354995568,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 1.4830508474576272,
+              "y": 0.18107786016949148,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.0383765195035876,
+              "w": 0.99926335004882
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 4.398705354995568
+      }
+    },
+    {
+      "name": "goal_002",
+      "captured_at": "2026-07-24T12:43:25.804Z",
+      "received_at": null,
+      "yaw_degrees": 15.109575122340486,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 3.2468220338983054,
+              "y": 0.2393405720338984,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.13147417510981546,
+              "w": 0.9913195959322068
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 15.109575122340486
+      }
+    },
+    {
+      "name": "goal_003",
+      "captured_at": "2026-07-24T12:43:44.649Z",
+      "received_at": null,
+      "yaw_degrees": 64.70797884297878,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.470338983050848,
+              "y": 1.3357388771186443,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.5351485964909909,
+              "w": 0.844757941468278
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 64.70797884297878
+      }
+    }
+  ]
+}
--- a/src/navigation/obstacle_nav2/config/json/test2.json
+++ b/src/navigation/obstacle_nav2/config/json/test2.json
@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-07-24T12:44:38.669Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_004",
+      "captured_at": "2026-07-24T12:44:18.861Z",
+      "received_at": null,
+      "yaw_degrees": 12.938056317186437,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 1.6472457627118644,
+              "y": 0.5094676906779659,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.11266611144839717,
+              "w": 0.9936329037079525
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 12.938056317186437
+      }
+    },
+    {
+      "name": "goal_005",
+      "captured_at": "2026-07-24T12:44:28.102Z",
+      "received_at": null,
+      "yaw_degrees": 22.02305549681122,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 3.1673728813559325,
+              "y": 0.5677304025423728,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.1910064921196254,
+              "w": 0.981588773350712
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 22.02305549681122
+      }
+    },
+    {
+      "name": "goal_006",
+      "captured_at": "2026-07-24T12:44:37.758Z",
+      "received_at": null,
+      "yaw_degrees": 55.12467165539786,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.4173728813559325,
+              "y": 1.394001588983051,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.4627133768932059,
+              "w": 0.8865079417828619
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 55.12467165539786
+      }
+    }
+  ]
+}
--- a/src/navigation/obstacle_nav2/config/json/test3.json
+++ b/src/navigation/obstacle_nav2/config/json/test3.json
@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-07-24T12:45:16.793Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_007",
+      "captured_at": "2026-07-24T12:44:56.224Z",
+      "received_at": null,
+      "yaw_degrees": 10.701350723899163,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 1.461864406779661,
+              "y": 0.5783236228813559,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.09325122181983936,
+              "w": 0.9956426113968341
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 10.701350723899163
+      }
+    },
+    {
+      "name": "goal_008",
+      "captured_at": "2026-07-24T12:45:04.283Z",
+      "received_at": null,
+      "yaw_degrees": 20.94747058695114,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 3.0826271186440675,
+              "y": 0.7319253177966101,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.18178477679916172,
+              "w": 0.9833383420390354
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 20.94747058695114
+      }
+    },
+    {
+      "name": "goal_009",
+      "captured_at": "2026-07-24T12:45:15.848Z",
+      "received_at": null,
+      "yaw_degrees": 85.03025927188973,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.735169491525424,
+              "y": 1.3092558262711866,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6757848709593751,
+              "w": 0.7370989134318546
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 85.03025927188973
+      }
+    }
+  ]
+}
--- a/src/navigation/obstacle_nav2/config/json/test4.json
+++ b/src/navigation/obstacle_nav2/config/json/test4.json
@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-07-24T12:47:26.413Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_015",
+      "captured_at": "2026-07-24T12:47:14.682Z",
+      "received_at": null,
+      "yaw_degrees": 17.709724523862732,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 1.5625,
+              "y": 0.7001456567796612,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.15393202146824025,
+              "w": 0.9880814403513009
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 17.709724523862732
+      }
+    },
+    {
+      "name": "goal_016",
+      "captured_at": "2026-07-24T12:47:18.969Z",
+      "received_at": null,
+      "yaw_degrees": 22.036226940145426,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.9978813559322033,
+              "y": 0.9490863347457628,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.1911193171514086,
+              "w": 0.9815668120976683
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 22.036226940145426
+      }
+    },
+    {
+      "name": "goal_017",
+      "captured_at": "2026-07-24T12:47:24.482Z",
+      "received_at": null,
+      "yaw_degrees": 75.9637565320735,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.523305084745763,
+              "y": 1.3304422669491527,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6154122094026355,
+              "w": 0.7882054380161093
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 75.9637565320735
+      }
+    }
+  ]
+}
--- a/src/navigation/obstacle_nav2/config/json/test5.json
+++ b/src/navigation/obstacle_nav2/config/json/test5.json
@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-07-24T12:48:05.057Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_019",
+      "captured_at": "2026-07-24T12:47:52.274Z",
+      "received_at": null,
+      "yaw_degrees": 9.130176482278682,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 1.4088983050847457,
+              "y": 0.1387049788135596,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.0795915470489837,
+              "w": 0.9968275606334074
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 9.130176482278682
+      }
+    },
+    {
+      "name": "goal_020",
+      "captured_at": "2026-07-24T12:47:56.654Z",
+      "received_at": null,
+      "yaw_degrees": 15.2551187030578,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 3.0614406779661016,
+              "y": 0.18637447033898297,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.1327331510254088,
+              "w": 0.9911518100769761
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 15.2551187030578
+      }
+    },
+    {
+      "name": "goal_021",
+      "captured_at": "2026-07-24T12:48:03.562Z",
+      "received_at": null,
+      "yaw_degrees": 89.99999999999986,
+      "odom": {
+        "header": {
+          "frame_id": "map",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.703389830508474,
+              "y": 1.3516287076271185,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.7071067811865467,
+              "w": 0.7071067811865485
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 89.99999999999986
+      }
+    }
+  ]
+}
--- a/src/navigation/obstacle_nav2/config/nav2_params.yaml
+++ b/src/navigation/obstacle_nav2/config/nav2_params.yaml
@@ -112,8 +112,8 @@ controller_server:
      CostCritic:
        enabled: true
        cost_power: 1
-        cost_weight: 4.5
-        critical_cost: 300.0
+        cost_weight: 6.0
+        critical_cost: 250.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 0.8
@@ -134,7 +134,7 @@ controller_server:
      PathAlignCritic:
        enabled: true
        cost_power: 1
-        cost_weight: 16.0
+        cost_weight: 8.0
        max_path_occupancy_ratio: 0.07
        trajectory_point_step: 4
        threshold_to_consider: 0.45
@@ -144,14 +144,14 @@ controller_server:
      PathFollowCritic:
        enabled: true
        cost_power: 1
-        cost_weight: 6.0
+        cost_weight: 4.0
        offset_from_furthest: 4
        threshold_to_consider: 1.0

      PathAngleCritic:
        enabled: true
        cost_power: 1
-        cost_weight: 6.0
+        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 0.45
        max_angle_to_furthest: 1.2
@@ -160,7 +160,7 @@ controller_server:
      PreferForwardCritic:
        enabled: true
        cost_power: 1
-        cost_weight: 2.0
+        cost_weight: 1.0
        threshold_to_consider: 0.4

 controller_server_rclcpp_node:
@@ -170,7 +170,7 @@ controller_server_rclcpp_node:
 local_costmap:
  local_costmap:
    ros__parameters:
-      update_frequency: 8.0
+      update_frequency: 10.0
      publish_frequency: 4.0
      transform_tolerance: 0.5
      global_frame: odom
@@ -188,12 +188,13 @@ local_costmap:
        plugin: "obstacle_nav2::ObstacleArrayLayer"
        enabled: true
        topic: /obstacles
-        obstacle_timeout: 0.5
-        transform_tolerance: 0.2
+        obstacle_timeout: 1.0
+        transform_tolerance: 0.02
        default_obstacle_radius: 0.05
        minimum_obstacle_radius: 0.02
        maximum_obstacle_radius: 0.06
        extra_inflation: 0.02
+        retain_previous_on_empty_snapshot: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
@@ -209,8 +210,8 @@ local_costmap:
 global_costmap:
  global_costmap:
    ros__parameters:
-      update_frequency: 2.0
-      publish_frequency: 1.0
+      update_frequency: 5.0
+      publish_frequency: 3.0
      transform_tolerance: 0.5
      global_frame: odom
      robot_base_frame: base_footprint
@@ -227,12 +228,13 @@ global_costmap:
        plugin: "obstacle_nav2::ObstacleArrayLayer"
        enabled: true
        topic: /obstacles
-        obstacle_timeout: 0.5
-        transform_tolerance: 0.2
+        obstacle_timeout: 1.0
+        transform_tolerance: 0.02
        default_obstacle_radius: 0.05
        minimum_obstacle_radius: 0.02
        maximum_obstacle_radius: 0.50
        extra_inflation: 0.02
+        retain_previous_on_empty_snapshot: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
@@ -264,10 +266,10 @@ planner_server:
      analytic_expansion_max_length: 2.0
      minimum_turning_radius: 0.60
      reverse_penalty: 2.2
-      change_penalty: 0.5
-      non_straight_penalty: 0.85
-      cost_penalty: 4.5
-      retrospective_penalty: 0.015
+      change_penalty: 0.0
+      non_straight_penalty: 0.8
+      cost_penalty: 5.0
+      retrospective_penalty: 0.01
      lookup_table_size: 5.0
      cache_obstacle_heuristic: false
      viz_expansions: false
--- a/src/navigation/obstacle_nav2/config/nav2_profile_10
+++ b/src/navigation/obstacle_nav2/config/nav2_profile_10
@@ -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
--- a/src/navigation/obstacle_nav2/config/nav2_profile_10
+++ b/src/navigation/obstacle_nav2/config/nav2_profile_10
@@ -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
--- a/src/navigation/obstacle_nav2/config/nav2_profile_10
+++ b/src/navigation/obstacle_nav2/config/nav2_profile_10
@@ -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: 15.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 40
+      model_dt: 0.06666666666666666
+      batch_size: 900
+      vx_std: 0.24
+      vy_std: 0.0
+      wz_std: 0.48
+      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.6
+      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: 11.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+        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: 11.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.20
+      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.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: 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.45
+      reverse_penalty: 4.0
+      change_penalty: 1.2
+      non_straight_penalty: 0.7
+      cost_penalty: 3.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: 700
+        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","wait","backup"]
+    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
--- a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml
+++ b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml
@@ -0,0 +1,345 @@
+# ============================================================================
+# 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: 10.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 30
+      model_dt: 0.1
+      batch_size: 700
+      vx_std: 0.50
+      vy_std: 0.0
+      wz_std: 0.80
+      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: 1.8
+        critical_cost: 250.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: 4.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: 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.25
+      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.3
+      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: 1.0
+      non_straight_penalty: 1.2
+      cost_penalty: 10.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: 10
+    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.25, 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
--- a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml.bak_20260807_global_inflation_055
+++ b/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
--- a/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml
+++ b/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml
@@ -0,0 +1,345 @@
+# ============================================================================
+# 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: 10.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 30
+      model_dt: 0.1
+      batch_size: 700
+      vx_std: 0.40
+      vy_std: 0.0
+      wz_std: 0.70
+      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: 1.8
+        critical_cost: 250.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: 4.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: 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.25
+      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.3
+      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: 1.0
+      non_straight_penalty: 1.2
+      cost_penalty: 6.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: 10
+    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.25, 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
--- a/src/navigation/obstacle_nav2/config/q_waypoint_selector.py
+++ b/src/navigation/obstacle_nav2/config/q_waypoint_selector.py
@@ -0,0 +1,595 @@
+#!/usr/bin/env python3
+"""Q_i candidate waypoint selector for Nav2 NavigateToPose.
+
+用途
+----
+这个节点用于比赛/实车场景下的多路线候选导航。它会从同一个 JSON
+目录中读取多个 ``*.json`` 文件。每个 JSON 文件表示一条完整路线，文件
+中的 ``points`` 数组表示该路线按顺序经过的导航点。
+
+多 JSON 编组规则
+--------------
+假设目录中存在多个文件：
+
+  test1.json: p_0, p_1, p_2, ...
+  test2.json: p_0, p_1, p_2, ...
+  test3.json: p_0, p_1, p_2, ...
+
+节点会把每个文件的第 i 个点组合成 Q_i：
+
+  Q_0 = [test1[0], test2[0], test3[0], ...]
+  Q_1 = [test1[1], test2[1], test3[1], ...]
+  Q_2 = [test1[2], test2[2], test3[2], ...]
+
+文件按自然顺序排序，因此 ``test2.json`` 会排在 ``test10.json`` 前面。
+如果某条路线点数较少，缺失的索引会被跳过，不影响其他路线候选。
+
+JSON 格式
+---------
+推荐格式为：
+
+  {
+    "points": [
+      {
+        "name": "goal_001",
+        "yaw_degrees": 4.39,
+        "odom": {
+          "pose": {
+            "pose": {
+              "position": {"x": 1.48, "y": 0.18, "z": 0.0},
+              "orientation": {"x": 0.0, "y": 0.0, "z": 0.03, "w": 0.99}
+            }
+          }
+        }
+      }
+    ]
+  }
+
+同时也支持顶层直接是 list 的 JSON。坐标 ``x/y`` 必须与 ``goal_frame``
+一致，例如 ``odom`` 或 ``odom_combined``。
+
+候选点选择规则
+--------------
+1. 第一次优先选择第 0 条路线，也就是自然排序后的第一个 JSON 文件。
+2. 到达某个 Q_i 后，进入 Q_{i+1} 时优先保持同一条路线。
+3. 如果该候选点在全局 costmap 中不可用，则按路线顺序循环尝试下一个候选。
+4. 如果某个 Q_i 中所有候选都不可用，则跳过该 Q，进入 Q_{i+1}。
+5. 如果 Nav2 action 被拒绝、失败、取消或超时，则当前候选在本轮 Q 中
+   被标记为失败，下一轮 tick 会尝试下一个候选。
+
+不可用判断
+---------
+发送目标前会查询 ``/global_costmap/costmap``：
+
+* 点在 costmap 外：不可用。
+* costmap 数据异常：不可用。
+* cost >= ``occupied_threshold``：不可用。
+* cost = -1：由 ``treat_unknown_as_occupied`` 决定。
+
+当前默认适配无静态地图场景：``treat_unknown_as_occupied=False``，因此未知
+区域不会直接导致候选点被拒绝；真正障碍仍由 cost 阈值过滤。
+
+关键参数
+--------
+* ``json_dir``: JSON 路线目录。
+* ``goal_frame``: 下发 NavigateToPose 目标使用的坐标系，默认 ``odom``。
+* ``costmap_topic``: 用于判断候选点占用状态的 costmap，默认
+  ``/global_costmap/costmap``。
+* ``navigate_action``: Nav2 action 名称，默认 ``/navigate_to_pose``。
+* ``occupied_threshold``: costmap 占用阈值，默认 50。
+* ``treat_unknown_as_occupied``: 是否把 cost=-1 视为不可通行，默认 False。
+* ``goal_timeout_sec``: 单个候选目标超时时间，默认 180 秒。
+* ``selection_period_sec``: 主循环周期，默认 0.5 秒。
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import re
+import sys
+import traceback
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Dict, List, Optional, Sequence, Set
+
+ROS_IMPORT_ERROR: Optional[BaseException] = None
+
+try:
+    import rclpy
+    from geometry_msgs.msg import PoseStamped
+    from nav2_msgs.action import NavigateToPose
+    from nav_msgs.msg import OccupancyGrid
+    from rclpy.action import ActionClient
+    from rclpy.duration import Duration
+    from rclpy.node import Node
+    from rclpy.qos import QoSProfile
+except BaseException as exc:  # pragma: no cover - used for remote runtime diagnosis
+    ROS_IMPORT_ERROR = exc
+    rclpy = None  # type: ignore[assignment]
+    PoseStamped = object  # type: ignore[assignment]
+    NavigateToPose = object  # type: ignore[assignment]
+    OccupancyGrid = object  # type: ignore[assignment]
+    ActionClient = object  # type: ignore[assignment]
+    Duration = object  # type: ignore[assignment]
+    Node = object  # type: ignore[assignment]
+    QoSProfile = object  # type: ignore[assignment]
+
+
+@dataclass(frozen=True)
+class CandidateWaypoint:
+    q_index: int
+    route_index: int
+    file_name: str
+    point_name: str
+    x: float
+    y: float
+    yaw: float
+
+
+def yaw_from_quaternion(q: Dict[str, float]) -> float:
+    x = float(q.get("x", 0.0))
+    y = float(q.get("y", 0.0))
+    z = float(q.get("z", 0.0))
+    w = float(q.get("w", 1.0))
+    return math.atan2(
+        2.0 * (w * z + x * y),
+        1.0 - 2.0 * (y * y + z * z),
+    )
+
+
+def pose_from_json_point(point: Dict, q_index: int, route_index: int, file_name: str) -> CandidateWaypoint:
+    odom = point.get("odom", {})
+    pose = odom.get("pose", {}).get("pose", {})
+    position = pose.get("position", {})
+    orientation = pose.get("orientation", {})
+
+    x = float(position["x"])
+    y = float(position["y"])
+    yaw_degrees = point.get("yaw_degrees", None)
+    if yaw_degrees is None:
+        yaw = yaw_from_quaternion(orientation)
+    else:
+        yaw = math.radians(float(yaw_degrees))
+
+    return CandidateWaypoint(
+        q_index=q_index,
+        route_index=route_index,
+        file_name=file_name,
+        point_name=str(point.get("name", f"{Path(file_name).stem}_{q_index}")),
+        x=x,
+        y=y,
+        yaw=yaw,
+    )
+
+
+def natural_json_sort_key(path: Path) -> List[object]:
+    return [
+        int(part) if part.isdigit() else part
+        for part in re.split(r"(\d+)", path.name.lower())
+    ]
+
+
+def load_points_from_json(path: Path) -> List[Dict]:
+    try:
+        data = json.loads(path.read_text(encoding="utf-8"))
+    except Exception as exc:
+        raise RuntimeError(f"failed to parse JSON file {path}: {exc}") from exc
+
+    if isinstance(data, list):
+        points = data
+    elif isinstance(data, dict):
+        points = data.get("points", [])
+    else:
+        raise RuntimeError(
+            f"{path} must contain either a top-level list or a dict field named 'points'"
+        )
+
+    if not isinstance(points, list):
+        raise RuntimeError(f"{path} does not contain a list field named 'points'")
+    return points
+
+
+def load_q_groups(json_dir: Path) -> tuple[List[List[CandidateWaypoint]], List[str], List[int]]:
+    if not json_dir.exists():
+        raise RuntimeError(f"JSON directory does not exist: {json_dir}")
+    if not json_dir.is_dir():
+        raise RuntimeError(f"JSON path is not a directory: {json_dir}")
+
+    files = sorted(json_dir.glob("*.json"), key=natural_json_sort_key)
+    if not files:
+        raise RuntimeError(f"no JSON files found in {json_dir}")
+
+    routes: List[List[Dict]] = []
+    for path in files:
+        routes.append(load_points_from_json(path))
+
+    max_points = max(len(route) for route in routes)
+    groups: List[List[CandidateWaypoint]] = []
+    for q_index in range(max_points):
+        group: List[CandidateWaypoint] = []
+        for route_index, points in enumerate(routes):
+            if q_index >= len(points):
+                continue
+            group.append(
+                pose_from_json_point(
+                    points[q_index],
+                    q_index=q_index,
+                    route_index=route_index,
+                    file_name=files[route_index].name,
+                )
+            )
+        groups.append(group)
+    return groups, [path.name for path in files], [len(route) for route in routes]
+
+
+class QWaypointSelector(Node):
+    def __init__(self) -> None:
+        super().__init__("q_waypoint_selector")
+
+        self._declare_parameter_if_needed("use_sim_time", False)
+        self._declare_parameter_if_needed("json_dir", "")
+        self._declare_parameter_if_needed("goal_frame", "odom")
+        self._declare_parameter_if_needed("costmap_topic", "/global_costmap/costmap")
+        self._declare_parameter_if_needed("navigate_action", "/navigate_to_pose")
+        self._declare_parameter_if_needed("occupied_threshold", 50)
+        self._declare_parameter_if_needed("treat_unknown_as_occupied", False)
+        self._declare_parameter_if_needed("goal_timeout_sec", 180.0)
+        self._declare_parameter_if_needed("selection_period_sec", 0.5)
+
+        json_dir = Path(str(self.get_parameter("json_dir").value)).expanduser()
+        if not json_dir.is_absolute():
+            json_dir = Path.cwd() / json_dir
+        self.goal_frame = str(self.get_parameter("goal_frame").value)
+        self.costmap_topic = str(self.get_parameter("costmap_topic").value)
+        self.navigate_action = str(self.get_parameter("navigate_action").value)
+        self.occupied_threshold = int(self.get_parameter("occupied_threshold").value)
+        self.treat_unknown_as_occupied = bool(
+            self.get_parameter("treat_unknown_as_occupied").value
+        )
+        self.goal_timeout_sec = max(
+            1.0, float(self.get_parameter("goal_timeout_sec").value)
+        )
+        self.selection_period_sec = max(
+            0.1, float(self.get_parameter("selection_period_sec").value)
+        )
+
+        self.q_groups, self.route_file_names, self.route_point_counts = load_q_groups(json_dir)
+        self.current_q_index = 0
+        self.preferred_route_index = 0
+        self.rejected_routes_for_current_q: Set[int] = set()
+        self.active_candidate: Optional[CandidateWaypoint] = None
+        self.active_goal_handle = None
+        self.active_goal_start_time = None
+        self.active_goal_token: Optional[int] = None
+        self.next_goal_token = 0
+        self.done = False
+        self.latest_costmap: Optional[OccupancyGrid] = None
+        self.last_costmap_wait_log_ns = 0
+
+        qos = QoSProfile(depth=1)
+        self.costmap_sub = self.create_subscription(
+            OccupancyGrid, self.costmap_topic, self._costmap_callback, qos
+        )
+        self.action_client = ActionClient(self, NavigateToPose, self.navigate_action)
+        self.timer = self.create_timer(self.selection_period_sec, self._tick)
+
+        self.get_logger().info(
+            "loaded Q waypoint groups "
+            f"json_dir={json_dir} groups={len(self.q_groups)} "
+            f"routes={len(self.route_file_names)} "
+            f"goal_frame={self.goal_frame} costmap_topic={self.costmap_topic}"
+        )
+        route_summary = ", ".join(
+            f"{name}:{count}"
+            for name, count in zip(self.route_file_names, self.route_point_counts)
+        )
+        self.get_logger().info(f"loaded JSON waypoint routes {route_summary}")
+
+    def _declare_parameter_if_needed(self, name: str, default_value) -> None:
+        if not self.has_parameter(name):
+            self.declare_parameter(name, default_value)
+
+    def _costmap_callback(self, msg: OccupancyGrid) -> None:
+        self.latest_costmap = msg
+
+    def _log_waiting_for_costmap(self) -> None:
+        now_ns = self.get_clock().now().nanoseconds
+        if now_ns - self.last_costmap_wait_log_ns < 2_000_000_000:
+            return
+        self.last_costmap_wait_log_ns = now_ns
+
+        publishers = self.get_publishers_info_by_topic(self.costmap_topic)
+        if not publishers:
+            self.get_logger().warn(
+                f"waiting for costmap {self.costmap_topic}; publisher_count=0; "
+                "no goal will be sent yet"
+            )
+            return
+
+        details = []
+        for info in publishers[:3]:
+            details.append(
+                f"{info.node_namespace}/{info.node_name} "
+                f"type={info.topic_type} qos={info.qos_profile}"
+            )
+        more = "" if len(publishers) <= 3 else f" ... +{len(publishers) - 3} more"
+        self.get_logger().warn(
+            f"waiting for costmap {self.costmap_topic}; "
+            f"publisher_count={len(publishers)} publishers={details}{more}; "
+            "no goal will be sent yet"
+        )
+
+    def _tick(self) -> None:
+        if self.done:
+            return
+
+        if self.current_q_index >= len(self.q_groups):
+            self.done = True
+            self.get_logger().info("all Q waypoint groups completed")
+            return
+
+        if not self.action_client.server_is_ready():
+            self.get_logger().info(
+                f"waiting for NavigateToPose action server {self.navigate_action}"
+            )
+            self.action_client.wait_for_server(timeout_sec=0.1)
+            return
+
+        if self.latest_costmap is None:
+            self._log_waiting_for_costmap()
+            return
+
+        if self.active_goal_handle is not None:
+            if self._active_goal_timed_out():
+                self.get_logger().warn(
+                    "active goal timed out; cancelling and trying next candidate "
+                    f"q={self.current_q_index} candidate={self._candidate_label(self.active_candidate)}"
+                )
+                cancel_future = self.active_goal_handle.cancel_goal_async()
+                cancel_future.add_done_callback(lambda _: None)
+                self._reject_active_candidate()
+            return
+
+        candidate = self._select_candidate()
+        if candidate is None:
+            self.get_logger().warn(
+                f"all candidates in Q_{self.current_q_index} are occupied/unusable; skipping group"
+            )
+            self.current_q_index += 1
+            self.rejected_routes_for_current_q.clear()
+            return
+
+        self._send_goal(candidate)
+
+    def _select_candidate(self) -> Optional[CandidateWaypoint]:
+        group = self.q_groups[self.current_q_index]
+        if not group:
+            return None
+
+        route_to_candidate = {candidate.route_index: candidate for candidate in group}
+        ordered_route_indices = self._cyclic_route_order(
+            sorted(route_to_candidate.keys()), self.preferred_route_index
+        )
+
+        for route_index in ordered_route_indices:
+            if route_index in self.rejected_routes_for_current_q:
+                continue
+            candidate = route_to_candidate[route_index]
+            occupied, reason = self._candidate_occupied(candidate)
+            if occupied:
+                self.rejected_routes_for_current_q.add(route_index)
+                self.get_logger().info(
+                    f"skip occupied candidate Q_{candidate.q_index} "
+                    f"{self._candidate_label(candidate)} reason={reason}"
+                )
+                continue
+            return candidate
+        return None
+
+    @staticmethod
+    def _cyclic_route_order(route_indices: Sequence[int], preferred: int) -> List[int]:
+        if not route_indices:
+            return []
+        if preferred not in route_indices:
+            preferred = route_indices[0]
+        start = route_indices.index(preferred)
+        return list(route_indices[start:]) + list(route_indices[:start])
+
+    def _candidate_occupied(self, candidate: CandidateWaypoint) -> tuple[bool, str]:
+        costmap = self.latest_costmap
+        if costmap is None:
+            return True, "no_costmap"
+
+        origin = costmap.info.origin
+        resolution = costmap.info.resolution
+        width = int(costmap.info.width)
+        height = int(costmap.info.height)
+        if resolution <= 0.0 or width <= 0 or height <= 0:
+            return True, "invalid_costmap"
+
+        origin_yaw = yaw_from_quaternion(
+            {
+                "x": origin.orientation.x,
+                "y": origin.orientation.y,
+                "z": origin.orientation.z,
+                "w": origin.orientation.w,
+            }
+        )
+        dx = candidate.x - origin.position.x
+        dy = candidate.y - origin.position.y
+        cos_yaw = math.cos(-origin_yaw)
+        sin_yaw = math.sin(-origin_yaw)
+        local_x = cos_yaw * dx - sin_yaw * dy
+        local_y = sin_yaw * dx + cos_yaw * dy
+        mx = int(math.floor(local_x / resolution))
+        my = int(math.floor(local_y / resolution))
+
+        if mx < 0 or my < 0 or mx >= width or my >= height:
+            return True, f"outside_costmap cell=({mx},{my})"
+
+        index = my * width + mx
+        if index < 0 or index >= len(costmap.data):
+            return True, "costmap_data_too_short"
+
+        cost = int(costmap.data[index])
+        if cost < 0:
+            return self.treat_unknown_as_occupied, f"unknown cost={cost}"
+        if cost >= self.occupied_threshold:
+            return True, f"occupied cost={cost}"
+        return False, f"free cost={cost}"
+
+    def _send_goal(self, candidate: CandidateWaypoint) -> None:
+        goal_msg = NavigateToPose.Goal()
+        goal_msg.pose = self._to_pose_stamped(candidate)
+        self.next_goal_token += 1
+        token = self.next_goal_token
+        self.active_candidate = candidate
+        self.active_goal_start_time = self.get_clock().now()
+        self.active_goal_token = token
+
+        self.get_logger().info(
+            f"send Q_{candidate.q_index} candidate {self._candidate_label(candidate)} "
+            f"pose=({candidate.x:.3f},{candidate.y:.3f},{candidate.yaw:.3f})"
+        )
+        send_future = self.action_client.send_goal_async(goal_msg)
+        send_future.add_done_callback(
+            lambda future, goal_token=token: self._goal_response_callback(future, goal_token)
+        )
+
+    def _to_pose_stamped(self, candidate: CandidateWaypoint) -> PoseStamped:
+        pose = PoseStamped()
+        pose.header.frame_id = self.goal_frame
+        pose.header.stamp = self.get_clock().now().to_msg()
+        pose.pose.position.x = candidate.x
+        pose.pose.position.y = candidate.y
+        pose.pose.position.z = 0.0
+        pose.pose.orientation.z = math.sin(candidate.yaw * 0.5)
+        pose.pose.orientation.w = math.cos(candidate.yaw * 0.5)
+        return pose
+
+    def _goal_response_callback(self, future, goal_token: int) -> None:
+        if goal_token != self.active_goal_token:
+            return
+        try:
+            goal_handle = future.result()
+        except Exception as exc:
+            self.get_logger().error(
+                "NavigateToPose send_goal_async failed; try next candidate: "
+                f"{type(exc).__name__}: {exc}"
+            )
+            self._reject_active_candidate()
+            return
+        if not goal_handle.accepted:
+            self.get_logger().warn(
+                f"goal rejected by Nav2 {self._candidate_label(self.active_candidate)}"
+            )
+            self._reject_active_candidate()
+            return
+
+        self.active_goal_handle = goal_handle
+        result_future = goal_handle.get_result_async()
+        result_future.add_done_callback(
+            lambda future, token=goal_token: self._goal_result_callback(future, token)
+        )
+
+    def _goal_result_callback(self, future, goal_token: int) -> None:
+        if goal_token != self.active_goal_token:
+            return
+        try:
+            result = future.result()
+        except Exception as exc:
+            self.get_logger().error(
+                "NavigateToPose result failed; try next candidate: "
+                f"{type(exc).__name__}: {exc}"
+            )
+            self._reject_active_candidate()
+            return
+        candidate = self.active_candidate
+        status = int(result.status)
+
+        if status == 4:
+            if candidate is not None:
+                self.preferred_route_index = candidate.route_index
+                self.get_logger().info(
+                    f"reached Q_{candidate.q_index} {self._candidate_label(candidate)}; "
+                    f"advance to Q_{candidate.q_index + 1}"
+                )
+            self.current_q_index += 1
+            self.rejected_routes_for_current_q.clear()
+            self.active_candidate = None
+            self.active_goal_handle = None
+            self.active_goal_start_time = None
+            self.active_goal_token = None
+            return
+
+        self.get_logger().warn(
+            f"goal failed status={status}; try next candidate "
+            f"{self._candidate_label(candidate)}"
+        )
+        self._reject_active_candidate()
+
+    def _active_goal_timed_out(self) -> bool:
+        if self.active_goal_start_time is None:
+            return False
+        elapsed = self.get_clock().now() - self.active_goal_start_time
+        return elapsed > Duration(seconds=self.goal_timeout_sec)
+
+    def _reject_active_candidate(self) -> None:
+        if self.active_candidate is not None:
+            self.rejected_routes_for_current_q.add(self.active_candidate.route_index)
+        self.active_candidate = None
+        self.active_goal_handle = None
+        self.active_goal_start_time = None
+        self.active_goal_token = None
+
+    @staticmethod
+    def _candidate_label(candidate: Optional[CandidateWaypoint]) -> str:
+        if candidate is None:
+            return "none"
+        return (
+            f"route={candidate.route_index} file={candidate.file_name} "
+            f"name={candidate.point_name}"
+        )
+
+
+def main() -> int:
+    if ROS_IMPORT_ERROR is not None:
+        print(
+            "FATAL q_waypoint_selector: failed to import ROS/Nav2 Python modules.",
+            file=sys.stderr,
+        )
+        traceback.print_exception(
+            type(ROS_IMPORT_ERROR),
+            ROS_IMPORT_ERROR,
+            ROS_IMPORT_ERROR.__traceback__,
+            file=sys.stderr,
+        )
+        print(
+            "Hint: source /opt/ros/humble/setup.bash and install/source Nav2 "
+            "packages, especially nav2_msgs.",
+            file=sys.stderr,
+        )
+        return 1
+
+    rclpy.init()
+    node = None
+    try:
+        node = QWaypointSelector()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    except BaseException:
+        print("FATAL q_waypoint_selector: unhandled exception", file=sys.stderr)
+        traceback.print_exc(file=sys.stderr)
+        return 1
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
--- a/src/navigation/obstacle_nav2/config/q_waypoint_selector_manual.md
+++ b/src/navigation/obstacle_nav2/config/q_waypoint_selector_manual.md
@@ -0,0 +1,270 @@
+# Q 路点选择器 使用手册
+
+## 1. 概述
+
+`q_waypoint_selector.py` 是一个独立的 ROS 2 Python 节点，用于比赛场景下的**自动有序路点导航**。它从 JSON 文件中加载多路线、多候选的路点坐标，结合全局 costmap 实时判断候选点是否可通行，并通过 Nav2 的 `NavigateToPose` action 下发导航目标，依次逐个完成所有路点。
+
+### 核心能力
+
+- **多路线冗余**：同一个路点序号可配置多个候选坐标（来自不同 JSON 文件），一个被障碍物占据时自动切换
+- **costmap 感知**：下发目标前先检查该坐标在全局 costmap 上是否被占用，避免朝障碍物导航
+- **路线偏好记忆**：成功后记住当前路线编号，后续 Q 优先选择同一条路线
+- **超时自动切换**：单个目标超时后自动取消，尝试下一个候选
+
+---
+
+## 2. 路点 JSON 文件格式
+
+### 目录约定
+
+所有 JSON 文件放在同一目录下，文件名按字母序排列。节点加载时：
+
+```
+json_dir/
+├── test1.json   # 路线 0
+├── test2.json   # 路线 1
+├── test3.json   # 路线 2
+└── ...
+```
+
+每个 JSON 文件代表一条**完整路线**，包含有序的路点列表。
+
+### JSON 结构
+
+```json
+{
+  "points": [
+    {
+      "name": "goal_001",
+      "yaw_degrees": 4.39,
+      "odom": {
+        "pose": {
+          "pose": {
+            "position": { "x": 1.48, "y": 0.18, "z": 0.0 },
+            "orientation": { "x": 0.0, "y": 0.0, "z": 0.03, "w": 0.99 }
+          }
+        }
+      }
+    },
+    {
+      "name": "goal_002",
+      "yaw_degrees": 15.10,
+      "odom": {
+        "pose": {
+          "pose": {
+            "position": { "x": 3.24, "y": 0.23, "z": 0.0 },
+            "orientation": { "x": 0.0, "y": 0.0, "z": 0.13, "w": 0.99 }
+          }
+        }
+      }
+    }
+  ]
+}
+```
+
+### 字段说明
+
+| 字段 | 类型 | 必需 | 说明 |
+|------|------|------|------|
+| `points` | array | 是 | 有序路点数组，按索引 0, 1, 2... 依次导航 |
+| `points[i].name` | string | 否 | 路点名称（日志显示用，缺省为 `{文件名}_{索引}`） |
+| `points[i].yaw_degrees` | number | 否 | 目标朝向角（度），优先级高于 orientation 四元数 |
+| `points[i].odom.pose.pose.position` | object | 是 | 路点坐标 `{x, y, z}` |
+| `points[i].odom.pose.pose.orientation` | object | 否 | 目标朝向四元数 `{x, y, z, w}`，仅在 `yaw_degrees` 缺失时使用 |
+
+> **注意**：`position.x` 和 `position.y` 必须与 `goal_frame` 参数指定的坐标系一致（如 `odom` 或 `odom_combined`）。
+
+---
+
+## 3. Q 路点组概念
+
+### 路点编组规则
+
+假设有 2 个 JSON 文件，经过编组后：
+
+```
+test1.json: [p0, p1, p2]        (路线 0)
+test2.json: [p0, p1, p2]        (路线 1)
+           ↓ 转置编组
+Q_0 = [test1[0], test2[0]]     ← 两个候选路点，指向同一目标位置的不同路线
+Q_1 = [test1[1], test2[1]]
+Q_2 = [test1[2], test2[2]]
+```
+
+**路径数不同时的处理**：编组时以最长路线为准，较短路线在对应索引上缺失的点被跳过，不会影响其他路线的候选。
+
+### 候选选择策略
+
+节点在每个 Q 组内按以下优先级选择候选：
+
+1. **偏好路线优先**（`preferred_route_index`）：上次导航成功的路线编号
+2. **循环顺序**：若偏好路线不可用，按路线编号循环尝试
+3. **costmap 过滤**：被 costmap 判定为占用的候选直接跳过
+4. **已拒绝列表**：本次 Q 内已失败/被拒绝的候选不再重试
+
+---
+
+## 4. 运行流程
+
+```
+启动节点
+  │
+  ├─ 加载 json_dir 下所有 .json → 编组为 Q_0, Q_1, ...
+  ├─ 等待 NavigateToPose action server 就绪
+  ├─ 等待 costmap 话题有数据
+  │
+  └─ 定时器循环 (selection_period_sec)
+       │
+       ├─ 所有 Q 完成? → 退出
+       │
+       ├─ 有活跃 goal ?
+       │   ├─ 进行中 → 等待
+       │   └─ 超时 → 取消 goal，拒绝当前候选，下一轮切换到下一个候选
+       │
+       ├─ 从 Q_current 选择候选
+       │   ├─ 有可用候选 → 通过 NavigateToPose 下发
+       │   └─ 全部被占用 → 跳过当前 Q，前进到 Q_{current+1}
+       │
+       └─ goal 结果回调
+           ├─ status=4 (SUCCEEDED) → 记录路线偏好，前进到下一 Q
+           └─ 其他 (失败/拒绝)  → 拒绝当前候选，下次 tick 换候选
+```
+
+---
+
+## 5. 参数说明
+
+### 启动参数
+
+| 参数 | 类型 | 默认值 | 说明 |
+|------|------|--------|------|
+| `json_dir` | string | `obstacle_nav2/config/json` | JSON 路点文件目录 |
+| `goal_frame` | string | `odom` | 下发导航目标使用的坐标帧，实车应设为 `odom_combined` |
+| `costmap_topic` | string | `/global_costmap/costmap` | 全局 costmap 话题，用于避占检测 |
+| `navigate_action` | string | `/navigate_to_pose` | Nav2 的 NavigateToPose action 名称 |
+| `occupied_threshold` | int | `50` | costmap 占用阈值 (0-100)，栅格值 >= 此值视为被占据 |
+| `treat_unknown_as_occupied` | bool | `true` | 是否将未知区域 (costmap 值 = -1) 视为不可通行 |
+| `goal_timeout_sec` | float | `180.0` | 单个导航目标超时时间（秒），取值范围 [1.0, ∞) |
+| `selection_period_sec` | float | `0.5` | 选择器主循环周期（秒），取值范围 [0.1, ∞) |
+| `use_sim_time` | bool | `false` | 是否使用仿真时间 |
+
+### 参数调优建议
+
+| 场景 | 建议设置 |
+|------|---------|
+| 路点间距大（>10m） | `goal_timeout_sec` 适当增大（如 300s） |
+| 障碍物密集区域 | `occupied_threshold` 降低（如 30），更保守地避让 |
+| 动态障碍物多 | `selection_period_sec` 设短（如 0.2s），更快响应 costmap 变化 |
+| costmap 覆盖不全 | `treat_unknown_as_occupied` 视情况关闭，允许向未知区域导航 |
+
+---
+
+## 6. 启动方式
+
+### 方式 A：独立启动（我使用的是这个，测试成功
+
+```bash
+python3 q_waypoint_selector.py \
+    --ros-args \
+    -p json_dir:=/home/user/waypoints \
+    -p goal_frame:=odom_combined \
+    -p costmap_topic:=/global_costmap/costmap \
+    -p goal_timeout_sec:=120.0
+```
+
+### 方式 B：通过 launch 文件启动
+
+```bash
+ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \
+    use_sim_time:=false \
+    global_frame:=odom_combined \
+    json_dir:=/home/user/waypoints \
+    goal_frame:=odom_combined \
+    goal_timeout_sec:=120.0 \
+    occupied_threshold:=50
+```
+
+这个 launch 文件会**同时启动**：底盘驱动、激光雷达、obstacle_scanner、Nav2 导航栈 和 Q 路点选择器。
+
+### 方式 C：在已有导航栈上单独启动
+
+```bash
+ros2 run obstacle_nav2 q_waypoint_selector.py \
+    --ros-args \
+    -p json_dir:=/home/user/waypoints \
+    -p goal_frame:=odom_combined
+```
+
+---
+
+## 7. 运行日志解读
+
+### 正常日志
+
+```
+[INFO] loaded Q waypoint groups json_dir=/... groups=5 routes=3 goal_frame=odom
+```
+启动成功，加载了 5 个 Q 组，每条 Q 最多 3 个候选路线。
+
+```
+[INFO] send Q_0 candidate route=0 file=test1.json name=goal_001 pose=(1.480,0.180,0.076)
+```
+下发了 Q_0 的第一个候选路点。
+
+```
+[INFO] reached Q_0 route=0 file=test1.json name=goal_001; advance to Q_1
+```
+到达目标，记录路线偏好并进入下一路点。
+
+### 异常日志及处理
+
+| 日志 | 含义 | 处理方式 |
+|------|------|---------|
+| `waiting for NavigateToPose action server` | Nav2 未启动或未就绪 | 等待 nav2_bringup 完成启动 |
+| `waiting for costmap; publisher_count=0` | costmap 话题无发布者 | 检查 global_costmap 节点是否启动 |
+| `skip occupied candidate ... reason=occupied cost=100` | 候选路点栅格被障碍物占据 | 自动尝试下一个候选路线 |
+| `skip occupied candidate ... reason=outside_costmap` | 候选路点超出当前 costmap 范围 | 增大 costmap 尺寸或调整路点坐标 |
+| `active goal timed out; cancelling` | 导航目标超时 | 自动取消并切换候选，可能需要增大 `goal_timeout_sec` |
+| `all candidates in Q_N are occupied; skipping` | 该 Q 所有候选均不可用 | 自动跳过到下一个 Q |
+| `all Q waypoint groups completed` | 所有路点已完成 | 正常退出信号 |
+
+---
+
+## 8. Nav2 goal 状态码参考
+
+节点根据 `NavigateToPose` action 的结果状态码决定下一步行为：
+
+| status | 含义 | 节点行为 |
+|--------|------|---------|
+| 0 | UNKNOWN | 拒绝当前候选，换下一个 |
+| 1 | ACCEPTED | (中间状态，不触发回调) |
+| 2 | EXECUTING | (中间状态，不触发回调) |
+| 3 | CANCELING | (中间状态，不触发回调) |
+| 4 | **SUCCEEDED** | 记录路线偏好，前进到下一 Q |
+| 5 | CANCELED | 拒绝当前候选，换下一个 |
+| 6 | ABORTED | 拒绝当前候选，换下一个 |
+
+---
+
+## 9. 常见问题
+
+**Q: 节点启动后一直等待 costmap，不发送目标？**
+
+检查 `costmap_topic` 参数是否与实际话题一致（默认 `/global_costmap/costmap`）：
+```bash
+ros2 topic list | grep costmap
+```
+
+**Q: 所有候选路点都被判定为占用？**
+
+1. 确认路点坐标与 `goal_frame` 坐标系一致
+2. 检查 `occupied_threshold` 是否过低（costmap 中膨胀区域可能为 50-100）
+3. 尝试设置 `treat_unknown_as_occupied:=false` 如果 costmap 覆盖不全
+
+**Q: 机器人到达路点后没有立即进入下一个？**
+
+节点的定时器周期为 `selection_period_sec`（默认 0.5s），最坏情况下需要等待一个周期。如果 goal 结果回调已触发，下一个 tick 就会下发新目标。
+
+**Q: 如何录制路点 JSON 文件？**
+
+路点 JSON 可以通过监听 `/odom_combined` (实车) 或 `/odom` (仿真) 话题并手动记录坐标生成。JSON 格式参见第 2 节。
--- a/src/navigation/obstacle_nav2/config/trajectory_guard.yaml
+++ b/src/navigation/obstacle_nav2/config/trajectory_guard.yaml
@@ -0,0 +1,27 @@
+trajectory_guard_node:
+  ros__parameters:
+    input_path_topic: /trajectory_guard/input_path
+    patched_path_topic: /trajectory_guard/patched_path
+    costmap_topic: /local_costmap/costmap
+    odom_topic: /odom_combined
+    planner_action: /compute_path_to_pose
+    follow_action: /follow_path
+    planner_id: GridBased
+    controller_id: FollowPath
+    goal_checker_id: ""
+    check_period_sec: 0.5
+    lookahead_distance: 2.0
+    rejoin_min_distance: 1.5
+    rejoin_max_distance: 5.0
+    occupied_threshold: 50
+    treat_unknown_as_occupied: true
+    footprint_half_length: 0.14
+    footprint_half_width: 0.085
+    footprint_padding: 0.04
+    footprint_sample_step: 0.05
+    repair_cooldown_sec: 0.5
+    blocked_retry_wait_sec: 0.8
+    wait_for_planner_costmap_update: true
+    min_repair_progress_indices: 5
+    publish_zero_on_blocked: true
+    cmd_vel_topic: /cmd_vel
--- a/src/navigation/obstacle_nav2/include/obstacle_nav2/nav2_profile_loader.hpp
+++ b/src/navigation/obstacle_nav2/include/obstacle_nav2/nav2_profile_loader.hpp
@@ -0,0 +1,31 @@
+#ifndef OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_
+#define OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_
+
+#include <string>
+#include <set>
+#include <vector>
+
+#include "rclcpp/parameter.hpp"
+
+namespace obstacle_nav2
+{
+
+struct NodeParameterSet
+{
+  std::string node_name;
+  std::vector<rclcpp::Parameter> parameters;
+};
+
+struct Nav2Profile
+{
+  std::vector<NodeParameterSet> nodes;
+};
+
+Nav2Profile loadNav2Profile(const std::string & path);
+std::vector<rclcpp::Parameter> filterDeclaredParameters(
+  const std::vector<rclcpp::Parameter> & parameters,
+  const std::set<std::string> & declared_names);
+
+}  // namespace obstacle_nav2
+
+#endif  // OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_
--- a/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp
+++ b/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp
@@ -5,6 +5,7 @@
 #include <cstdint>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <thread>
 #include <vector>
@@ -67,6 +68,10 @@ public:
    nav2_costmap_2d::Costmap2D & grid,
    const std::vector<CircleObstacle> & obstacles,
    double resolution, double origin_x, double origin_y);
+  static std::optional<SnapshotBounds> applySnapshotIfNotEmpty(
+    nav2_costmap_2d::Costmap2D & grid,
+    const std::vector<CircleObstacle> & obstacles,
+    double resolution, double origin_x, double origin_y);

 private:
  void obstacleCallback(const obstacle_scanner::msg::ObstacleArray::SharedPtr msg);
@@ -95,6 +100,7 @@ private:
  std::string global_frame_;
  double obstacle_timeout_{0.5};
  double transform_tolerance_{0.2};
+  bool retain_previous_on_empty_snapshot_{true};
  double default_obstacle_radius_{0.05};
  double minimum_obstacle_radius_{0.02};
  double maximum_obstacle_radius_{0.50};
--- a/src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp
+++ b/src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp
@@ -0,0 +1,89 @@
+#ifndef OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
+#define OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
+
+#include <cstddef>
+#include <optional>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+namespace obstacle_nav2
+{
+
+struct CollisionCheckResult
+{
+  bool blocked{false};
+  std::size_t path_index{0};
+  std::string reason{"clear"};
+};
+
+struct GuardSettings
+{
+  double lookahead_distance{2.0};
+  double rejoin_min_distance{1.0};
+  double rejoin_max_distance{4.0};
+  int occupied_threshold{50};
+  bool treat_unknown_as_occupied{true};
+  double footprint_half_length{0.14};
+  double footprint_half_width{0.085};
+  double footprint_padding{0.04};
+  double footprint_sample_step{0.05};
+};
+
+double yawFromPose(const geometry_msgs::msg::PoseStamped & pose);
+
+double distance2d(
+  const geometry_msgs::msg::PoseStamped & a,
+  const geometry_msgs::msg::PoseStamped & b);
+
+std::size_t nearestPathIndex(
+  const nav_msgs::msg::Path & path,
+  const geometry_msgs::msg::PoseStamped & pose);
+
+std::size_t advanceByDistance(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  double distance_m);
+
+CollisionCheckResult checkPathAhead(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const GuardSettings & settings);
+
+std::optional<std::size_t> findClearRejoinIndex(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const GuardSettings & settings);
+
+std::optional<std::size_t> findClearRejoinIndexWithPlannerCostmap(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & local_costmap,
+  const nav_msgs::msg::OccupancyGrid * planner_costmap,
+  const GuardSettings & settings);
+
+bool shouldRetryBlockedRepair(
+  const std::optional<std::size_t> & last_repair_nearest_index,
+  std::size_t nearest_index,
+  int min_repair_progress_indices,
+  double seconds_since_last_repair,
+  double blocked_retry_wait_sec,
+  bool costmap_updated_since_repair);
+
+nav_msgs::msg::Path slicePath(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  std::size_t end_index_inclusive);
+
+nav_msgs::msg::Path stitchPaths(
+  const nav_msgs::msg::Path & bypass,
+  const nav_msgs::msg::Path & original,
+  std::size_t rejoin_index);
+
+}  // namespace obstacle_nav2
+
+#endif  // OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
--- a/src/navigation/obstacle_nav2/launch/pycache/obstacle_nav2.launch.cpython-310.pyc
+++ b/src/navigation/obstacle_nav2/launch/pycache/obstacle_nav2.launch.cpython-310.pyc
--- a/src/navigation/obstacle_nav2/launch/nav2_profile_tuner.launch.py
+++ b/src/navigation/obstacle_nav2/launch/nav2_profile_tuner.launch.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('obstacle_nav2')
+    profile_10_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
+    profile_11_path = os.path.join(pkg_dir, 'config', 'nav2_profile_11.yaml')
+
+    profile_10_yaml = LaunchConfiguration('profile_10_yaml')
+    profile_11_yaml = LaunchConfiguration('profile_11_yaml')
+    profile_trigger_topic = LaunchConfiguration('profile_trigger_topic')
+
+    nav2_profile_tuner = Node(
+        package='obstacle_nav2',
+        executable='nav2_profile_tuner',
+        name='nav2_profile_tuner',
+        output='screen',
+        parameters=[{
+            'profile_10_yaml': profile_10_yaml,
+            'profile_11_yaml': profile_11_yaml,
+            'trigger_topic': profile_trigger_topic,
+        }],
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'profile_10_yaml',
+            default_value=profile_10_path,
+            description='YAML profile applied by default and when sign4return=10'),
+        DeclareLaunchArgument(
+            'profile_11_yaml',
+            default_value=profile_11_path,
+            description='YAML profile applied when sign4return=11'),
+        DeclareLaunchArgument(
+            'profile_trigger_topic',
+            default_value='/sign4return',
+            description='Int32 topic used to switch Nav2 runtime profiles'),
+
+        nav2_profile_tuner,
+    ])
--- a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py
+++ b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py
@@ -22,13 +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_ros.actions import SetRemap
+from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node, SetRemap
 from nav2_common.launch import RewrittenYaml


@@ -47,16 +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_params.yaml')
+    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')
+    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,
    )
@@ -93,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(
@@ -103,6 +140,7 @@ def generate_launch_description():
            'autostart': 'true',
        }.items(),
    )
+
    # ========================== Assembly ======================================
    return LaunchDescription([
        DeclareLaunchArgument(
@@ -137,9 +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'),
+

        safe_base_bringup,
        lslidar_launch,
        obstacle_scanner_launch,
+        static_map_publisher,
+        initial_pose_to_tf,
        navigation_launch,
    ])
--- a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py.bak_20260809_through_bt
+++ b/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,
+    ])
--- a/src/navigation/obstacle_nav2/launch/obstacle_nav2_q_waypoints.launch.py
+++ b/src/navigation/obstacle_nav2/launch/obstacle_nav2_q_waypoints.launch.py
@@ -0,0 +1,265 @@
+#!/usr/bin/env python3
+"""
+obstacle_nav2_q_waypoints.launch.py
+====================================
+复合启动文件：同时启动 obstacle_nav2 导航系统 + Q 路点候选选择器节点。
+
+架构概览
+--------
+本 launch 文件包含两个核心组件：
+  1. obstacle_nav2  : 完整的障碍物导航栈（底盘驱动、激光雷达、Nav2 导航、障碍物扫描）
+  2. q_waypoint_selector : 独立的 Python 节点，周期性从 costmap 中评估并选择
+     最优的 Q 路点候选目标，通过 NavigateToPose action 下发导航目标。
+
+Q 路点选择器的设计意图
+----------------------
+在比赛中，我们需要依次访问一系列路点（Q₁, Q₂, ..., Qₙ），但每个路点可能有多个
+候选位置。选择器从 JSON 文件中加载有序路点及其候选坐标，结合全局 costmap 的
+占用栅格信息，过滤掉被障碍物占据的候选点，并向 Nav2 下发当前最优的导航目标。
+
+启动方式
+--------
+  ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \
+      use_sim_time:=false \
+      global_frame:=odom \
+      json_dir:=/path/to/json \
+      goal_frame:=odom \
+      costmap_topic:=/global_costmap/costmap
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import FindExecutable
+
+
+def generate_launch_description():
+    # =========================================================================
+    # 路径配置 —— 定位 obstacle_nav2 包内的关键文件
+    # =========================================================================
+
+    # obstacle_nav2 包的共享目录（install 后的 share/obstacle_nav2）
+    pkg_dir = get_package_share_directory("obstacle_nav2")
+
+    # 基础导航 launch 文件路径，将被 IncludeLaunchDescription 引入
+    base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py")
+
+    # Q 路点选择器的 Python 脚本路径，将被 ExecuteProcess 作为独立进程运行
+    selector_script = os.path.join(pkg_dir, "config", "q_waypoint_selector.py")
+
+    # 默认的 JSON 路点文件目录
+    default_json_dir = os.path.join(pkg_dir, "config", "json")
+
+    # =========================================================================
+    # LaunchConfiguration —— 运行时参数占位符
+    # 这些变量在 launch 时被替换为实际的参数值（命令行传入或默认值）。
+    # 使用 LaunchConfiguration 而非直接读取值，是为了支持 launch 系统的
+    # 延迟求值（lazy evaluation）机制。
+    # =========================================================================
+
+    # ---- obstacle_nav2 基础导航栈参数 ----
+
+    # 是否使用仿真时间（实车=false，Gazebo 仿真=true）
+    use_sim_time = LaunchConfiguration("use_sim_time")
+
+    # 全局坐标系名称（仿真用 "odom"，实车用 "odom_combined"）
+    global_frame = LaunchConfiguration("global_frame")
+
+    # 是否加载静态地图（已知地图导航=true，纯 SLAM 探索=false）
+    use_static_map = LaunchConfiguration("use_static_map")
+
+    # 静态地图 YAML 文件路径（为空时不加载 map_server）
+    map_yaml = LaunchConfiguration("map_yaml")
+
+    # 是否启用底盘运动控制（调试时可关闭以只运行感知）
+    enable_motion = LaunchConfiguration("enable_motion")
+
+    # 是否启动底盘驱动节点（实车=true，纯仿真可视化=false）
+    start_base = LaunchConfiguration("start_base")
+
+    # 是否启动激光雷达驱动节点
+    start_lidar = LaunchConfiguration("start_lidar")
+
+    # 是否启动障碍物扫描节点（obstacle_scanner）
+    start_obstacle_scanner = LaunchConfiguration("start_obstacle_scanner")
+
+    # ---- Q 路点选择器专属参数 ----
+
+    # JSON 路点文件所在目录，包含如 Q1.json, Q2.json 等有序路点定义
+    json_dir = LaunchConfiguration("json_dir")
+
+    # 下发 NavigateToPose 导航目标时使用的坐标系
+    goal_frame = LaunchConfiguration("goal_frame")
+
+    # 全局 costmap 话题，用于判断候选路点是否被障碍物占据
+    costmap_topic = LaunchConfiguration("costmap_topic")
+
+    # Nav2 的 NavigateToPose action 名称
+    navigate_action = LaunchConfiguration("navigate_action")
+
+    # 占用阈值：costmap 栅格值 >= 此值的单元格视为被占用（范围 0-100）
+    occupied_threshold = LaunchConfiguration("occupied_threshold")
+
+    # 是否将未知区域（costmap 值 == -1）视为占用
+    treat_unknown_as_occupied = LaunchConfiguration("treat_unknown_as_occupied")
+
+    # 单个候选目标的超时时间（秒），超时后取消当前目标并尝试下一个候选
+    goal_timeout_sec = LaunchConfiguration("goal_timeout_sec")
+
+    # 选择器的控制周期（秒），即多久重新评估一次最优候选路点
+    selection_period_sec = LaunchConfiguration("selection_period_sec")
+
+    # =========================================================================
+    # 组件 1 —— obstacle_nav2 基础导航栈
+    # 通过 IncludeLaunchDescription 引入 obstacle_nav2.launch.py，
+    # 将其作为一个子 launch 嵌入当前 launch 描述中。
+    # 传入的参数会透传给子 launch 文件，实现参数的统一管理。
+    # =========================================================================
+    obstacle_nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(base_launch),
+        launch_arguments={
+            "use_sim_time": use_sim_time,
+            "global_frame": global_frame,
+            "use_static_map": use_static_map,
+            "map_yaml": map_yaml,
+            "enable_motion": enable_motion,
+            "start_base": start_base,
+            "start_lidar": start_lidar,
+            "start_obstacle_scanner": start_obstacle_scanner,
+        }.items(),
+    )
+
+    # =========================================================================
+    # 组件 2 —— Q 路点候选选择器
+    # 作为一个独立的 Python 进程运行（ExecuteProcess），而非 ROS 2 Node action。
+    # 选择器内部会创建一个 rclpy 节点，订阅 costmap、发布导航目标。
+    # 参数通过 ROS 2 标准的 --ros-args -p 方式传递。
+    # =========================================================================
+    q_waypoint_selector = ExecuteProcess(
+        cmd=[
+            # 使用 FindExecutable 查找 python3 可执行文件，确保跨平台兼容
+            FindExecutable(name="python3"),
+            selector_script,
+            # ROS 2 命令行参数传递标准格式
+            "--ros-args",
+            "-p", ["json_dir:=", json_dir],
+            "-p", ["goal_frame:=", goal_frame],
+            "-p", ["costmap_topic:=", costmap_topic],
+            "-p", ["navigate_action:=", navigate_action],
+            "-p", ["occupied_threshold:=", occupied_threshold],
+            "-p", ["treat_unknown_as_occupied:=", treat_unknown_as_occupied],
+            "-p", ["goal_timeout_sec:=", goal_timeout_sec],
+            "-p", ["selection_period_sec:=", selection_period_sec],
+            "-p", ["use_sim_time:=", use_sim_time],
+        ],
+        # 将进程的标准输出打印到终端，便于调试
+        output="screen",
+        emulate_tty=True,
+    )
+
+    # =========================================================================
+    # LaunchDescription —— 组装并返回完整的 launch 描述
+    # 包含三部分：
+    #   1. DeclareLaunchArgument —— 声明所有可配置参数及其默认值
+    #   2. obstacle_nav2           —— 基础导航栈
+    #   3. q_waypoint_selector     —— Q 路点选择器
+    # =========================================================================
+    return LaunchDescription(
+        [
+            # =================================================================
+            # obstacle_nav2 基础导航栈参数声明
+            # =================================================================
+
+            # 是否使用仿真时间（/clock 话题），实车场景应设为 false
+            DeclareLaunchArgument("use_sim_time", default_value="false"),
+
+            # 全局坐标系：仿真用 "odom"，实车 EKF 融合后用 "odom_combined"
+            DeclareLaunchArgument("global_frame", default_value="odom"),
+
+            # 是否启用静态地图层（map_server），在线 SLAM 建图时设为 false
+            DeclareLaunchArgument("use_static_map", default_value="false"),
+
+            # 静态地图 yaml 文件路径，为空字符串时不加载地图
+            DeclareLaunchArgument("map_yaml", default_value=""),
+
+            # 是否启用底盘运动控制指令下发，调试时可关闭
+            DeclareLaunchArgument("enable_motion", default_value="true"),
+
+            # 是否启动底盘串口驱动节点（实车需要，仿真不需要）
+            DeclareLaunchArgument("start_base", default_value="true"),
+
+            # 是否启动激光雷达驱动节点
+            DeclareLaunchArgument("start_lidar", default_value="true"),
+
+            # 是否启动障碍物扫描与避障节点
+            DeclareLaunchArgument("start_obstacle_scanner", default_value="true"),
+
+            # =================================================================
+            # Q 路点选择器参数声明
+            # =================================================================
+
+            # JSON 路点文件目录，每个 JSON 文件定义一个路点的候选坐标列表
+            DeclareLaunchArgument(
+                "json_dir",
+                default_value=default_json_dir,
+                description="Directory containing ordered waypoint JSON files.",
+            ),
+
+            # 导航目标使用的坐标帧，应与全局 costmap 的坐标系一致
+            DeclareLaunchArgument(
+                "goal_frame",
+                default_value="odom",
+                description="Frame used for NavigateToPose goals.",
+            ),
+
+            # 全局 costmap 话题，选择器订阅此话题来评估候选点的可通行性
+            DeclareLaunchArgument(
+                "costmap_topic",
+                default_value="/global_costmap/costmap",
+                description="OccupancyGrid topic used to reject occupied candidates.",
+            ),
+
+            # NavigateToPose action 的服务名，与 Nav2 的 bt_navigator 对应
+            DeclareLaunchArgument(
+                "navigate_action",
+                default_value="/navigate_to_pose",
+                description="Nav2 NavigateToPose action name.",
+            ),
+
+            # costmap 占用判定阈值（0-100），>= 此值的栅格视为障碍物
+            DeclareLaunchArgument(
+                "occupied_threshold",
+                default_value="50",
+                description="Costmap cells >= this value are considered occupied.",
+            ),
+
+            # 是否将未知区域（costmap 值 == -1）视为不可通行
+            DeclareLaunchArgument(
+                "treat_unknown_as_occupied",
+                default_value="true",
+                description="Treat costmap value -1 as occupied.",
+            ),
+
+            # 单个导航目标的超时时间，超时后自动取消并切换候选
+            DeclareLaunchArgument(
+                "goal_timeout_sec",
+                default_value="60.0",
+                description="Cancel a candidate goal if it does not finish before this timeout.",
+            ),
+
+            # 选择器主循环的控制周期（秒），值越小响应越快但 CPU 开销越大
+            DeclareLaunchArgument(
+                "selection_period_sec",
+                default_value="0.5",
+                description="Selector control period.",
+            ),
+
+            # ---- 组装已配置的 action ----
+            obstacle_nav2,
+            q_waypoint_selector,
+        ]
+    )
--- a/src/navigation/obstacle_nav2/launch/trajectory_guard.launch.py
+++ b/src/navigation/obstacle_nav2/launch/trajectory_guard.launch.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+"""Launch obstacle_nav2 with the C++ trajectory guard node."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("obstacle_nav2")
+    base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py")
+    guard_params = LaunchConfiguration("guard_params")
+
+    obstacle_nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(base_launch),
+        launch_arguments={
+            "use_sim_time": LaunchConfiguration("use_sim_time"),
+            "global_frame": LaunchConfiguration("global_frame"),
+            "use_static_map": LaunchConfiguration("use_static_map"),
+            "map_yaml": LaunchConfiguration("map_yaml"),
+            "enable_motion": LaunchConfiguration("enable_motion"),
+            "start_base": LaunchConfiguration("start_base"),
+            "start_lidar": LaunchConfiguration("start_lidar"),
+            "start_obstacle_scanner": LaunchConfiguration("start_obstacle_scanner"),
+        }.items(),
+    )
+
+    trajectory_guard = Node(
+        package="obstacle_nav2",
+        executable="trajectory_guard_node",
+        name="trajectory_guard_node",
+        output="screen",
+        parameters=[guard_params],
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("use_sim_time", default_value="false"),
+            DeclareLaunchArgument("global_frame", default_value="odom"),
+            DeclareLaunchArgument("use_static_map", default_value="false"),
+            DeclareLaunchArgument("map_yaml", default_value=""),
+            DeclareLaunchArgument("enable_motion", default_value="true"),
+            DeclareLaunchArgument("start_base", default_value="true"),
+            DeclareLaunchArgument("start_lidar", default_value="true"),
+            DeclareLaunchArgument("start_obstacle_scanner", default_value="true"),
+            DeclareLaunchArgument(
+                "guard_params",
+                default_value=os.path.join(pkg_dir, "config", "trajectory_guard.yaml"),
+            ),
+            obstacle_nav2,
+            trajectory_guard,
+        ]
+    )
--- a/src/navigation/obstacle_nav2/package.xml
+++ b/src/navigation/obstacle_nav2/package.xml
@@ -19,6 +19,10 @@
  <depend>geometry_msgs</depend>
  <depend>nav_msgs</depend>
  <depend>std_msgs</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>nav2_msgs</depend>
+  <depend>rclcpp_action</depend>
+  <depend>yaml-cpp</depend>

  <exec_depend>launch</exec_depend>
  <exec_depend>launch_ros</exec_depend>
--- a/src/navigation/obstacle_nav2/scripts/initial_pose_to_tf.py
+++ b/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()
--- a/src/navigation/obstacle_nav2/scripts/static_map_publisher.py
+++ b/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()
--- a/src/navigation/obstacle_nav2/src/nav2_profile_loader.cpp
+++ b/src/navigation/obstacle_nav2/src/nav2_profile_loader.cpp
@@ -0,0 +1,238 @@
+#include "obstacle_nav2/nav2_profile_loader.hpp"
+
+#include <algorithm>
+#include <cctype>
+#include <cstdint>
+#include <regex>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "yaml-cpp/yaml.h"
+
+namespace obstacle_nav2
+{
+namespace
+{
+
+std::string lowerCopy(std::string value)
+{
+  std::transform(
+    value.begin(), value.end(), value.begin(),
+    [](unsigned char c) {return static_cast<char>(std::tolower(c));});
+  return value;
+}
+
+bool isBoolScalar(const std::string & value)
+{
+  const auto lower = lowerCopy(value);
+  return lower == "true" || lower == "false";
+}
+
+bool isIntegerScalar(const std::string & value)
+{
+  static const std::regex pattern(R"(^[-+]?[0-9]+$)");
+  return std::regex_match(value, pattern);
+}
+
+bool isDoubleScalar(const std::string & value)
+{
+  static const std::regex pattern(
+    R"(^[-+]?(([0-9]+\.?[0-9]*)|(\.[0-9]+))([eE][-+]?[0-9]+)?$)");
+  return std::regex_match(value, pattern);
+}
+
+rclcpp::Parameter scalarToParameter(const std::string & name, const YAML::Node & node)
+{
+  const auto value = node.Scalar();
+  if (isBoolScalar(value)) {
+    return rclcpp::Parameter(name, node.as<bool>());
+  }
+  if (isIntegerScalar(value)) {
+    return rclcpp::Parameter(name, static_cast<int64_t>(node.as<int64_t>()));
+  }
+  if (isDoubleScalar(value)) {
+    return rclcpp::Parameter(name, node.as<double>());
+  }
+  return rclcpp::Parameter(name, value);
+}
+
+rclcpp::Parameter sequenceToParameter(const std::string & name, const YAML::Node & node)
+{
+  std::vector<std::string> values;
+  values.reserve(node.size());
+  for (const auto & item : node) {
+    if (!item.IsScalar()) {
+      throw std::runtime_error("Parameter array '" + name + "' contains a non-scalar value");
+    }
+    values.push_back(item.Scalar());
+  }
+
+  const auto all_bool = std::all_of(values.begin(), values.end(), isBoolScalar);
+  if (all_bool) {
+    std::vector<bool> parsed;
+    parsed.reserve(values.size());
+    for (const auto & value : values) {
+      parsed.push_back(YAML::Load(value).as<bool>());
+    }
+    return rclcpp::Parameter(name, parsed);
+  }
+
+  const auto all_integer = std::all_of(values.begin(), values.end(), isIntegerScalar);
+  if (all_integer) {
+    std::vector<int64_t> parsed;
+    parsed.reserve(values.size());
+    for (const auto & value : values) {
+      parsed.push_back(YAML::Load(value).as<int64_t>());
+    }
+    return rclcpp::Parameter(name, parsed);
+  }
+
+  const auto all_double = std::all_of(values.begin(), values.end(), isDoubleScalar);
+  if (all_double) {
+    std::vector<double> parsed;
+    parsed.reserve(values.size());
+    for (const auto & value : values) {
+      parsed.push_back(YAML::Load(value).as<double>());
+    }
+    return rclcpp::Parameter(name, parsed);
+  }
+
+  return rclcpp::Parameter(name, values);
+}
+
+rclcpp::Parameter yamlToParameter(const std::string & name, const YAML::Node & node)
+{
+  if (node.IsScalar()) {
+    return scalarToParameter(name, node);
+  }
+  if (node.IsSequence()) {
+    return sequenceToParameter(name, node);
+  }
+  throw std::runtime_error("Parameter '" + name + "' must be a scalar or sequence");
+}
+
+void appendFlattenedParameters(
+  const YAML::Node & parameters,
+  const std::string & prefix,
+  std::vector<rclcpp::Parameter> & output)
+{
+  if (!parameters.IsMap()) {
+    throw std::runtime_error("ros__parameters must contain a parameter map");
+  }
+
+  for (const auto & parameter_entry : parameters) {
+    const auto key = parameter_entry.first.as<std::string>();
+    if (key.empty()) {
+      throw std::runtime_error("ros__parameters contains an empty parameter name");
+    }
+
+    const auto parameter_name = prefix.empty() ? key : prefix + "." + key;
+    const auto value = parameter_entry.second;
+    if (value.IsMap()) {
+      appendFlattenedParameters(value, parameter_name, output);
+    } else {
+      output.push_back(yamlToParameter(parameter_name, value));
+    }
+  }
+}
+
+NodeParameterSet loadNodeParameterSet(
+  const std::string & node_name,
+  const YAML::Node & parameters)
+{
+  if (node_name.empty()) {
+    throw std::runtime_error("Nav2 profile contains an empty node name");
+  }
+  if (!parameters.IsMap()) {
+    throw std::runtime_error("Node '" + node_name + "' must contain a parameter map");
+  }
+
+  NodeParameterSet node_set;
+  node_set.node_name = node_name;
+  for (const auto & parameter_entry : parameters) {
+    const auto parameter_name = parameter_entry.first.as<std::string>();
+    if (parameter_name.empty()) {
+      throw std::runtime_error("Node '" + node_name + "' contains an empty parameter name");
+    }
+    node_set.parameters.push_back(yamlToParameter(parameter_name, parameter_entry.second));
+  }
+  return node_set;
+}
+
+bool isNativeRuntimeProfileNode(const std::string & path)
+{
+  return path == "controller_server" || path == "velocity_smoother";
+}
+
+void collectRosParameterNodes(
+  const YAML::Node & yaml_node,
+  const std::string & path,
+  Nav2Profile & profile)
+{
+  if (!yaml_node.IsMap()) {
+    return;
+  }
+
+  const auto ros_parameters = yaml_node["ros__parameters"];
+  if (ros_parameters) {
+    if (path.empty()) {
+      throw std::runtime_error("ros__parameters must be nested under a node name");
+    }
+
+    NodeParameterSet node_set;
+    node_set.node_name = path.front() == '/' ? path : "/" + path;
+    appendFlattenedParameters(ros_parameters, "", node_set.parameters);
+    if (isNativeRuntimeProfileNode(path)) {
+      profile.nodes.push_back(node_set);
+    }
+    return;
+  }
+
+  for (const auto & child_entry : yaml_node) {
+    const auto key = child_entry.first.as<std::string>();
+    const auto child_path = path.empty() ? key : path + "/" + key;
+    collectRosParameterNodes(child_entry.second, child_path, profile);
+  }
+}
+
+}  // namespace
+
+Nav2Profile loadNav2Profile(const std::string & path)
+{
+  const auto root = YAML::LoadFile(path);
+  const auto nodes = root["nodes"];
+
+  Nav2Profile profile;
+  if (nodes && nodes.IsMap()) {
+    for (const auto & node_entry : nodes) {
+      const auto node_name = node_entry.first.as<std::string>();
+      profile.nodes.push_back(loadNodeParameterSet(node_name, node_entry.second));
+    }
+    return profile;
+  }
+
+  collectRosParameterNodes(root, "", profile);
+  if (profile.nodes.empty()) {
+    throw std::runtime_error(
+      "Nav2 profile '" + path + "' must contain a nodes map or ros__parameters blocks");
+  }
+
+  return profile;
+}
+
+std::vector<rclcpp::Parameter> filterDeclaredParameters(
+  const std::vector<rclcpp::Parameter> & parameters,
+  const std::set<std::string> & declared_names)
+{
+  std::vector<rclcpp::Parameter> filtered;
+  filtered.reserve(parameters.size());
+  for (const auto & parameter : parameters) {
+    if (declared_names.count(parameter.get_name()) > 0) {
+      filtered.push_back(parameter);
+    }
+  }
+  return filtered;
+}
+
+}  // namespace obstacle_nav2
--- a/src/navigation/obstacle_nav2/src/nav2_profile_tuner.cpp
+++ b/src/navigation/obstacle_nav2/src/nav2_profile_tuner.cpp
@@ -0,0 +1,210 @@
+#include <chrono>
+#include <map>
+#include <memory>
+#include <set>
+#include <string>
+
+#include "obstacle_nav2/nav2_profile_loader.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+
+namespace obstacle_nav2
+{
+namespace
+{
+
+std::string setParametersServiceName(const std::string & node_name)
+{
+  if (node_name.empty()) {
+    return "/set_parameters";
+  }
+  if (node_name.front() == '/') {
+    return node_name + "/set_parameters";
+  }
+  return "/" + node_name + "/set_parameters";
+}
+
+std::string listParametersServiceName(const std::string & node_name)
+{
+  if (node_name.empty()) {
+    return "/list_parameters";
+  }
+  if (node_name.front() == '/') {
+    return node_name + "/list_parameters";
+  }
+  return "/" + node_name + "/list_parameters";
+}
+
+}  // namespace
+
+class Nav2ProfileTuner : public rclcpp::Node
+{
+public:
+  Nav2ProfileTuner()
+  : Node("nav2_profile_tuner")
+  {
+    const auto profile_10_path = declare_parameter<std::string>("profile_10_yaml", "");
+    const auto profile_11_path = declare_parameter<std::string>("profile_11_yaml", "");
+    const auto trigger_topic = declare_parameter<std::string>("trigger_topic", "/sign4return");
+
+    if (profile_10_path.empty() || profile_11_path.empty()) {
+      throw std::runtime_error("profile_10_yaml and profile_11_yaml must both be set");
+    }
+
+    profiles_[10] = loadNav2Profile(profile_10_path);
+    profiles_[11] = loadNav2Profile(profile_11_path);
+
+    trigger_sub_ = create_subscription<std_msgs::msg::Int32>(
+      trigger_topic, 10,
+      [this](const std_msgs::msg::Int32::SharedPtr msg) {
+        handleTrigger(msg->data);
+      });
+
+    retry_timer_ = create_wall_timer(
+      std::chrono::milliseconds(500),
+      [this]() {
+        tryApplyPendingProfile();
+      });
+
+    requestProfile(10);
+  }
+
+private:
+  using SetParameters = rcl_interfaces::srv::SetParameters;
+  using ListParameters = rcl_interfaces::srv::ListParameters;
+
+  void handleTrigger(const int32_t command)
+  {
+    if (command == 10 || command == 11) {
+      requestProfile(command);
+      return;
+    }
+    RCLCPP_DEBUG(get_logger(), "Ignoring sign4return=%d", command);
+  }
+
+  void requestProfile(const int command)
+  {
+    if (active_profile_ == command && !has_pending_profile_) {
+      return;
+    }
+    pending_profile_ = command;
+    has_pending_profile_ = true;
+    RCLCPP_INFO(get_logger(), "Queued Nav2 profile %d", command);
+    tryApplyPendingProfile();
+  }
+
+  void tryApplyPendingProfile()
+  {
+    if (!has_pending_profile_) {
+      return;
+    }
+
+    const auto profile_it = profiles_.find(pending_profile_);
+    if (profile_it == profiles_.end()) {
+      RCLCPP_ERROR(get_logger(), "No Nav2 profile loaded for command %d", pending_profile_);
+      has_pending_profile_ = false;
+      return;
+    }
+
+    for (const auto & node_set : profile_it->second.nodes) {
+      const auto set_service_name = setParametersServiceName(node_set.node_name);
+      auto & set_client = set_clients_[set_service_name];
+      if (!set_client) {
+        set_client = create_client<SetParameters>(set_service_name);
+      }
+      if (!set_client->wait_for_service(std::chrono::milliseconds(0))) {
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), *get_clock(), 3000,
+          "Waiting for parameter service %s", set_service_name.c_str());
+        return;
+      }
+
+      const auto list_service_name = listParametersServiceName(node_set.node_name);
+      auto & list_client = list_clients_[list_service_name];
+      if (!list_client) {
+        list_client = create_client<ListParameters>(list_service_name);
+      }
+      if (!list_client->wait_for_service(std::chrono::milliseconds(0))) {
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), *get_clock(), 3000,
+          "Waiting for parameter service %s", list_service_name.c_str());
+        return;
+      }
+    }
+
+    const auto command = pending_profile_;
+    for (const auto & node_set : profile_it->second.nodes) {
+      const auto list_service_name = listParametersServiceName(node_set.node_name);
+      auto request = std::make_shared<ListParameters::Request>();
+      request->depth = ListParameters::Request::DEPTH_RECURSIVE;
+
+      list_clients_.at(list_service_name)->async_send_request(
+        request,
+        [this, node_set](rclcpp::Client<ListParameters>::SharedFuture future) {
+          const auto response = future.get();
+          const std::set<std::string> declared_names(
+            response->result.names.begin(), response->result.names.end());
+          const auto parameters = filterDeclaredParameters(node_set.parameters, declared_names);
+
+          if (parameters.empty()) {
+            RCLCPP_WARN(
+              get_logger(), "No declared parameters from profile for %s",
+              node_set.node_name.c_str());
+            return;
+          }
+
+          auto set_request = std::make_shared<SetParameters::Request>();
+          set_request->parameters.reserve(parameters.size());
+          for (const auto & parameter : parameters) {
+            set_request->parameters.push_back(parameter.to_parameter_msg());
+          }
+
+          const auto set_service_name = setParametersServiceName(node_set.node_name);
+          set_clients_.at(set_service_name)->async_send_request(
+            set_request,
+            [this, node_name = node_set.node_name](
+              rclcpp::Client<SetParameters>::SharedFuture set_future) {
+              const auto set_response = set_future.get();
+              for (const auto & result : set_response->results) {
+                if (!result.successful) {
+                  RCLCPP_WARN(
+                    get_logger(), "Parameter set failed on %s: %s",
+                    node_name.c_str(), result.reason.c_str());
+                }
+              }
+            });
+        });
+    }
+
+    active_profile_ = command;
+    has_pending_profile_ = false;
+    RCLCPP_INFO(get_logger(), "Applied Nav2 profile %d", active_profile_);
+  }
+
+  std::map<int, Nav2Profile> profiles_;
+  std::map<std::string, rclcpp::Client<SetParameters>::SharedPtr> set_clients_;
+  std::map<std::string, rclcpp::Client<ListParameters>::SharedPtr> list_clients_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr trigger_sub_;
+  rclcpp::TimerBase::SharedPtr retry_timer_;
+  int active_profile_{0};
+  int pending_profile_{0};
+  bool has_pending_profile_{false};
+};
+
+}  // namespace obstacle_nav2
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  try {
+    rclcpp::spin(std::make_shared<obstacle_nav2::Nav2ProfileTuner>());
+  } catch (const std::exception & e) {
+    RCLCPP_FATAL(rclcpp::get_logger("nav2_profile_tuner"), "%s", e.what());
+    rclcpp::shutdown();
+    return 1;
+  }
+  rclcpp::shutdown();
+  return 0;
+}
--- a/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp
+++ b/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp
@@ -3,6 +3,7 @@
 #include <algorithm>
 #include <chrono>
 #include <cmath>
+#include <optional>
 #include <string>
 #include <vector>

@@ -53,11 +54,12 @@ void ObstacleArrayLayer::onInitialize()
  node->declare_parameter(name_ + ".enabled", true);
  node->declare_parameter(name_ + ".topic", std::string("/obstacles"));
  node->declare_parameter(name_ + ".obstacle_timeout", 0.5);
-  node->declare_parameter(name_ + ".transform_tolerance", 0.2);
+  node->declare_parameter(name_ + ".transform_tolerance", 0.02);
  node->declare_parameter(name_ + ".default_obstacle_radius", 0.05);
  node->declare_parameter(name_ + ".minimum_obstacle_radius", 0.02);
  node->declare_parameter(name_ + ".maximum_obstacle_radius", 0.50);
  node->declare_parameter(name_ + ".extra_inflation", 0.02);
+  node->declare_parameter(name_ + ".retain_previous_on_empty_snapshot", true);

  node->get_parameter(name_ + ".enabled", enabled_);
  node->get_parameter(name_ + ".topic", topic_);
@@ -67,6 +69,8 @@ void ObstacleArrayLayer::onInitialize()
  node->get_parameter(name_ + ".minimum_obstacle_radius", minimum_obstacle_radius_);
  node->get_parameter(name_ + ".maximum_obstacle_radius", maximum_obstacle_radius_);
  node->get_parameter(name_ + ".extra_inflation", extra_inflation_);
+  node->get_parameter(
+    name_ + ".retain_previous_on_empty_snapshot", retain_previous_on_empty_snapshot_);

  global_frame_ = layered_costmap_->getGlobalFrameID();

@@ -78,7 +82,7 @@ void ObstacleArrayLayer::onInitialize()
  rclcpp::SubscriptionOptions subscription_options;
  subscription_options.callback_group = callback_group_;
  obstacle_sub_ = node->create_subscription<obstacle_scanner::msg::ObstacleArray>(
-    topic_, rclcpp::QoS(10).reliable(),
+    topic_, rclcpp::QoS(rclcpp::KeepLast(1)).reliable(),
    std::bind(&ObstacleArrayLayer::obstacleCallback, this, std::placeholders::_1),
    subscription_options);
  callback_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
@@ -87,9 +91,7 @@ void ObstacleArrayLayer::onInitialize()
  callback_stop_.store(false, std::memory_order_release);
  callback_thread_ = std::thread(
    [this]() {
-      while (!callback_stop_.load(std::memory_order_acquire)) {
-        callback_executor_->spin_once(std::chrono::milliseconds(100));
-      }
+      callback_executor_->spin();
    });

  current_ = true;
@@ -290,6 +292,17 @@ ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::applySnapshot(
  return bounds;
 }

+std::optional<ObstacleArrayLayer::SnapshotBounds> ObstacleArrayLayer::applySnapshotIfNotEmpty(
+  nav2_costmap_2d::Costmap2D & grid,
+  const std::vector<CircleObstacle> & obstacles,
+  double resolution, double origin_x, double origin_y)
+{
+  if (obstacles.empty()) {
+    return std::nullopt;
+  }
+  return applySnapshot(grid, obstacles, resolution, origin_x, origin_y);
+}
+
 ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::mergeBounds(
  const SnapshotBounds & first, const SnapshotBounds & second)
 {
@@ -378,18 +391,20 @@ void ObstacleArrayLayer::obstacleCallback(
    valid_obstacles.push_back({obs.center_x, obs.center_y, effective_r});
  }

-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    const SnapshotBounds previous_bounds = current_bounds_;
-    current_bounds_ = applySnapshot(
-      *this, valid_obstacles, getResolution(), origin_x, origin_y);
-    pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, previous_bounds);
-    if (previous_bounds.valid || current_bounds_.valid) {
-      ++bounds_generation_;
-    }
-    has_received_obstacles_ = current_bounds_.valid;
-    last_obstacle_time_ = node->now();
+  if (valid_obstacles.empty() && retain_previous_on_empty_snapshot_) {
+    return;
  }
+
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  const SnapshotBounds previous_bounds = current_bounds_;
+  current_bounds_ = applySnapshot(
+    *this, valid_obstacles, getResolution(), origin_x, origin_y);
+  pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, previous_bounds);
+  if (previous_bounds.valid || current_bounds_.valid) {
+    ++bounds_generation_;
+  }
+  has_received_obstacles_ = current_bounds_.valid;
+  last_obstacle_time_ = node->now();
 }

 }  // namespace obstacle_nav2
--- a/src/navigation/obstacle_nav2/src/trajectory_guard.cpp
+++ b/src/navigation/obstacle_nav2/src/trajectory_guard.cpp
@@ -0,0 +1,290 @@
+#include "obstacle_nav2/trajectory_guard.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+namespace obstacle_nav2
+{
+namespace
+{
+
+bool worldToMap(
+  const nav_msgs::msg::OccupancyGrid & grid,
+  double wx,
+  double wy,
+  int & mx,
+  int & my)
+{
+  const double resolution = grid.info.resolution;
+  if (resolution <= 0.0) {
+    return false;
+  }
+
+  const double ox = grid.info.origin.position.x;
+  const double oy = grid.info.origin.position.y;
+  if (wx < ox || wy < oy) {
+    return false;
+  }
+
+  mx = static_cast<int>((wx - ox) / resolution);
+  my = static_cast<int>((wy - oy) / resolution);
+  return mx >= 0 && my >= 0 &&
+    mx < static_cast<int>(grid.info.width) &&
+    my < static_cast<int>(grid.info.height);
+}
+
+bool occupiedAt(
+  const nav_msgs::msg::OccupancyGrid & grid,
+  double wx,
+  double wy,
+  const GuardSettings & settings)
+{
+  int mx = 0;
+  int my = 0;
+  if (!worldToMap(grid, wx, wy, mx, my)) {
+    return settings.treat_unknown_as_occupied;
+  }
+
+  const auto value = grid.data[my * grid.info.width + mx];
+  if (value < 0) {
+    return settings.treat_unknown_as_occupied;
+  }
+  return value >= settings.occupied_threshold;
+}
+
+bool footprintOccupied(
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  double cx,
+  double cy,
+  double yaw,
+  const GuardSettings & settings)
+{
+  const double half_l = settings.footprint_half_length + settings.footprint_padding;
+  const double half_w = settings.footprint_half_width + settings.footprint_padding;
+  const double step = std::max(0.02, settings.footprint_sample_step);
+  const double c = std::cos(yaw);
+  const double s = std::sin(yaw);
+
+  auto sample = [&](double x, double y) {
+      const double wx = cx + c * x - s * y;
+      const double wy = cy + s * x + c * y;
+      return occupiedAt(costmap, wx, wy, settings);
+    };
+
+  if (sample(0.0, 0.0)) {
+    return true;
+  }
+  for (double x = -half_l; x <= half_l + 1.0e-9; x += step) {
+    if (sample(x, -half_w) || sample(x, 0.0) || sample(x, half_w)) {
+      return true;
+    }
+  }
+  for (double y = -half_w; y <= half_w + 1.0e-9; y += step) {
+    if (sample(-half_l, y) || sample(0.0, y) || sample(half_l, y)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+}  // namespace
+
+double yawFromPose(const geometry_msgs::msg::PoseStamped & pose)
+{
+  const auto & q = pose.pose.orientation;
+  return std::atan2(
+    2.0 * (q.w * q.z + q.x * q.y),
+    1.0 - 2.0 * (q.y * q.y + q.z * q.z));
+}
+
+double distance2d(
+  const geometry_msgs::msg::PoseStamped & a,
+  const geometry_msgs::msg::PoseStamped & b)
+{
+  const double dx = a.pose.position.x - b.pose.position.x;
+  const double dy = a.pose.position.y - b.pose.position.y;
+  return std::hypot(dx, dy);
+}
+
+std::size_t nearestPathIndex(
+  const nav_msgs::msg::Path & path,
+  const geometry_msgs::msg::PoseStamped & pose)
+{
+  std::size_t best_index = 0;
+  double best_distance = std::numeric_limits<double>::infinity();
+
+  for (std::size_t i = 0; i < path.poses.size(); ++i) {
+    const double d = distance2d(path.poses[i], pose);
+    if (d < best_distance) {
+      best_distance = d;
+      best_index = i;
+    }
+  }
+  return best_index;
+}
+
+std::size_t advanceByDistance(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  double distance_m)
+{
+  if (path.poses.empty()) {
+    return 0;
+  }
+
+  std::size_t index = std::min(start_index, path.poses.size() - 1);
+  double traveled = 0.0;
+  while (index + 1 < path.poses.size() && traveled < distance_m) {
+    traveled += distance2d(path.poses[index], path.poses[index + 1]);
+    ++index;
+  }
+  return index;
+}
+
+CollisionCheckResult checkPathAhead(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const GuardSettings & settings)
+{
+  if (path.poses.empty()) {
+    return CollisionCheckResult{true, 0, "empty_path"};
+  }
+
+  const auto clamped_start = std::min(start_index, path.poses.size() - 1);
+  const auto end_index =
+    advanceByDistance(path, clamped_start, settings.lookahead_distance);
+  const double step = std::max(0.02, settings.footprint_sample_step);
+
+  for (std::size_t i = clamped_start; i <= end_index && i < path.poses.size(); ++i) {
+    if (i + 1 >= path.poses.size() || i == end_index) {
+      const auto & pose = path.poses[i];
+      if (footprintOccupied(
+          costmap, pose.pose.position.x, pose.pose.position.y, yawFromPose(pose), settings))
+      {
+        return CollisionCheckResult{true, i, "occupied"};
+      }
+      continue;
+    }
+
+    const auto & a = path.poses[i];
+    const auto & b = path.poses[i + 1];
+    const double dx = b.pose.position.x - a.pose.position.x;
+    const double dy = b.pose.position.y - a.pose.position.y;
+    const double segment_length = std::hypot(dx, dy);
+    const double yaw = segment_length > 1.0e-6 ? std::atan2(dy, dx) : yawFromPose(a);
+    const int samples = std::max(1, static_cast<int>(std::ceil(segment_length / step)));
+
+    for (int sample_index = 0; sample_index <= samples; ++sample_index) {
+      const double t = static_cast<double>(sample_index) / static_cast<double>(samples);
+      const double x = a.pose.position.x + t * dx;
+      const double y = a.pose.position.y + t * dy;
+      if (footprintOccupied(costmap, x, y, yaw, settings)) {
+        return CollisionCheckResult{true, i, "occupied"};
+      }
+    }
+  }
+
+  return CollisionCheckResult{false, end_index, "clear"};
+}
+
+std::optional<std::size_t> findClearRejoinIndex(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const GuardSettings & settings)
+{
+  if (path.poses.empty()) {
+    return std::nullopt;
+  }
+
+  const auto first = advanceByDistance(path, start_index, settings.rejoin_min_distance);
+  const auto last = advanceByDistance(path, start_index, settings.rejoin_max_distance);
+  GuardSettings single_pose_settings = settings;
+  single_pose_settings.lookahead_distance = 0.0;
+
+  for (std::size_t i = first; i <= last && i < path.poses.size(); ++i) {
+    const auto result = checkPathAhead(path, i, costmap, single_pose_settings);
+    if (!result.blocked) {
+      return i;
+    }
+  }
+  return std::nullopt;
+}
+
+std::optional<std::size_t> findClearRejoinIndexWithPlannerCostmap(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & local_costmap,
+  const nav_msgs::msg::OccupancyGrid * planner_costmap,
+  const GuardSettings & settings)
+{
+  if (planner_costmap != nullptr) {
+    return findClearRejoinIndex(path, start_index, *planner_costmap, settings);
+  }
+  return findClearRejoinIndex(path, start_index, local_costmap, settings);
+}
+
+bool shouldRetryBlockedRepair(
+  const std::optional<std::size_t> & last_repair_nearest_index,
+  std::size_t nearest_index,
+  int min_repair_progress_indices,
+  double seconds_since_last_repair,
+  double blocked_retry_wait_sec,
+  bool costmap_updated_since_repair)
+{
+  if (!last_repair_nearest_index.has_value()) {
+    return true;
+  }
+
+  const auto min_progress =
+    static_cast<std::size_t>(std::max(0, min_repair_progress_indices));
+  if (nearest_index > *last_repair_nearest_index + min_progress) {
+    return true;
+  }
+
+  return seconds_since_last_repair >= blocked_retry_wait_sec &&
+    costmap_updated_since_repair;
+}
+
+nav_msgs::msg::Path slicePath(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  std::size_t end_index_inclusive)
+{
+  nav_msgs::msg::Path out;
+  out.header = path.header;
+  if (path.poses.empty()) {
+    return out;
+  }
+
+  const auto begin = std::min(start_index, path.poses.size() - 1);
+  const auto end = std::min(end_index_inclusive, path.poses.size() - 1);
+  for (std::size_t i = begin; i <= end; ++i) {
+    out.poses.push_back(path.poses[i]);
+  }
+  return out;
+}
+
+nav_msgs::msg::Path stitchPaths(
+  const nav_msgs::msg::Path & bypass,
+  const nav_msgs::msg::Path & original,
+  std::size_t rejoin_index)
+{
+  nav_msgs::msg::Path out;
+  out.header = bypass.header.frame_id.empty() ? original.header : bypass.header;
+  out.poses = bypass.poses;
+
+  if (original.poses.empty()) {
+    return out;
+  }
+
+  const auto begin = std::min(rejoin_index, original.poses.size());
+  for (std::size_t i = begin; i < original.poses.size(); ++i) {
+    out.poses.push_back(original.poses[i]);
+  }
+  return out;
+}
+
+}  // namespace obstacle_nav2
--- a/src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp
+++ b/src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp
@@ -0,0 +1,459 @@
+#include <algorithm>
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "nav2_msgs/action/follow_path.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "obstacle_nav2/trajectory_guard.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+using namespace std::chrono_literals;
+
+namespace obstacle_nav2
+{
+
+class TrajectoryGuardNode : public rclcpp::Node
+{
+public:
+  using ComputePathToPose = nav2_msgs::action::ComputePathToPose;
+  using FollowPath = nav2_msgs::action::FollowPath;
+  using ComputeGoalHandle = rclcpp_action::ClientGoalHandle<ComputePathToPose>;
+  using FollowGoalHandle = rclcpp_action::ClientGoalHandle<FollowPath>;
+
+  TrajectoryGuardNode()
+  : Node("trajectory_guard_node")
+  {
+    declare_parameter("input_path_topic", "/trajectory_guard/input_path");
+    declare_parameter("patched_path_topic", "/trajectory_guard/patched_path");
+    declare_parameter("reject_path_topic", "/reject_path");
+    declare_parameter("costmap_topic", "/local_costmap/costmap");
+    declare_parameter("planner_costmap_topic", "/global_costmap/costmap");
+    declare_parameter("odom_topic", "/odom_combined");
+    declare_parameter("planner_action", "/compute_path_to_pose");
+    declare_parameter("follow_action", "/follow_path");
+    declare_parameter("planner_id", "GridBased");
+    declare_parameter("controller_id", "FollowPath");
+    declare_parameter("goal_checker_id", "");
+    declare_parameter("check_period_sec", 0.5);
+    declare_parameter("lookahead_distance", 2.0);
+    declare_parameter("rejoin_min_distance", 1.0);
+    declare_parameter("rejoin_max_distance", 4.0);
+    declare_parameter("occupied_threshold", 50);
+    declare_parameter("treat_unknown_as_occupied", true);
+    declare_parameter("footprint_half_length", 0.14);
+    declare_parameter("footprint_half_width", 0.085);
+    declare_parameter("footprint_padding", 0.04);
+    declare_parameter("footprint_sample_step", 0.05);
+    declare_parameter("repair_cooldown_sec", 1.0);
+    declare_parameter("blocked_retry_wait_sec", 0.8);
+    declare_parameter("wait_for_planner_costmap_update", true);
+    declare_parameter("min_repair_progress_indices", 5);
+    declare_parameter("publish_zero_on_blocked", true);
+    declare_parameter("cmd_vel_topic", "/cmd_vel");
+    declare_parameter("execute_follow_path", true);
+
+    loadParameters();
+    last_repair_request_time_ =
+      get_clock()->now() - rclcpp::Duration::from_seconds(repair_cooldown_sec_);
+
+    path_sub_ = create_subscription<nav_msgs::msg::Path>(
+      get_parameter("input_path_topic").as_string(), 1,
+      [this](nav_msgs::msg::Path::SharedPtr msg) { onPath(msg); });
+    costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+      get_parameter("costmap_topic").as_string(), 1,
+      [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+        latest_costmap_ = msg;
+        ++local_costmap_generation_;
+      });
+    planner_costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+      get_parameter("planner_costmap_topic").as_string(), 1,
+      [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+        latest_planner_costmap_ = msg;
+        ++planner_costmap_generation_;
+      });
+    odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+      get_parameter("odom_topic").as_string(), 10,
+      [this](nav_msgs::msg::Odometry::SharedPtr msg) { latest_odom_ = msg; });
+
+    patched_path_pub_ = create_publisher<nav_msgs::msg::Path>(
+      get_parameter("patched_path_topic").as_string(), 1);
+    reject_path_pub_ = create_publisher<nav_msgs::msg::Path>(
+      get_parameter("reject_path_topic").as_string(), 1);
+    cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
+      get_parameter("cmd_vel_topic").as_string(), 1);
+
+    planner_client_ = rclcpp_action::create_client<ComputePathToPose>(
+      this, get_parameter("planner_action").as_string());
+    follow_client_ = rclcpp_action::create_client<FollowPath>(
+      this, get_parameter("follow_action").as_string());
+
+    const auto period = std::chrono::duration<double>(
+      std::max(0.1, get_parameter("check_period_sec").as_double()));
+    timer_ = create_wall_timer(
+      std::chrono::duration_cast<std::chrono::milliseconds>(period),
+      [this]() { tick(); });
+
+    RCLCPP_INFO(
+      get_logger(), "trajectory guard ready check_period_sec=%.2f input=%s patched=%s",
+      get_parameter("check_period_sec").as_double(),
+      get_parameter("input_path_topic").as_string().c_str(),
+      get_parameter("patched_path_topic").as_string().c_str());
+  }
+
+private:
+  void loadParameters()
+  {
+    settings_.lookahead_distance = get_parameter("lookahead_distance").as_double();
+    settings_.rejoin_min_distance = get_parameter("rejoin_min_distance").as_double();
+    settings_.rejoin_max_distance = get_parameter("rejoin_max_distance").as_double();
+    settings_.occupied_threshold = get_parameter("occupied_threshold").as_int();
+    settings_.treat_unknown_as_occupied =
+      get_parameter("treat_unknown_as_occupied").as_bool();
+    settings_.footprint_half_length = get_parameter("footprint_half_length").as_double();
+    settings_.footprint_half_width = get_parameter("footprint_half_width").as_double();
+    settings_.footprint_padding = get_parameter("footprint_padding").as_double();
+    settings_.footprint_sample_step = get_parameter("footprint_sample_step").as_double();
+    repair_cooldown_sec_ = std::max(0.0, get_parameter("repair_cooldown_sec").as_double());
+    blocked_retry_wait_sec_ =
+      std::max(0.0, get_parameter("blocked_retry_wait_sec").as_double());
+    wait_for_planner_costmap_update_ =
+      get_parameter("wait_for_planner_costmap_update").as_bool();
+    min_repair_progress_indices_ =
+      std::max(0, static_cast<int>(get_parameter("min_repair_progress_indices").as_int()));
+    publish_zero_on_blocked_ = get_parameter("publish_zero_on_blocked").as_bool();
+    execute_follow_path_ = get_parameter("execute_follow_path").as_bool();
+  }
+
+  void onPath(const nav_msgs::msg::Path::SharedPtr msg)
+  {
+    active_path_ = *msg;
+    sent_original_for_current_path_ = false;
+    last_repair_nearest_index_.reset();
+    last_repair_rejoin_index_.reset();
+    retry_without_planner_costmap_update_ = false;
+    waiting_for_planner_costmap_update_ = false;
+    RCLCPP_INFO(
+      get_logger(), "received replay path poses=%zu frame=%s",
+      active_path_.poses.size(), active_path_.header.frame_id.c_str());
+  }
+
+  std::optional<geometry_msgs::msg::PoseStamped> currentPose() const
+  {
+    if (!latest_odom_) {
+      return std::nullopt;
+    }
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header = latest_odom_->header;
+    pose.pose = latest_odom_->pose.pose;
+    return pose;
+  }
+
+  void tick()
+  {
+    if (active_path_.poses.empty() || !latest_costmap_) {
+      return;
+    }
+
+    const auto pose = currentPose();
+    if (!pose.has_value()) {
+      return;
+    }
+
+    const auto nearest = nearestPathIndex(active_path_, *pose);
+    const auto result = checkPathAhead(active_path_, nearest, *latest_costmap_, settings_);
+    if (!result.blocked) {
+      if (!sent_original_for_current_path_) {
+        publishAndMaybeFollow(active_path_, "original_clear");
+        sent_original_for_current_path_ = true;
+      }
+      return;
+    }
+
+    const auto now = get_clock()->now();
+    if (repair_in_flight_ ||
+      (now - last_repair_request_time_).seconds() < repair_cooldown_sec_)
+    {
+      return;
+    }
+
+    if (!waiting_for_planner_costmap_update_) {
+      waiting_for_planner_costmap_update_ = true;
+      waiting_planner_costmap_generation_ = planner_costmap_generation_;
+      RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 2000,
+        "blocked replay path; waiting for next planner costmap frame before requesting bypass");
+      publishStop();
+      return;
+    }
+
+    if (planner_costmap_generation_ <= waiting_planner_costmap_generation_) {
+      if (retry_without_planner_costmap_update_ &&
+        (now - last_repair_request_time_).seconds() >= blocked_retry_wait_sec_)
+      {
+        waiting_for_planner_costmap_update_ = false;
+      } else {
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), *get_clock(), 2000,
+          "blocked replay path; planner costmap has not updated yet");
+        publishStop();
+        return;
+      }
+    }
+
+    waiting_for_planner_costmap_update_ = false;
+
+    const bool costmap_updated_since_repair =
+      !wait_for_planner_costmap_update_ ||
+      planner_costmap_generation_ > last_repair_planner_costmap_generation_;
+    const bool timeout_retry_allowed =
+      retry_without_planner_costmap_update_ &&
+      (now - last_repair_request_time_).seconds() >= blocked_retry_wait_sec_;
+    if (!shouldRetryBlockedRepair(
+        last_repair_nearest_index_, nearest, min_repair_progress_indices_,
+        (now - last_repair_request_time_).seconds(), blocked_retry_wait_sec_,
+        costmap_updated_since_repair) && !timeout_retry_allowed)
+    {
+      RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 2000,
+        "patched path still blocked near the last repair point; waiting for path progress or costmap update");
+      publishStop();
+      return;
+    }
+
+    retry_without_planner_costmap_update_ = false;
+
+    std::size_t search_start = nearest;
+    if (last_repair_rejoin_index_.has_value()) {
+      const auto bump = static_cast<std::size_t>(std::max(1, min_repair_progress_indices_));
+      const auto advanced = std::min(
+        active_path_.poses.size() - 1, *last_repair_rejoin_index_ + bump);
+      search_start = std::max(search_start, advanced);
+    }
+
+    const auto rejoin = findClearRejoinIndexWithPlannerCostmap(
+      active_path_, search_start, *latest_costmap_, latest_planner_costmap_.get(), settings_);
+    if (!rejoin.has_value()) {
+      RCLCPP_WARN(get_logger(), "blocked replay path but no clear rejoin point found");
+      publishStop();
+      return;
+    }
+
+    RCLCPP_WARN(
+      get_logger(), "blocked replay path index=%zu reason=%s rejoin_index=%zu",
+      result.path_index, result.reason.c_str(), *rejoin);
+    requestBypass(nearest, *rejoin);
+  }
+
+  void publishAndMaybeFollow(const nav_msgs::msg::Path & path, const std::string & label)
+  {
+    if (path.poses.empty()) {
+      return;
+    }
+
+    nav_msgs::msg::Path stamped_path = path;
+    stamped_path.header.stamp = now();
+    for (auto & pose : stamped_path.poses) {
+      pose.header.stamp = stamped_path.header.stamp;
+      if (pose.header.frame_id.empty()) {
+        pose.header.frame_id = stamped_path.header.frame_id;
+      }
+    }
+    patched_path_pub_->publish(stamped_path);
+
+    if (!execute_follow_path_) {
+      RCLCPP_INFO(
+        get_logger(), "published %s path poses=%zu without FollowPath execution",
+        label.c_str(), stamped_path.poses.size());
+      return;
+    }
+
+    sendFollowPath(stamped_path, label);
+  }
+
+  void sendFollowPath(const nav_msgs::msg::Path & path, const std::string & label)
+  {
+    if (!follow_client_->wait_for_action_server(100ms)) {
+      RCLCPP_WARN(get_logger(), "FollowPath action server not ready");
+      return;
+    }
+
+    if (active_follow_goal_handle_) {
+      follow_client_->async_cancel_goal(active_follow_goal_handle_);
+      active_follow_goal_handle_.reset();
+    }
+
+    FollowPath::Goal goal;
+    goal.path = path;
+    goal.controller_id = get_parameter("controller_id").as_string();
+    goal.goal_checker_id = get_parameter("goal_checker_id").as_string();
+
+    auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
+    options.goal_response_callback =
+      [this, label](FollowGoalHandle::SharedPtr goal_handle) {
+        if (!goal_handle) {
+          RCLCPP_WARN(get_logger(), "FollowPath %s rejected", label.c_str());
+          return;
+        }
+        active_follow_goal_handle_ = goal_handle;
+        RCLCPP_INFO(get_logger(), "FollowPath %s accepted", label.c_str());
+      };
+    options.result_callback =
+      [this, label](const FollowGoalHandle::WrappedResult & result) {
+        active_follow_goal_handle_.reset();
+        RCLCPP_INFO(
+          get_logger(), "FollowPath %s finished code=%d",
+          label.c_str(), static_cast<int>(result.code));
+      };
+
+    follow_client_->async_send_goal(goal, options);
+    RCLCPP_INFO(get_logger(), "sent FollowPath %s poses=%zu", label.c_str(), path.poses.size());
+  }
+
+  void requestBypass(std::size_t nearest_index, std::size_t rejoin_index)
+  {
+    const auto start = currentPose();
+    if (!start.has_value() || rejoin_index >= active_path_.poses.size()) {
+      return;
+    }
+
+    if (!planner_client_->wait_for_action_server(100ms)) {
+      RCLCPP_WARN(get_logger(), "ComputePathToPose action server not ready");
+      publishStop();
+      return;
+    }
+
+    repair_in_flight_ = true;
+    last_repair_request_time_ = get_clock()->now();
+    last_repair_nearest_index_ = nearest_index;
+    last_repair_rejoin_index_ = rejoin_index;
+    last_repair_planner_costmap_generation_ = planner_costmap_generation_;
+    waiting_for_planner_costmap_update_ = false;
+
+    ComputePathToPose::Goal goal;
+    goal.start = *start;
+    goal.goal = active_path_.poses[rejoin_index];
+    goal.planner_id = get_parameter("planner_id").as_string();
+    goal.use_start = true;
+
+    auto options = rclcpp_action::Client<ComputePathToPose>::SendGoalOptions();
+    options.goal_response_callback =
+      [this](ComputeGoalHandle::SharedPtr goal_handle) {
+        if (!goal_handle) {
+          repair_in_flight_ = false;
+          RCLCPP_WARN(get_logger(), "bypass planning goal rejected");
+        }
+      };
+    options.result_callback =
+      [this, rejoin_index](const ComputeGoalHandle::WrappedResult & result) {
+        repair_in_flight_ = false;
+        if (result.code != rclcpp_action::ResultCode::SUCCEEDED ||
+          result.result->path.poses.empty())
+        {
+          RCLCPP_WARN(get_logger(), "bypass planning failed code=%d", static_cast<int>(result.code));
+          publishStop();
+          return;
+        }
+
+        const auto patched = stitchPaths(result.result->path, active_path_, rejoin_index);
+        if (!patchedPathIsClear(patched)) {
+          reject_path_pub_->publish(patched);
+          retry_without_planner_costmap_update_ = true;
+          waiting_for_planner_costmap_update_ = false;
+          RCLCPP_WARN(
+            get_logger(),
+            "planned bypass still blocked in local costmap; stopping and retrying from a later rejoin point");
+          publishStop();
+          return;
+        }
+
+        active_path_ = patched;
+        sent_original_for_current_path_ = true;
+        waiting_for_planner_costmap_update_ = false;
+        retry_without_planner_costmap_update_ = false;
+        publishAndMaybeFollow(patched, "patched_bypass");
+      };
+
+    planner_client_->async_send_goal(goal, options);
+    RCLCPP_INFO(get_logger(), "requested bypass to rejoin_index=%zu", rejoin_index);
+  }
+
+  bool patchedPathIsClear(const nav_msgs::msg::Path & patched) const
+  {
+    if (!latest_costmap_) {
+      return false;
+    }
+
+    const auto pose = currentPose();
+    if (!pose.has_value()) {
+      return false;
+    }
+
+    const auto nearest = nearestPathIndex(patched, *pose);
+    const auto result = checkPathAhead(patched, nearest, *latest_costmap_, settings_);
+    return !result.blocked;
+  }
+
+  void publishStop()
+  {
+    if (!publish_zero_on_blocked_) {
+      return;
+    }
+    geometry_msgs::msg::Twist stop;
+    cmd_vel_pub_->publish(stop);
+  }
+
+  GuardSettings settings_;
+  double repair_cooldown_sec_{1.0};
+  double blocked_retry_wait_sec_{0.8};
+  int min_repair_progress_indices_{5};
+  bool publish_zero_on_blocked_{true};
+  bool execute_follow_path_{true};
+  bool wait_for_planner_costmap_update_{true};
+  bool retry_without_planner_costmap_update_{false};
+  bool repair_in_flight_{false};
+  bool sent_original_for_current_path_{false};
+  bool waiting_for_planner_costmap_update_{false};
+  rclcpp::Time last_repair_request_time_{0, 0, RCL_ROS_TIME};
+  std::optional<std::size_t> last_repair_nearest_index_;
+  std::optional<std::size_t> last_repair_rejoin_index_;
+  std::uint64_t local_costmap_generation_{0};
+  std::uint64_t planner_costmap_generation_{0};
+  std::uint64_t last_repair_planner_costmap_generation_{0};
+  std::uint64_t waiting_planner_costmap_generation_{0};
+
+  nav_msgs::msg::Path active_path_;
+  nav_msgs::msg::OccupancyGrid::SharedPtr latest_costmap_;
+  nav_msgs::msg::OccupancyGrid::SharedPtr latest_planner_costmap_;
+  nav_msgs::msg::Odometry::SharedPtr latest_odom_;
+  FollowGoalHandle::SharedPtr active_follow_goal_handle_;
+
+  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_sub_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr planner_costmap_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr patched_path_pub_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr reject_path_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+  rclcpp_action::Client<ComputePathToPose>::SharedPtr planner_client_;
+  rclcpp_action::Client<FollowPath>::SharedPtr follow_client_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}  // namespace obstacle_nav2
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<obstacle_nav2::TrajectoryGuardNode>());
+  rclcpp::shutdown();
+  return 0;
+}
--- a/src/navigation/obstacle_nav2/test/test_nav2_profile_loader.cpp
+++ b/src/navigation/obstacle_nav2/test/test_nav2_profile_loader.cpp
@@ -0,0 +1,127 @@
+#include <gtest/gtest.h>
+
+#include <fstream>
+#include <set>
+#include <string>
+
+#include "obstacle_nav2/nav2_profile_loader.hpp"
+
+namespace
+{
+
+std::string writeTempProfile(const std::string & yaml)
+{
+  const auto path = "/tmp/nav2_profile_loader_test.yaml";
+  std::ofstream out(path);
+  out << yaml;
+  out.close();
+  return path;
+}
+
+}  // namespace
+
+TEST(Nav2ProfileLoaderTest, LoadsScalarAndArrayParameters)
+{
+  const auto path = writeTempProfile(R"(
+nodes:
+  /controller_server:
+    FollowPath.vx_max: 0.35
+    FollowPath.vx_min: -0.10
+    FollowPath.wz_max: 1.2
+    FollowPath.AckermannConstraints.min_turning_r: 0.75
+  /velocity_smoother:
+    max_velocity: [0.35, 0.0, 1.2]
+    min_velocity: [-0.10, 0.0, -1.2]
+)");
+
+  const auto profile = obstacle_nav2::loadNav2Profile(path);
+
+  ASSERT_EQ(profile.nodes.size(), 2u);
+  EXPECT_EQ(profile.nodes[0].node_name, "/controller_server");
+  ASSERT_EQ(profile.nodes[0].parameters.size(), 4u);
+  EXPECT_EQ(profile.nodes[0].parameters[0].get_name(), "FollowPath.vx_max");
+  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[0].as_double(), 0.35);
+  EXPECT_EQ(
+    profile.nodes[0].parameters[3].get_name(),
+    "FollowPath.AckermannConstraints.min_turning_r");
+  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[3].as_double(), 0.75);
+
+  EXPECT_EQ(profile.nodes[1].node_name, "/velocity_smoother");
+  ASSERT_EQ(profile.nodes[1].parameters.size(), 2u);
+  const auto max_velocity = profile.nodes[1].parameters[0].as_double_array();
+  ASSERT_EQ(max_velocity.size(), 3u);
+  EXPECT_DOUBLE_EQ(max_velocity[0], 0.35);
+  EXPECT_DOUBLE_EQ(max_velocity[2], 1.2);
+}
+
+TEST(Nav2ProfileLoaderTest, LoadsNativeNav2RosParametersFile)
+{
+  const auto path = writeTempProfile(R"(
+bt_navigator:
+  ros__parameters:
+    default_server_timeout: 20
+controller_server:
+  ros__parameters:
+    controller_frequency: 20.0
+    FollowPath:
+      vx_max: 0.35
+      wz_max: 1.2
+      motion_model: "Ackermann"
+      critics: ["ConstraintCritic", "CostCritic"]
+      AckermannConstraints:
+        min_turning_r: 0.75
+velocity_smoother:
+  ros__parameters:
+    max_velocity: [0.35, 0.0, 1.2]
+    min_velocity: [-0.10, 0.0, -1.2]
+)");
+
+  const auto profile = obstacle_nav2::loadNav2Profile(path);
+
+  ASSERT_EQ(profile.nodes.size(), 2u);
+  EXPECT_EQ(profile.nodes[0].node_name, "/controller_server");
+  ASSERT_EQ(profile.nodes[0].parameters.size(), 6u);
+  EXPECT_EQ(profile.nodes[0].parameters[1].get_name(), "FollowPath.vx_max");
+  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[1].as_double(), 0.35);
+  EXPECT_EQ(
+    profile.nodes[0].parameters[5].get_name(),
+    "FollowPath.AckermannConstraints.min_turning_r");
+  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[5].as_double(), 0.75);
+
+  EXPECT_EQ(profile.nodes[1].node_name, "/velocity_smoother");
+  ASSERT_EQ(profile.nodes[1].parameters.size(), 2u);
+  const auto max_velocity = profile.nodes[1].parameters[0].as_double_array();
+  ASSERT_EQ(max_velocity.size(), 3u);
+  EXPECT_DOUBLE_EQ(max_velocity[2], 1.2);
+}
+
+TEST(Nav2ProfileLoaderTest, RejectsProfileWithoutNodesMap)
+{
+  const auto path = writeTempProfile(R"(
+controller_server:
+  FollowPath.vx_max: 0.35
+)");
+
+  EXPECT_THROW(
+    obstacle_nav2::loadNav2Profile(path),
+    std::runtime_error);
+}
+
+TEST(Nav2ProfileLoaderTest, FiltersParametersByDeclaredNames)
+{
+  const std::vector<rclcpp::Parameter> parameters{
+    rclcpp::Parameter("FollowPath.vx_max", 0.35),
+    rclcpp::Parameter("FollowPath.CostCritic.trajectory_point_step", 2),
+    rclcpp::Parameter("max_velocity", std::vector<double>{0.35, 0.0, 1.2}),
+  };
+  const std::set<std::string> declared_names{
+    "FollowPath.vx_max",
+    "max_velocity",
+  };
+
+  const auto filtered = obstacle_nav2::filterDeclaredParameters(parameters, declared_names);
+
+  ASSERT_EQ(filtered.size(), 2u);
+  EXPECT_EQ(filtered[0].get_name(), "FollowPath.vx_max");
+  EXPECT_EQ(filtered[1].get_name(), "max_velocity");
+}
--- a/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp
+++ b/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp
@@ -186,6 +186,26 @@ TEST(ObstacleArrayLayerTest, EmptySnapshotClearsPreviousObstacle)
  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE);
 }

+TEST(ObstacleArrayLayerTest, EmptySnapshotCanBeIgnoredToPreservePreviousObstacle)
+{
+  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);
+  const std::vector<ObstacleArrayLayer::CircleObstacle> occupied{{0.0, 0.0, 0.1}};
+
+  const auto occupied_bounds = ObstacleArrayLayer::applySnapshot(
+    grid, occupied, kResolution, kOriginX, kOriginY);
+
+  unsigned int mx, my;
+  ASSERT_TRUE(grid.worldToMap(0.0, 0.0, mx, my));
+  EXPECT_TRUE(occupied_bounds.valid);
+  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  const auto retained = ObstacleArrayLayer::applySnapshotIfNotEmpty(
+    grid, {}, kResolution, kOriginX, kOriginY);
+
+  EXPECT_FALSE(retained.has_value());
+  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
+}
+
 TEST(ObstacleArrayLayerTest, TransformSnapshotUsesMessageTimestamp)
 {
  auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
--- a/src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp
+++ b/src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp
@@ -0,0 +1,190 @@
+#include <gtest/gtest.h>
+
+#include "obstacle_nav2/trajectory_guard.hpp"
+
+namespace
+{
+
+geometry_msgs::msg::PoseStamped pose(double x, double y)
+{
+  geometry_msgs::msg::PoseStamped p;
+  p.header.frame_id = "odom";
+  p.pose.position.x = x;
+  p.pose.position.y = y;
+  p.pose.orientation.w = 1.0;
+  return p;
+}
+
+nav_msgs::msg::Path straightPath()
+{
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "odom";
+  for (int i = 0; i <= 10; ++i) {
+    path.poses.push_back(pose(0.2 * i, 0.0));
+  }
+  return path;
+}
+
+nav_msgs::msg::OccupancyGrid gridWithOrigin(double origin_x, double origin_y)
+{
+  nav_msgs::msg::OccupancyGrid grid;
+  grid.header.frame_id = "odom";
+  grid.info.resolution = 0.1;
+  grid.info.width = 40;
+  grid.info.height = 30;
+  grid.info.origin.position.x = origin_x;
+  grid.info.origin.position.y = origin_y;
+  grid.data.assign(grid.info.width * grid.info.height, 0);
+  return grid;
+}
+
+void markOccupied(nav_msgs::msg::OccupancyGrid & grid, int mx, int my)
+{
+  ASSERT_GE(mx, 0);
+  ASSERT_GE(my, 0);
+  ASSERT_LT(mx, static_cast<int>(grid.info.width));
+  ASSERT_LT(my, static_cast<int>(grid.info.height));
+  grid.data[my * grid.info.width + mx] = 100;
+}
+
+}  // namespace
+
+TEST(TrajectoryGuard, NearestPathIndexFindsClosestPose)
+{
+  const auto path = straightPath();
+  EXPECT_EQ(obstacle_nav2::nearestPathIndex(path, pose(0.43, 0.02)), 2u);
+}
+
+TEST(TrajectoryGuard, AdvanceByDistanceStopsAtRequestedArcLength)
+{
+  const auto path = straightPath();
+  EXPECT_EQ(obstacle_nav2::advanceByDistance(path, 0, 0.55), 3u);
+}
+
+TEST(TrajectoryGuard, SlicePathIncludesEndIndex)
+{
+  const auto path = straightPath();
+  const auto sliced = obstacle_nav2::slicePath(path, 2, 4);
+  ASSERT_EQ(sliced.poses.size(), 3u);
+  EXPECT_DOUBLE_EQ(sliced.poses.front().pose.position.x, 0.4);
+  EXPECT_DOUBLE_EQ(sliced.poses.back().pose.position.x, 0.8);
+}
+
+TEST(TrajectoryGuard, StitchPathsAppendsOriginalFromRejoinIndex)
+{
+  const auto original = straightPath();
+  nav_msgs::msg::Path bypass;
+  bypass.header.frame_id = "odom";
+  bypass.poses.push_back(pose(0.0, 0.0));
+  bypass.poses.push_back(pose(0.5, 0.3));
+
+  const auto stitched = obstacle_nav2::stitchPaths(bypass, original, 4);
+  ASSERT_EQ(stitched.poses.size(), 9u);
+  EXPECT_DOUBLE_EQ(stitched.poses[0].pose.position.y, 0.0);
+  EXPECT_DOUBLE_EQ(stitched.poses[1].pose.position.y, 0.3);
+  EXPECT_DOUBLE_EQ(stitched.poses[2].pose.position.x, 0.8);
+}
+
+TEST(TrajectoryGuard, CheckPathAheadReportsOccupiedCell)
+{
+  auto path = straightPath();
+  auto grid = gridWithOrigin(-1.0, -1.0);
+  markOccupied(grid, 15, 10);
+
+  obstacle_nav2::GuardSettings settings;
+  settings.lookahead_distance = 2.0;
+  settings.occupied_threshold = 50;
+  settings.footprint_half_length = 0.01;
+  settings.footprint_half_width = 0.01;
+  settings.footprint_padding = 0.0;
+
+  const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
+  EXPECT_TRUE(result.blocked);
+  EXPECT_EQ(result.reason, "occupied");
+}
+
+TEST(TrajectoryGuard, CheckPathAheadKeepsClearPathUnblocked)
+{
+  auto path = straightPath();
+  auto grid = gridWithOrigin(-1.0, -1.0);
+
+  obstacle_nav2::GuardSettings settings;
+  settings.lookahead_distance = 2.0;
+  settings.occupied_threshold = 50;
+  settings.footprint_half_length = 0.01;
+  settings.footprint_half_width = 0.01;
+  settings.footprint_padding = 0.0;
+
+  const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
+  EXPECT_FALSE(result.blocked);
+  EXPECT_EQ(result.reason, "clear");
+}
+
+TEST(TrajectoryGuard, FindClearRejoinIndexSkipsBlockedArea)
+{
+  auto path = straightPath();
+  auto grid = gridWithOrigin(-1.0, -1.0);
+  for (int mx = 13; mx <= 18; ++mx) {
+    markOccupied(grid, mx, 10);
+  }
+
+  obstacle_nav2::GuardSettings settings;
+  settings.rejoin_min_distance = 0.5;
+  settings.rejoin_max_distance = 2.0;
+  settings.occupied_threshold = 50;
+  settings.footprint_half_length = 0.01;
+  settings.footprint_half_width = 0.01;
+  settings.footprint_padding = 0.0;
+
+  const auto rejoin = obstacle_nav2::findClearRejoinIndex(path, 0, grid, settings);
+  ASSERT_TRUE(rejoin.has_value());
+  EXPECT_GE(*rejoin, 5u);
+}
+
+TEST(TrajectoryGuard, FindClearRejoinIndexCanUsePlannerCostmapBeyondLocalWindow)
+{
+  auto path = straightPath();
+  auto local_grid = gridWithOrigin(-0.5, -1.0);
+  local_grid.info.width = 15;
+  local_grid.data.assign(local_grid.info.width * local_grid.info.height, 0);
+
+  auto planner_grid = gridWithOrigin(-1.0, -1.0);
+  planner_grid.info.width = 80;
+  planner_grid.data.assign(planner_grid.info.width * planner_grid.info.height, 0);
+
+  obstacle_nav2::GuardSettings settings;
+  settings.rejoin_min_distance = 1.5;
+  settings.rejoin_max_distance = 3.0;
+  settings.occupied_threshold = 50;
+  settings.treat_unknown_as_occupied = true;
+  settings.footprint_half_length = 0.01;
+  settings.footprint_half_width = 0.01;
+  settings.footprint_padding = 0.0;
+
+  EXPECT_FALSE(obstacle_nav2::findClearRejoinIndex(path, 0, local_grid, settings).has_value());
+
+  const auto rejoin = obstacle_nav2::findClearRejoinIndexWithPlannerCostmap(
+    path, 0, local_grid, &planner_grid, settings);
+  ASSERT_TRUE(rejoin.has_value());
+  EXPECT_GE(*rejoin, 8u);
+}
+
+TEST(TrajectoryGuard, RetryBlockedRepairWaitsForCostmapUpdateAndRetryDelay)
+{
+  const std::optional<std::size_t> last_repair_index = 10u;
+
+  EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
+    last_repair_index, 11u, 5, 0.8, 1.0, true));
+  EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
+    last_repair_index, 11u, 5, 1.2, 1.0, false));
+  EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
+    last_repair_index, 11u, 5, 1.2, 1.0, true));
+}
+
+TEST(TrajectoryGuard, RetryBlockedRepairAllowsProgressedPathImmediately)
+{
+  const std::optional<std::size_t> last_repair_index = 10u;
+
+  EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
+    last_repair_index, 16u, 5, 0.1, 1.0, false));
+}
--- a/src/obstacle_scanner/src/obstacle_scanner_node.cpp
+++ b/src/obstacle_scanner/src/obstacle_scanner_node.cpp
@@ -63,7 +63,7 @@ public:
      1, static_cast<int>(declare_parameter<int>("debug_info_stride", 1)));

    obstacles_pub_ = create_publisher<obstacle_scanner::msg::ObstacleArray>(
-      "/obstacles", 10);
+      "/obstacles", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());

    if (debug_) {
      debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
--- a/src/origincar_base/CMakeLists.txt
+++ b/src/origincar_base/CMakeLists.txt
@@ -46,6 +46,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Position.msg"
 )
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
@@ -64,6 +65,12 @@ set(origincar_base_node_SRCS
  src/origincar_base.cpp
 )

+add_library(origincar_base_log STATIC src/log.cpp)
+target_include_directories(origincar_base_log PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 add_executable(origincar_base_node src/origincar_base.cpp)
 ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description)

@@ -80,7 +87,7 @@ target_include_directories(wall_kalman_filter PUBLIC
 )
 target_link_libraries(wall_kalman_filter wall_fit_core)

-target_link_libraries(origincar_base_node wall_kalman_filter wall_fit_core)
+target_link_libraries(origincar_base_node origincar_base_log wall_kalman_filter wall_fit_core)

 add_executable(wall_fit_calibrator src/wall_fit_calibrator.cpp)
 target_link_libraries(wall_fit_calibrator wall_fit_core)
@@ -102,6 +109,19 @@ if(BUILD_TESTING)
  add_executable(wall_kalman_filter_test test/wall_kalman_filter_test.cpp)
  target_link_libraries(wall_kalman_filter_test wall_kalman_filter)
  add_test(NAME wall_kalman_filter_test COMMAND wall_kalman_filter_test)
+
+  add_executable(scan_odom_timing_logger_test test/scan_odom_timing_logger_test.cpp)
+  target_link_libraries(scan_odom_timing_logger_test origincar_base_log)
+  add_test(NAME scan_odom_timing_logger_test COMMAND scan_odom_timing_logger_test)
+
+  ament_add_gtest(command_gate_test test/command_gate_test.cpp)
+  target_include_directories(command_gate_test PRIVATE include)
+
+  ament_add_gtest(command_frame_test test/command_frame_test.cpp)
+  target_include_directories(command_frame_test PRIVATE include)
+
+  ament_add_gtest(serial_frame_parser_test test/serial_frame_parser_test.cpp)
+  target_include_directories(serial_frame_parser_test PRIVATE include)
 endif()

 #add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)
--- a/src/origincar_base/config/ekf.yaml
+++ b/src/origincar_base/config/ekf.yaml
@@ -1,7 +1,7 @@
 ### ekf config file ###
 ekf_filter_node:
    ros__parameters:
-        frequency: 20.0
+        frequency: 50.0
        sensor_timeout: 2.0
        two_d_mode: true
        transform_time_offset: 0.0
--- a/src/origincar_base/include/origincar_base/command_frame.hpp
+++ b/src/origincar_base/include/origincar_base/command_frame.hpp
@@ -0,0 +1,43 @@
+#ifndef ORIGINCAR_BASE__COMMAND_FRAME_HPP_
+#define ORIGINCAR_BASE__COMMAND_FRAME_HPP_
+
+#include <array>
+#include <cstddef>
+#include <cstdint>
+
+#include "origincar_base/command_gate.hpp"
+
+namespace origincar_base_core
+{
+
+inline void writeInt16(std::array<uint8_t, 11> * frame, std::size_t high_index, int16_t value)
+{
+  const auto bits = static_cast<uint16_t>(value);
+  (*frame)[high_index] = static_cast<uint8_t>(bits >> 8);
+  (*frame)[high_index + 1] = static_cast<uint8_t>(bits & 0xFF);
+}
+
+inline std::array<uint8_t, 11> encodeCommandFrame(const Command & command)
+{
+  std::array<uint8_t, 11> frame{};
+  frame[0] = 0x7B;
+
+  writeInt16(&frame, 3, static_cast<int16_t>(command.linear_x * 1000.0));
+  if (command.ackermann) {
+    writeInt16(&frame, 5, 0);
+    writeInt16(&frame, 7, static_cast<int16_t>(command.steering_angle * 500.0));
+  } else {
+    writeInt16(&frame, 5, static_cast<int16_t>(command.linear_y * 1000.0));
+    writeInt16(&frame, 7, static_cast<int16_t>(command.angular_z * 1000.0));
+  }
+
+  for (std::size_t index = 0; index < 9; ++index) {
+    frame[9] ^= frame[index];
+  }
+  frame[10] = 0x7D;
+  return frame;
+}
+
+}  // namespace origincar_base_core
+
+#endif  // ORIGINCAR_BASE__COMMAND_FRAME_HPP_
--- a/src/origincar_base/include/origincar_base/command_gate.hpp
+++ b/src/origincar_base/include/origincar_base/command_gate.hpp
@@ -0,0 +1,63 @@
+#ifndef ORIGINCAR_BASE__COMMAND_GATE_HPP_
+#define ORIGINCAR_BASE__COMMAND_GATE_HPP_
+
+#include <chrono>
+#include <mutex>
+
+namespace origincar_base_core
+{
+
+struct Command
+{
+  double linear_x{0.0};
+  double linear_y{0.0};
+  double angular_z{0.0};
+  double steering_angle{0.0};
+  bool ackermann{false};
+};
+
+inline bool operator==(const Command & lhs, const Command & rhs)
+{
+  return lhs.linear_x == rhs.linear_x &&
+         lhs.linear_y == rhs.linear_y &&
+         lhs.angular_z == rhs.angular_z &&
+         lhs.steering_angle == rhs.steering_angle &&
+         lhs.ackermann == rhs.ackermann;
+}
+
+class CommandGate
+{
+public:
+  explicit CommandGate(std::chrono::milliseconds timeout)
+  : timeout_(timeout)
+  {
+  }
+
+  void update(const Command & command, std::chrono::steady_clock::time_point now)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    latest_command_ = command;
+    last_command_time_ = now;
+    has_command_ = true;
+  }
+
+  Command commandAt(std::chrono::steady_clock::time_point now) const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!has_command_ || now - last_command_time_ >= timeout_) {
+      return {};
+    }
+    return latest_command_;
+  }
+
+private:
+  const std::chrono::milliseconds timeout_;
+  mutable std::mutex mutex_;
+  Command latest_command_{};
+  std::chrono::steady_clock::time_point last_command_time_{};
+  bool has_command_{false};
+};
+
+}  // namespace origincar_base_core
+
+#endif  // ORIGINCAR_BASE__COMMAND_GATE_HPP_
--- a/src/origincar_base/include/origincar_base/log.hpp
+++ b/src/origincar_base/include/origincar_base/log.hpp
@@ -0,0 +1,59 @@
+#ifndef ORIGINCAR_BASE_LOG_HPP_
+#define ORIGINCAR_BASE_LOG_HPP_
+
+#include <chrono>
+#include <cstdint>
+#include <fstream>
+#include <limits>
+#include <string>
+#include <unordered_map>
+
+namespace origincar_base_logging
+{
+class ScanOdomTimingLogger
+{
+public:
+  using Clock = std::chrono::steady_clock;
+  using TimePoint = Clock::time_point;
+
+  explicit ScanOdomTimingLogger(
+      const std::string &node_name,
+      const std::string &base_log_dir = "/home/sunrise/yiliao_ws/running_logs",
+      TimePoint start_time = Clock::now());
+
+  uint64_t startFrame(TimePoint now = Clock::now());
+  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;
+
+private:
+  struct Stats
+  {
+    uint64_t count{0};
+    double total_ms{0.0};
+    double min_ms{std::numeric_limits<double>::max()};
+    double max_ms{0.0};
+    double last_ms{0.0};
+  };
+
+  std::string makeLogPath(const std::string &node_name, const std::string &base_log_dir) const;
+  void writeFinishedFrame(uint64_t frame_id, double duration_ms);
+  void writeCanceledFrame(uint64_t frame_id, double elapsed_ms, const std::string &reason);
+  std::string formatStatsLine(const std::string &prefix) const;
+
+  std::unordered_map<uint64_t, TimePoint> active_frames_;
+  uint64_t next_frame_id_{1};
+  Stats stats_;
+  TimePoint last_summary_time_;
+  std::string log_path_;
+  std::ofstream log_file_;
+  bool print_to_terminal_{false};
+};
+}  // namespace origincar_base_logging
+
+#endif  // ORIGINCAR_BASE_LOG_HPP_
--- a/src/origincar_base/include/origincar_base/origincar_base.h
+++ b/src/origincar_base/include/origincar_base/origincar_base.h
@@ -1,13 +1,19 @@
 #ifndef _ORIGINCAR_BASE_H_
 #define _ORIGINCAR_BASE_H_

+#include <atomic>
+#include <chrono>
 #include <memory>
 #include <inttypes.h>
 #include <array>
+#include <cstdint>
 #include <mutex>
 #include <vector>
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "origincar_base/log.hpp"
+#include "origincar_base/command_gate.hpp"
+#include "origincar_base/serial_frame_parser.hpp"
 #include "origincar_base/wall_fit_core.hpp"
 #include "origincar_base/wall_kalman_filter.hpp"
 #include <csignal>
@@ -151,6 +157,11 @@ private:
 	auto createQuaternionMsgFromYaw(double yaw);
 	void Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
 	void Apply_Wall_Update();
+	void Print_Timing_Log_If_Due();
+	void Sensor_Receive_Loop();
+	void Control_Timer_Callback();
+	void Tx_Timer_Callback();
+	void Send_Command(const origincar_base_core::Command & command);

 	bool Get_Sensor_Data();
 	unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode);
@@ -212,6 +223,7 @@ private:
 	rclcpp::TimerBase::SharedPtr test_timer;

 	rclcpp::TimerBase::SharedPtr odom_timer;
+	rclcpp::TimerBase::SharedPtr tx_timer;
 	rclcpp::TimerBase::SharedPtr imu_timer;
 	rclcpp::TimerBase::SharedPtr voltage_timer;

@@ -225,10 +237,15 @@ 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;
 	int serial_baud_rate;
+	int cmd_watchdog_timeout_ms_;
+	int tx_period_ms_;
+	int serial_read_timeout_ms_;
+	int control_period_ms_;
 	RECEIVE_DATA Receive_Data;
 	SEND_DATA Send_Data;

@@ -252,10 +269,23 @@ private:
 	::origincar_wall::WallFitConfig wall_fit_config_;
 	std::unique_ptr<::origincar_wall::WallKalmanFilter> wall_filter_;
 	::origincar_wall::Pose2D laser_pose_;
+	::origincar_base_logging::ScanOdomTimingLogger scan_odom_timing_logger_;
 	std::mutex wall_scan_mutex_;
 	std::vector<::origincar_wall::WallPoint> latest_scan_points_;
+	uint64_t latest_scan_timing_frame_id_;
+	uint64_t pending_odom_timing_frame_id_;
 	bool has_latest_scan_;
 	bool latest_scan_consumed_;
+	bool has_latest_scan_timing_frame_;
+	bool has_pending_odom_timing_frame_;
+	std::unique_ptr<origincar_base_core::CommandGate> command_gate_;
+	origincar_base_core::SerialFrameParser serial_frame_parser_;
+	std::mutex serial_mutex_;
+	std::mutex sensor_data_mutex_;
+	std::thread sensor_thread_;
+	std::atomic<bool> sensor_thread_stop_{true};
+	std::atomic<uint64_t> sensor_sequence_{0};
+	uint64_t last_processed_sensor_sequence_{0};
 };

 #endif //_ORIGINCAR_BASE_H_
--- a/src/origincar_base/include/origincar_base/serial_frame_parser.hpp
+++ b/src/origincar_base/include/origincar_base/serial_frame_parser.hpp
@@ -0,0 +1,56 @@
+#ifndef ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_
+#define ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_
+
+#include <algorithm>
+#include <array>
+#include <cstddef>
+#include <cstdint>
+#include <vector>
+
+namespace origincar_base_core
+{
+
+class SerialFrameParser
+{
+public:
+  static constexpr std::size_t kFrameSize = 24;
+
+  static constexpr uint8_t frameHeader() {return 0x7B;}
+  static constexpr uint8_t frameTail() {return 0x7D;}
+
+  void append(const uint8_t * data, std::size_t size)
+  {
+    buffer_.insert(buffer_.end(), data, data + size);
+  }
+
+  bool popFrame(std::array<uint8_t, kFrameSize> * frame)
+  {
+    while (!buffer_.empty()) {
+      const auto header = std::find(buffer_.begin(), buffer_.end(), frameHeader());
+      if (header == buffer_.end()) {
+        buffer_.clear();
+        return false;
+      }
+      buffer_.erase(buffer_.begin(), header);
+      if (buffer_.size() < kFrameSize) {
+        return false;
+      }
+      if (buffer_[kFrameSize - 1] != frameTail()) {
+        buffer_.erase(buffer_.begin());
+        continue;
+      }
+
+      std::copy_n(buffer_.begin(), kFrameSize, frame->begin());
+      buffer_.erase(buffer_.begin(), buffer_.begin() + kFrameSize);
+      return true;
+    }
+    return false;
+  }
+
+private:
+  std::vector<uint8_t> buffer_;
+};
+
+}  // namespace origincar_base_core
+
+#endif  // ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_
--- a/src/origincar_base/include/origincar_base/wall_kalman_filter.hpp
+++ b/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_
--- a/src/origincar_base/launch/pycache/base_serial.launch.cpython-310.pyc
+++ b/src/origincar_base/launch/pycache/base_serial.launch.cpython-310.pyc
--- a/src/origincar_base/launch/base_serial.launch.py
+++ b/src/origincar_base/launch/base_serial.launch.py
@@ -5,7 +5,11 @@ import launch_ros.actions
 def generate_launch_description():
    robot_parameters = [
        {'usart_port_name': '/dev/ttyACM0',
-         'serial_baud_rate': 115200,
+         'serial_baud_rate': 921600,
+         'serial_read_timeout_ms': 20,
+         'tx_period_ms': 20,
+         'cmd_watchdog_timeout_ms': 500,
+         'control_period_ms': 50,
         'robot_frame_id': 'base_footprint',
         'odom_frame_id': 'odom',
         'combined_odom_topic': 'odom_combined',
--- a/src/origincar_base/launch/base_serial.launch.py.bak
+++ b/src/origincar_base/launch/base_serial.launch.py.bak
@@ -9,7 +9,7 @@ def generate_launch_description():

    robot_parameters = [
        {'usart_port_name': '/dev/ttyACM0',
-         'serial_baud_rate': 115200,
+         'serial_baud_rate': 921600,
         'robot_frame_id': 'base_link',
         'odom_frame_id': 'odom',
         'cmd_vel': 'cmd_vel',
--- a/src/origincar_base/package.xml
+++ b/src/origincar_base/package.xml
@@ -11,6 +11,7 @@

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
  <build_export_depend>tf2_geometry_msgs</build_export_depend>


--- a/src/origincar_base/src/log.cpp
+++ b/src/origincar_base/src/log.cpp
@@ -0,0 +1,242 @@
+#include "origincar_base/log.hpp"
+
+#include <cerrno>
+#include <cctype>
+#include <cmath>
+#include <ctime>
+#include <iomanip>
+#include <sstream>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+namespace origincar_base_logging
+{
+namespace
+{
+std::string sanitizeNodeName(const std::string &node_name)
+{
+  std::string result;
+  for (char ch : node_name)
+  {
+    const unsigned char value = static_cast<unsigned char>(ch);
+    if (std::isalnum(value) || ch == '_' || ch == '-')
+    {
+      result.push_back(ch);
+    }
+    else if (!result.empty() && result.back() != '_')
+    {
+      result.push_back('_');
+    }
+  }
+  return result.empty() ? "node" : result;
+}
+
+bool makeDirectory(const std::string &path)
+{
+  if (path.empty())
+  {
+    return false;
+  }
+  if (::mkdir(path.c_str(), 0755) == 0 || errno == EEXIST)
+  {
+    return true;
+  }
+  return false;
+}
+
+bool makeDirectories(const std::string &path)
+{
+  if (path.empty())
+  {
+    return false;
+  }
+
+  std::string current;
+  for (std::size_t index = 0; index < path.size(); ++index)
+  {
+    current.push_back(path[index]);
+    if ((path[index] == '/' && current.size() > 1) || index + 1 == path.size())
+    {
+      if (current.size() > 1 && current.back() == '/')
+      {
+        current.pop_back();
+      }
+      if (!current.empty() && !makeDirectory(current))
+      {
+        return false;
+      }
+      if (index + 1 < path.size() && path[index] == '/')
+      {
+        current.push_back('/');
+      }
+    }
+  }
+  return true;
+}
+
+std::string formatSystemTime()
+{
+  const std::time_t now = std::time(nullptr);
+  std::tm local_time;
+  localtime_r(&now, &local_time);
+  std::ostringstream stream;
+  stream << std::put_time(&local_time, "%Y-%m-%d %H:%M:%S");
+  return stream.str();
+}
+
+std::string formatFileTimestamp()
+{
+  const std::time_t now = std::time(nullptr);
+  std::tm local_time;
+  localtime_r(&now, &local_time);
+  std::ostringstream stream;
+  stream << std::put_time(&local_time, "%Y%m%d_%H%M%S");
+  return stream.str();
+}
+
+double elapsedMs(ScanOdomTimingLogger::TimePoint start, ScanOdomTimingLogger::TimePoint end)
+{
+  return std::chrono::duration_cast<std::chrono::duration<double, std::milli>>(end - start).count();
+}
+}  // namespace
+
+ScanOdomTimingLogger::ScanOdomTimingLogger(
+    const std::string &node_name,
+    const std::string &base_log_dir,
+    TimePoint start_time)
+    : last_summary_time_(start_time),
+      log_path_(makeLogPath(node_name, base_log_dir)),
+      log_file_(log_path_.c_str(), std::ios::out | std::ios::app)
+{
+}
+
+uint64_t ScanOdomTimingLogger::startFrame(TimePoint now)
+{
+  const uint64_t frame_id = next_frame_id_++;
+  active_frames_[frame_id] = now;
+  return frame_id;
+}
+
+bool ScanOdomTimingLogger::finishFrame(uint64_t frame_id, TimePoint now)
+{
+  const auto frame = active_frames_.find(frame_id);
+  if (frame == active_frames_.end())
+  {
+    return false;
+  }
+
+  const double duration_ms = elapsedMs(frame->second, now);
+  active_frames_.erase(frame);
+  stats_.count += 1;
+  stats_.total_ms += duration_ms;
+  stats_.last_ms = duration_ms;
+  stats_.max_ms = std::max(stats_.max_ms, duration_ms);
+  stats_.min_ms = std::min(stats_.min_ms, duration_ms);
+  writeFinishedFrame(frame_id, duration_ms);
+  return true;
+}
+
+bool ScanOdomTimingLogger::cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now)
+{
+  const auto frame = active_frames_.find(frame_id);
+  if (frame == active_frames_.end())
+  {
+    return false;
+  }
+
+  const double elapsed_ms = elapsedMs(frame->second, now);
+  active_frames_.erase(frame);
+  writeCanceledFrame(frame_id, elapsed_ms, reason);
+  return true;
+}
+
+bool ScanOdomTimingLogger::makeSummaryLineIfDue(TimePoint now, std::string *line)
+{
+  if (now - last_summary_time_ < std::chrono::seconds(1))
+  {
+    return false;
+  }
+  last_summary_time_ = now;
+  if (line)
+  {
+    *line = formatStatsLine("scan_to_odom timing");
+  }
+  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_;
+}
+
+bool ScanOdomTimingLogger::isOpen() const
+{
+  return log_file_.is_open();
+}
+
+std::string ScanOdomTimingLogger::makeLogPath(
+    const std::string &node_name,
+    const std::string &base_log_dir) const
+{
+  const std::string node_dir = base_log_dir + "/" + sanitizeNodeName(node_name);
+  makeDirectories(node_dir);
+  return node_dir + "/" + formatFileTimestamp() + ".log";
+}
+
+void ScanOdomTimingLogger::writeFinishedFrame(uint64_t frame_id, double duration_ms)
+{
+  if (!log_file_.is_open())
+  {
+    return;
+  }
+  log_file_ << formatSystemTime()
+            << " frame_id=" << frame_id
+            << " status=finished"
+            << std::fixed << std::setprecision(3)
+            << " duration_ms=" << duration_ms
+            << " " << formatStatsLine("stats")
+            << '\n';
+  log_file_.flush();
+}
+
+void ScanOdomTimingLogger::writeCanceledFrame(uint64_t frame_id, double elapsed_ms, const std::string &reason)
+{
+  if (!log_file_.is_open())
+  {
+    return;
+  }
+  log_file_ << formatSystemTime()
+            << " frame_id=" << frame_id
+            << " status=canceled"
+            << " reason=" << reason
+            << std::fixed << std::setprecision(3)
+            << " elapsed_ms=" << elapsed_ms
+            << '\n';
+  log_file_.flush();
+}
+
+std::string ScanOdomTimingLogger::formatStatsLine(const std::string &prefix) const
+{
+  const double average_ms = stats_.count == 0 ? 0.0 : stats_.total_ms / static_cast<double>(stats_.count);
+  const double min_ms = stats_.count == 0 ? 0.0 : stats_.min_ms;
+  std::ostringstream stream;
+  stream << prefix
+         << " frames=" << stats_.count
+         << std::fixed << std::setprecision(3)
+         << " last_ms=" << stats_.last_ms
+         << " avg_ms=" << average_ms
+         << " max_ms=" << stats_.max_ms
+         << " min_ms=" << min_ms;
+  return stream.str();
+}
+}  // namespace origincar_base_logging
--- a/src/origincar_base/src/origincar_base.cpp
+++ b/src/origincar_base/src/origincar_base.cpp
@@ -1,10 +1,12 @@
 #include "origincar_base/origincar_base.h"
+#include "origincar_base/command_frame.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
 #include "origincar_msg/msg/data.hpp"
 #include "robot_localization/srv/set_pose.hpp"
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <algorithm>
+#include <chrono>
 #include <cmath>

 using std::placeholders::_1;
@@ -93,78 +95,26 @@ float origincar_base::Odom_Trans(uint8_t Data_High, uint8_t Data_Low)

 void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
 {
-  short transition;
-  std::cout << "linerx" << akm_ctl->drive.speed << std::endl;
-  std::cout << "angular" << akm_ctl->drive.steering_angle << std::endl;
-
-  Send_Data.tx[0] = FRAME_HEADER;
-  Send_Data.tx[1] = 0;
-  Send_Data.tx[2] = 0;
-
-  transition = 0;
-  transition = akm_ctl->drive.speed * 1000;
-  Send_Data.tx[4] = transition;
-  Send_Data.tx[3] = transition >> 8;
-
-  transition = 0;
-  transition = akm_ctl->drive.steering_angle * 1000 / 2;
-  Send_Data.tx[8] = transition;
-  Send_Data.tx[7] = transition >> 8;
-
-  Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
-  Send_Data.tx[10] = FRAME_TAIL;
-
-  try
-  {
-    Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
-  }
-  catch (serial::IOException &e)
-  {
-    RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port"));
-  }
+  origincar_base_core::Command command;
+  command.linear_x = akm_ctl->drive.speed;
+  command.steering_angle = akm_ctl->drive.steering_angle;
+  command.ackermann = true;
+  command_gate_->update(command, std::chrono::steady_clock::now());
 }

 void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
 {
-//   RCLCPP_INFO(this->get_logger(), "linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z);
-  std::cout << "linerx" << twist_aux->linear.x << std::endl;
-  std::cout << "angular" << twist_aux->angular.z << std::endl;
-  short transition;
-  Send_Data.tx[0] = FRAME_HEADER;
-  Send_Data.tx[1] = 0;
-  Send_Data.tx[2] = 0;
-
-  transition = 0;
-  transition = twist_aux->linear.x * 1000;
-  Send_Data.tx[4] = transition;
-  Send_Data.tx[3] = transition >> 8;
-
-  transition = 0;
-  transition = twist_aux->linear.y * 1000;
-  Send_Data.tx[6] = transition;
-  Send_Data.tx[5] = transition >> 8;
-
-  transition = 0;
-  transition = (twist_aux->angular.z) * 1000;
-  Send_Data.tx[8] = transition;
-  Send_Data.tx[7] = transition >> 8;
-
-  Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
-  Send_Data.tx[10] = FRAME_TAIL;
-
-  try
-  {
-    Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
-  }
-  catch (serial::IOException &e)
-  {
-    RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port"));
-  }
+  origincar_base_core::Command command;
+  command.linear_x = twist_aux->linear.x;
+  command.linear_y = twist_aux->linear.y;
+  command.angular_z = twist_aux->angular.z;
+  command_gate_->update(command, std::chrono::steady_clock::now());
 }

 void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch)
 {
  (void)sign_switch;
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  if (sign_switch->data == -1)
  {
    memset(&Robot_Pos, 0, sizeof(Robot_Pos));
@@ -201,6 +151,7 @@ void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr

 void origincar_base::Publish_ImuSensor()
 {
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  tf2::Quaternion q;
  q.setRPY(0.0, 0.0, Robot_Pos.Z);

@@ -226,6 +177,7 @@ void origincar_base::Publish_ImuSensor()

 void origincar_base::Publish_Odom()
 {
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  tf2::Quaternion q;
  q.setRPY(0, 0, Robot_Pos.Z);
  geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q);
@@ -278,18 +230,36 @@ void origincar_base::Publish_Odom()
    tf_broadcaster_->sendTransform(t);
  }
  odom_publisher->publish(odom);
+  if (has_pending_odom_timing_frame_)
+  {
+    scan_odom_timing_logger_.finishFrame(pending_odom_timing_frame_id_);
+    has_pending_odom_timing_frame_ = false;
+  }
+  Print_Timing_Log_If_Due();
  robotpose_publisher->publish(robotpose);
  robotvel_publisher->publish(robotvel);
 }

 void origincar_base::Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
 {
+  {
+    std::lock_guard<std::mutex> lock(wall_scan_mutex_);
+    if (has_latest_scan_timing_frame_ && !latest_scan_consumed_)
+    {
+      scan_odom_timing_logger_.cancelFrame(latest_scan_timing_frame_id_, "replaced_by_new_scan");
+      has_latest_scan_timing_frame_ = false;
+    }
+  }
+
+  const uint64_t timing_frame_id = scan_odom_timing_logger_.startFrame();
  const auto scan_points = scanMsgToPoints(*scan, wall_scan_stride_);
  const auto pose_frame_points = ::origincar_wall::transformScanPointsToPoseFrame(scan_points, laser_pose_);
  std::lock_guard<std::mutex> lock(wall_scan_mutex_);
  latest_scan_points_ = pose_frame_points;
+  latest_scan_timing_frame_id_ = timing_frame_id;
  has_latest_scan_ = true;
  latest_scan_consumed_ = false;
+  has_latest_scan_timing_frame_ = true;
 }

 void origincar_base::Apply_Wall_Update()
@@ -299,6 +269,8 @@ void origincar_base::Apply_Wall_Update()
    return;
  }
  std::vector<::origincar_wall::WallPoint> scan_points;
+  uint64_t timing_frame_id = 0;
+  bool has_timing_frame = false;
  {
    std::lock_guard<std::mutex> lock(wall_scan_mutex_);
    if (!has_latest_scan_ || latest_scan_consumed_)
@@ -306,8 +278,16 @@ void origincar_base::Apply_Wall_Update()
      return;
    }
    scan_points = latest_scan_points_;
+    timing_frame_id = latest_scan_timing_frame_id_;
+    has_timing_frame = has_latest_scan_timing_frame_;
+    has_latest_scan_timing_frame_ = false;
    latest_scan_consumed_ = true;
  }
+  if (has_timing_frame)
+  {
+    pending_odom_timing_frame_id_ = timing_frame_id;
+    has_pending_odom_timing_frame_ = true;
+  }

  const auto result = ::origincar_wall::localizeFromScan(scan_points, wall_fit_config_, wall_filter_->pose());
  if (!result.ok)
@@ -325,8 +305,22 @@ void origincar_base::Apply_Wall_Update()
  wall_filter_->correct(result.pose, noise);
 }

+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))
+  {
+    RCLCPP_INFO(this->get_logger(), "%s", timing_summary.c_str());
+  }
+}
+
 void origincar_base::Publish_Voltage()
 {
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  std_msgs::msg::Float32 voltage_msgs;
  static float Count_Voltage_Pub = 0;

@@ -340,6 +334,7 @@ void origincar_base::Publish_Voltage()

 void origincar_base::Publish_GyroDebug()
 {
+  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  origincar_msg::msg::Data gyro_debug;
  gyro_debug.x = gyro_z_filtered_pre_bias_;
  gyro_debug.y = gyro_z_bias_model_;
@@ -373,14 +368,37 @@ bool origincar_base::Get_Sensor_Data()
 {
  short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
  uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
-  try
-  {
-    Stm32_Serial.read(Receive_Data_Pr, sizeof(Receive_Data_Pr));
-  }
-  catch (const serial::SerialException &e)
-  {
-    return false;
+  std::array<uint8_t, RECEIVE_DATA_SIZE> frame{};
+  if (!serial_frame_parser_.popFrame(&frame)) {
+    try
+    {
+      std::string incoming;
+      {
+        std::lock_guard<std::mutex> serial_lock(serial_mutex_);
+        const auto available = Stm32_Serial.available();
+        if (available == 0) {
+          return false;
+        }
+        incoming = Stm32_Serial.read(std::min<std::size_t>(available, 256));
+      }
+      if (incoming.empty()) {
+        return false;
+      }
+      serial_frame_parser_.append(
+          reinterpret_cast<const uint8_t *>(incoming.data()), incoming.size());
+    }
+    catch (const std::exception &e)
+    {
+      RCLCPP_ERROR_THROTTLE(
+          this->get_logger(), *this->get_clock(), 2000,
+          "Unable to read STM32 serial data: %s", e.what());
+      return false;
+    }
+    if (!serial_frame_parser_.popFrame(&frame)) {
+      return false;
+    }
  }
+  std::copy(frame.begin(), frame.end(), Receive_Data_Pr);
  for (j = 0; j < 24; j++)
  {
    if (Receive_Data_Pr[j] == FRAME_HEADER)
@@ -403,6 +421,7 @@ bool origincar_base::Get_Sensor_Data()
    return false;
  }

+  std::lock_guard<std::mutex> sensor_lock(sensor_data_mutex_);
  Receive_Data.Frame_Header = Receive_Data.rx[0];
  Receive_Data.Frame_Tail = Receive_Data.rx[23];
  if (Receive_Data.Frame_Header == FRAME_HEADER)
@@ -491,6 +510,7 @@ bool origincar_base::Get_Sensor_Data()
        transition_16 |= Receive_Data.rx[21];
        Power_voltage = transition_16 / 1000 + (transition_16 % 1000) * 0.001;

+        sensor_sequence_.fetch_add(1, std::memory_order_release);
        return true;
      }
    }
@@ -501,36 +521,91 @@ bool origincar_base::Get_Sensor_Data()

 void origincar_base::Control()
 {
-  rclcpp::Time current_time, last_time;
-  current_time = rclcpp::Node::now();
-  last_time = rclcpp::Node::now();
-  while (rclcpp::ok())
-  {
-    current_time = rclcpp::Node::now();
-    Sampling_Time = (current_time - last_time).seconds();
-    if (true == Get_Sensor_Data())
-    {
-      rclcpp::spin_some(this->get_node_base_interface());
-      if (wall_filter_)
-      {
-        wall_filter_->predict(1.03 * Robot_Vel.X, 1.01 * Robot_Vel.Y, Robot_Vel.Z, Sampling_Time);
-        Apply_Wall_Update();
-        const auto fused_pose = wall_filter_->pose();
-        Robot_Pos.X = static_cast<float>(fused_pose.x);
-        Robot_Pos.Y = static_cast<float>(fused_pose.y);
-        Robot_Pos.Z = static_cast<float>(fused_pose.theta);
-      }
-      Publish_ImuSensor();
-      Publish_GyroDebug();
-      Publish_Voltage();
-      Publish_Odom();
+  sensor_thread_stop_.store(false, std::memory_order_release);
+  sensor_thread_ = std::thread(&origincar_base::Sensor_Receive_Loop, this);
+  rclcpp::spin(this->get_node_base_interface());
+  sensor_thread_stop_.store(true, std::memory_order_release);
+  if (sensor_thread_.joinable()) {
+    sensor_thread_.join();
+  }
+}
+
+void origincar_base::Sensor_Receive_Loop()
+{
+  while (rclcpp::ok() && !sensor_thread_stop_.load(std::memory_order_acquire)) {
+    if (!Get_Sensor_Data()) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
-    last_time = current_time;
+  }
+}
+
+void origincar_base::Control_Timer_Callback()
+{
+  const auto sequence = sensor_sequence_.load(std::memory_order_acquire);
+  if (sequence == 0 || sequence == last_processed_sensor_sequence_) {
+    return;
+  }
+
+  const auto current_time = rclcpp::Node::now();
+  Sampling_Time = (current_time - _Last_Time).seconds();
+  if (Sampling_Time <= 0.0f || Sampling_Time > 0.5f) {
+    Sampling_Time = static_cast<float>(control_period_ms_) / 1000.0f;
+  }
+  _Last_Time = current_time;
+
+  Vel_Pos_Data velocity;
+  {
+    std::lock_guard<std::mutex> lock(sensor_data_mutex_);
+    velocity = Robot_Vel;
+  }
+  if (wall_filter_)
+  {
+    wall_filter_->predict(
+        1.03 * velocity.X, 1.01 * velocity.Y, velocity.Z, Sampling_Time);
+    Apply_Wall_Update();
+    const auto fused_pose = wall_filter_->pose();
+    std::lock_guard<std::mutex> lock(sensor_data_mutex_);
+    Robot_Pos.X = static_cast<float>(fused_pose.x);
+    Robot_Pos.Y = static_cast<float>(fused_pose.y);
+    Robot_Pos.Z = static_cast<float>(fused_pose.theta);
+  }
+
+  Publish_ImuSensor();
+  Publish_GyroDebug();
+  Publish_Voltage();
+  Publish_Odom();
+  last_processed_sensor_sequence_ = sequence;
+}
+
+void origincar_base::Tx_Timer_Callback()
+{
+  if (!command_gate_) {
+    return;
+  }
+  Send_Command(command_gate_->commandAt(std::chrono::steady_clock::now()));
+}
+
+void origincar_base::Send_Command(const origincar_base_core::Command & command)
+{
+  const auto frame = origincar_base_core::encodeCommandFrame(command);
+  try
+  {
+    std::lock_guard<std::mutex> lock(serial_mutex_);
+    if (Stm32_Serial.isOpen()) {
+      Stm32_Serial.write(frame.data(), frame.size());
+    }
+  }
+  catch (const std::exception &e)
+  {
+    RCLCPP_ERROR_THROTTLE(
+        this->get_logger(), *this->get_clock(), 2000,
+        "Unable to send STM32 command: %s", e.what());
  }
 }

 origincar_base::origincar_base()
-    : rclcpp::Node("origincar_base")
+    : rclcpp::Node("origincar_base"),
+      scan_odom_timing_logger_(this->get_name())
 {
  memset(&Robot_Pos, 0, sizeof(Robot_Pos));
  memset(&Robot_Vel, 0, sizeof(Robot_Vel));
@@ -550,11 +625,15 @@ origincar_base::origincar_base()
  gyro_z_low_pass_initialized_ = false;
  has_latest_scan_ = false;
  latest_scan_consumed_ = true;
+  latest_scan_timing_frame_id_ = 0;
+  pending_odom_timing_frame_id_ = 0;
+  has_latest_scan_timing_frame_ = false;
+  has_pending_odom_timing_frame_ = false;

-  int serial_baud_rate = 115200;
+  int serial_baud_rate = 921600;

  this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
-  this->declare_parameter<int>("serial_baud_rate", 115200);
+  this->declare_parameter<int>("serial_baud_rate", 921600);
  this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
  this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
  this->declare_parameter<std::string>("odom_frame_id", "odom");
@@ -564,11 +643,16 @@ 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);
  this->declare_parameter<double>("laser_yaw", 0.0);
  this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);
+  this->declare_parameter<int>("cmd_watchdog_timeout_ms", 150);
+  this->declare_parameter<int>("tx_period_ms", 20);
+  this->declare_parameter<int>("serial_read_timeout_ms", 20);
+  this->declare_parameter<int>("control_period_ms", 50);

  // Odom covariance parameters (tunable via YAML)
  this->declare_parameter<double>("odom_pose_cov_x", 0.01);
@@ -586,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(),
@@ -605,6 +690,16 @@ origincar_base::origincar_base()
  this->get_parameter("odom_pose_cov_x", odom_pose_cov_x_);
  this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_);
  this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_);
+  this->get_parameter("cmd_watchdog_timeout_ms", cmd_watchdog_timeout_ms_);
+  this->get_parameter("tx_period_ms", tx_period_ms_);
+  this->get_parameter("serial_read_timeout_ms", serial_read_timeout_ms_);
+  this->get_parameter("control_period_ms", control_period_ms_);
+  cmd_watchdog_timeout_ms_ = std::max(1, cmd_watchdog_timeout_ms_);
+  tx_period_ms_ = std::max(1, tx_period_ms_);
+  serial_read_timeout_ms_ = std::max(1, serial_read_timeout_ms_);
+  control_period_ms_ = std::max(1, control_period_ms_);
+  command_gate_ = std::make_unique<origincar_base_core::CommandGate>(
+      std::chrono::milliseconds(cmd_watchdog_timeout_ms_));

  wall_fit_config_ = ::origincar_wall::loadWallFitConfig(wall_config_path_);
  const auto initial_pose = ::origincar_wall::Pose2D{
@@ -613,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);
@@ -648,7 +744,7 @@ origincar_base::origincar_base()
  {
    Stm32_Serial.setPort(usart_port_name);
    Stm32_Serial.setBaudrate(serial_baud_rate);
-    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
+    serial::Timeout _time = serial::Timeout::simpleTimeout(serial_read_timeout_ms_);
    Stm32_Serial.setTimeout(_time);
    Stm32_Serial.open();
  }
@@ -660,55 +756,35 @@ origincar_base::origincar_base()
  {
    RCLCPP_INFO(this->get_logger(), "origincar_base serial port opened");
  }
+  if (scan_odom_timing_logger_.isOpen())
+  {
+    RCLCPP_INFO(this->get_logger(), "scan_to_odom timing log: %s", scan_odom_timing_logger_.logPath().c_str());
+  }
+  else
+  {
+    RCLCPP_WARN(this->get_logger(), "scan_to_odom timing log file could not be opened: %s", scan_odom_timing_logger_.logPath().c_str());
+  }
+  _Last_Time = rclcpp::Node::now();
+  tx_timer = create_wall_timer(
+      std::chrono::milliseconds(tx_period_ms_),
+      std::bind(&origincar_base::Tx_Timer_Callback, this));
+  odom_timer = create_wall_timer(
+      std::chrono::milliseconds(control_period_ms_),
+      std::bind(&origincar_base::Control_Timer_Callback, this));
 }

 void sigintHandler(int sig)
 {
-  sig = sig;
-  printf("OriginBot shutdown...\n");
-  serial::Serial Stm32_Serial;
-  Stm32_Serial.setPort("/dev/ttyACM0");
-  Stm32_Serial.setBaudrate(115200);
-  serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
-  Stm32_Serial.setTimeout(_time);
-  Stm32_Serial.open();
-  SEND_DATA Send_Data;
-  if (Stm32_Serial.isOpen())
-  {
-    Send_Data.tx[0] = FRAME_HEADER;
-    Send_Data.tx[1] = 0;
-    Send_Data.tx[2] = 0;
-
-    Send_Data.tx[4] = 0;
-    Send_Data.tx[3] = 0;
-
-    Send_Data.tx[6] = 0;
-    Send_Data.tx[5] = 0;
-
-    Send_Data.tx[7] = 0;
-    Send_Data.tx[8] = 0;
-    int check_sum = 0;
-    for (int k = 0; k < 9; k++)
-    {
-      check_sum = check_sum ^ Send_Data.tx[k];
-    }
-    Send_Data.tx[9] = check_sum;
-    Send_Data.tx[10] = FRAME_TAIL;
-
-    try
-    {
-      Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
-    }
-    catch (serial::IOException &e)
-    {
-    }
-  }
-  // Shutdown ROS2 and release resources.
+  (void)sig;
  rclcpp::shutdown();
 }

 origincar_base::~origincar_base()
 {
+  sensor_thread_stop_.store(true, std::memory_order_release);
+  if (sensor_thread_.joinable()) {
+    sensor_thread_.join();
+  }
+  Send_Command(origincar_base_core::Command{});
  RCLCPP_INFO(this->get_logger(), "Shutting down");
 }
-
--- a/src/origincar_base/src/origincar_base.cpp.bak
+++ b/src/origincar_base/src/origincar_base.cpp.bak
@@ -373,7 +373,7 @@ origincar_base::origincar_base()
  Robot_Pos.X = 0.54;
  Robot_Pos.Y = 0.2;

-  int serial_baud_rate = 115200;
+  int serial_baud_rate = 921600;

  this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
  this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
@@ -436,7 +436,7 @@ void sigintHandler(int sig)
      printf("OriginBot shutdown...\n");
    serial::Serial Stm32_Serial;
    Stm32_Serial.setPort("/dev/ttyACM0");
-    Stm32_Serial.setBaudrate(115200);
+    Stm32_Serial.setBaudrate(921600);
    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
    Stm32_Serial.setTimeout(_time);
    Stm32_Serial.open();                                       
--- a/src/origincar_base/src/wall_kalman_filter.cpp
+++ b/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
--- a/src/origincar_base/test/command_frame_test.cpp
+++ b/src/origincar_base/test/command_frame_test.cpp
@@ -0,0 +1,64 @@
+#include <array>
+#include <cstdint>
+
+#include <gtest/gtest.h>
+
+#include "origincar_base/command_frame.hpp"
+
+using origincar_base_core::Command;
+using origincar_base_core::encodeCommandFrame;
+
+namespace
+{
+uint8_t checksum(const std::array<uint8_t, 11> & frame)
+{
+  uint8_t value = 0;
+  for (std::size_t index = 0; index < 9; ++index) {
+    value ^= frame[index];
+  }
+  return value;
+}
+}  // namespace
+
+TEST(CommandFrameTest, EncodesTwistCommandUsingExistingProtocol)
+{
+  const auto frame = encodeCommandFrame(Command{0.25, -0.1, 0.4});
+
+  EXPECT_EQ(frame[0], 0x7B);
+  EXPECT_EQ(frame[3], 0x00);
+  EXPECT_EQ(frame[4], 0xFA);
+  EXPECT_EQ(frame[5], 0xFF);
+  EXPECT_EQ(frame[6], 0x9C);
+  EXPECT_EQ(frame[7], 0x01);
+  EXPECT_EQ(frame[8], 0x90);
+  EXPECT_EQ(frame[9], checksum(frame));
+  EXPECT_EQ(frame[10], 0x7D);
+}
+
+TEST(CommandFrameTest, EncodesAckermannSteeringWithFirmwareScale)
+{
+  Command command;
+  command.linear_x = 0.25;
+  command.steering_angle = 0.4;
+  command.ackermann = true;
+
+  const auto frame = encodeCommandFrame(command);
+
+  EXPECT_EQ(frame[3], 0x00);
+  EXPECT_EQ(frame[4], 0xFA);
+  EXPECT_EQ(frame[5], 0x00);
+  EXPECT_EQ(frame[6], 0x00);
+  EXPECT_EQ(frame[7], 0x00);
+  EXPECT_EQ(frame[8], 0xC8);
+  EXPECT_EQ(frame[9], checksum(frame));
+}
+
+TEST(CommandFrameTest, ZeroCommandEncodesZeroPayload)
+{
+  const auto frame = encodeCommandFrame(Command{});
+
+  for (std::size_t index = 1; index <= 8; ++index) {
+    EXPECT_EQ(frame[index], 0x00);
+  }
+  EXPECT_EQ(frame[9], checksum(frame));
+}
--- a/src/origincar_base/test/command_gate_test.cpp
+++ b/src/origincar_base/test/command_gate_test.cpp
@@ -0,0 +1,42 @@
+#include <chrono>
+
+#include <gtest/gtest.h>
+
+#include "origincar_base/command_gate.hpp"
+
+using origincar_base_core::Command;
+using origincar_base_core::CommandGate;
+
+TEST(CommandGateTest, ReturnsZeroBeforeFirstCommand)
+{
+  const auto t0 = std::chrono::steady_clock::time_point{};
+  CommandGate gate(std::chrono::milliseconds(150));
+
+  EXPECT_EQ(gate.commandAt(t0), (Command{}));
+}
+
+TEST(CommandGateTest, KeepsLatestCommandUntilWatchdogExpires)
+{
+  const auto t0 = std::chrono::steady_clock::time_point{};
+  CommandGate gate(std::chrono::milliseconds(150));
+  const Command command{0.25, 0.0, 0.4};
+
+  gate.update(command, t0);
+
+  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(149)), command);
+  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(150)), (Command{}));
+}
+
+TEST(CommandGateTest, NewCommandRefreshesWatchdog)
+{
+  const auto t0 = std::chrono::steady_clock::time_point{};
+  CommandGate gate(std::chrono::milliseconds(150));
+  const Command first{0.1, 0.0, 0.1};
+  const Command second{0.3, 0.0, -0.2};
+
+  gate.update(first, t0);
+  gate.update(second, t0 + std::chrono::milliseconds(100));
+
+  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(249)), second);
+  EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(250)), (Command{}));
+}
--- a/src/origincar_base/test/scan_odom_timing_logger_test.cpp
+++ b/src/origincar_base/test/scan_odom_timing_logger_test.cpp
@@ -0,0 +1,113 @@
+#include "origincar_base/log.hpp"
+
+#include <chrono>
+#include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+#include <unistd.h>
+
+namespace
+{
+using Clock = std::chrono::steady_clock;
+
+void require(bool condition, const std::string &message)
+{
+  if (!condition)
+  {
+    throw std::runtime_error(message);
+  }
+}
+
+void requireContains(const std::string &text, const std::string &expected, const std::string &message)
+{
+  if (text.find(expected) == std::string::npos)
+  {
+    throw std::runtime_error(message + " missing=" + expected + " text=" + text);
+  }
+}
+
+std::string readFile(const std::string &path)
+{
+  std::ifstream input(path.c_str());
+  return std::string((std::istreambuf_iterator<char>(input)), std::istreambuf_iterator<char>());
+}
+
+std::string makeTempDir()
+{
+  const std::string path = "/tmp/origincar_timing_logger_test_" + std::to_string(getpid());
+  std::string command = "rm -rf " + path + " && mkdir -p " + path;
+  require(std::system(command.c_str()) == 0, "create temp log dir");
+  return path;
+}
+
+void testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate()
+{
+  const auto root = makeTempDir();
+  const auto start = Clock::time_point(std::chrono::seconds(0));
+  origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root, start);
+
+  const auto first = logger.startFrame(start);
+  require(logger.finishFrame(first, start + std::chrono::milliseconds(15)), "finish first frame");
+  std::string contents = readFile(logger.logPath());
+  requireContains(contents, "frame_id=1", "first frame id");
+  requireContains(contents, "duration_ms=15.000", "first frame duration");
+  requireContains(contents, "avg_ms=15.000", "first frame average");
+  requireContains(contents, "max_ms=15.000", "first frame max");
+  requireContains(contents, "min_ms=15.000", "first frame min");
+
+  const auto second = logger.startFrame(start + std::chrono::milliseconds(20));
+  require(logger.finishFrame(second, start + std::chrono::milliseconds(50)), "finish second frame");
+  contents = readFile(logger.logPath());
+  requireContains(contents, "frame_id=2", "second frame id");
+  requireContains(contents, "duration_ms=30.000", "second frame duration");
+  requireContains(contents, "avg_ms=22.500", "second frame average");
+  requireContains(contents, "max_ms=30.000", "second frame max");
+  requireContains(contents, "min_ms=15.000", "second frame min");
+}
+
+void testSummaryPrintIsThrottledToOneHz()
+{
+  const auto root = makeTempDir();
+  const auto start = Clock::time_point(std::chrono::seconds(0));
+  origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root, start);
+  std::string summary;
+
+  const auto frame = logger.startFrame(start);
+  require(logger.finishFrame(frame, start + std::chrono::milliseconds(10)), "finish frame");
+  require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(999), &summary),
+          "summary should not be due before one second");
+  require(logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1000), &summary),
+          "summary should be due at one second");
+  requireContains(summary, "frames=1", "summary frame count");
+  requireContains(summary, "avg_ms=10.000", "summary average");
+  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()
+{
+  try
+  {
+    testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate();
+    testSummaryPrintIsThrottledToOneHz();
+    testTerminalOutputDefaultsToOff();
+  }
+  catch (const std::exception &error)
+  {
+    std::cerr << "scan_odom_timing_logger_test failed: " << error.what() << std::endl;
+    return 1;
+  }
+  return 0;
+}
--- a/src/origincar_base/test/serial_frame_parser_test.cpp
+++ b/src/origincar_base/test/serial_frame_parser_test.cpp
@@ -0,0 +1,66 @@
+#include <array>
+#include <cstdint>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "origincar_base/serial_frame_parser.hpp"
+
+using origincar_base_core::SerialFrameParser;
+
+namespace
+{
+std::array<uint8_t, SerialFrameParser::kFrameSize> makeFrame(uint8_t seed)
+{
+  std::array<uint8_t, SerialFrameParser::kFrameSize> frame{};
+  frame[0] = 0x7B;
+  frame[23] = 0x7D;
+  for (std::size_t index = 1; index < 23; ++index) {
+    frame[index] = static_cast<uint8_t>(seed + index);
+  }
+  return frame;
+}
+}  // namespace
+
+TEST(SerialFrameParserTest, ReassemblesFrameAcrossPartialReads)
+{
+  SerialFrameParser parser;
+  const auto expected = makeFrame(10);
+  std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
+
+  parser.append(expected.data(), 7);
+  EXPECT_FALSE(parser.popFrame(&actual));
+
+  parser.append(expected.data() + 7, expected.size() - 7);
+  ASSERT_TRUE(parser.popFrame(&actual));
+  EXPECT_EQ(actual, expected);
+}
+
+TEST(SerialFrameParserTest, DiscardsNoiseBeforeValidFrame)
+{
+  SerialFrameParser parser;
+  const auto expected = makeFrame(20);
+  const std::vector<uint8_t> noise{0x00, 0x01, 0x7D, 0x55};
+  std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
+
+  parser.append(noise.data(), noise.size());
+  parser.append(expected.data(), expected.size());
+
+  ASSERT_TRUE(parser.popFrame(&actual));
+  EXPECT_EQ(actual, expected);
+}
+
+TEST(SerialFrameParserTest, RejectsInvalidTailAndContinuesSearching)
+{
+  SerialFrameParser parser;
+  auto invalid = makeFrame(30);
+  invalid[23] = 0x00;
+  const auto expected = makeFrame(40);
+  std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
+
+  parser.append(invalid.data(), invalid.size());
+  parser.append(expected.data(), expected.size());
+
+  ASSERT_TRUE(parser.popFrame(&actual));
+  EXPECT_EQ(actual, expected);
+}
--- a/src/origincar_base/test/wall_kalman_filter_test.cpp
+++ b/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)
  {
--- a/src/planner/REAL_ROBOT_RUNBOOK.md
+++ b/src/planner/REAL_ROBOT_RUNBOOK.md
@@ -19,7 +19,7 @@ must be replaced by a map created from the real environment before driving.
 2. The LSLIDAR model, interface and serial device match the selected parameter
   file. The example N10 configuration uses `/dev/ttyCH343USB0`.
 3. The STM32 serial device and baud rate are correct. The default is
-   `/dev/ttyACM0` and 115200.
+   `/dev/ttyACM0` and 921600.
 4. The robot is Ackermann configured and the measured minimum turning radius is
   close to 0.40 m.
 5. The initial robot pose in the map is known and the start cell is free.
@@ -59,7 +59,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
  lidar_model:=N10 \
  start_lidar:=true \
  serial_port:=/dev/ttyACM0 \
-  serial_baud:=115200 \
+  serial_baud:=921600 \
  enable_motion:=false
 ```

--- a/src/planner/launch/odom_hybrid_astar.launch.py
+++ b/src/planner/launch/odom_hybrid_astar.launch.py
@@ -331,7 +331,7 @@ def generate_launch_description():
        DeclareLaunchArgument("start_base", default_value="true"),
        DeclareLaunchArgument("start_robot_state_publisher", default_value="true"),
        DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
-        DeclareLaunchArgument("serial_baud", default_value="115200"),
+        DeclareLaunchArgument("serial_baud", default_value="921600"),
        DeclareLaunchArgument(
            "enable_motion",
            default_value="false",
--- a/src/planner/launch/real_hybrid_astar.launch.py
+++ b/src/planner/launch/real_hybrid_astar.launch.py
@@ -273,7 +273,7 @@ def generate_launch_description():
            "lidar_params",
            description="Required: LSLIDAR YAML matching the actual sensor/interface.",
        ),
-        DeclareLaunchArgument("lidar_serial_port", default_value="/dev/ttyCH343USB0"),
+        DeclareLaunchArgument("lidar_serial_port", default_value="/dev/radar"),
        DeclareLaunchArgument("lidar_model", default_value="N10"),
        DeclareLaunchArgument(
            "start_lidar",
@@ -281,7 +281,7 @@ def generate_launch_description():
            description="Start LSLIDAR. Set false when /scan is provided externally.",
        ),
        DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
-        DeclareLaunchArgument("serial_baud", default_value="115200"),
+        DeclareLaunchArgument("serial_baud", default_value="921600"),
        DeclareLaunchArgument(
            "enable_motion",
            default_value="false",
--- a/src/planner/操作手册.md
+++ b/src/planner/操作手册.md
@@ -70,7 +70,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
  lidar_model:=N10 \
  start_lidar:=true \
  serial_port:=/dev/实际底盘串口 \
-  serial_baud:=115200 \
+  serial_baud:=921600 \
  enable_motion:=false
 ```

@@ -84,7 +84,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
  lidar_model:=N10 \
  start_lidar:=true \
  serial_port:=/dev/实际底盘串口 \
-  serial_baud:=115200 \
+  serial_baud:=921600 \
  enable_motion:=false
 ```

@@ -183,7 +183,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
  lidar_model:=N10 \
  start_lidar:=true \
  serial_port:=/dev/实际底盘串口 \
-  serial_baud:=115200 \
+  serial_baud:=921600 \
  wheelbase:=0.143 \
  max_steering_angle:=0.60 \
  enable_motion:=true
--- a/src/qr_detection/CMakeLists.txt
+++ b/src/qr_detection/CMakeLists.txt
@@ -25,12 +25,23 @@ target_link_libraries(qr_detect
 ament_target_dependencies(qr_detect
  rclcpp std_msgs sensor_msgs origincar_msg)

-install(TARGETS qr_detect
+add_executable(qr_dete_depth src/qr_dete_depth.cpp)
+target_include_directories(qr_dete_depth PUBLIC
+  ${OpenCV_INCLUDE_DIRS} ${ZBar_INCLUDE_DIRS})
+target_link_libraries(qr_dete_depth
+  ${OpenCV_LIBS} ${ZBar_LIBRARIES})
+ament_target_dependencies(qr_dete_depth
+  rclcpp std_msgs sensor_msgs origincar_msg)
+
+install(TARGETS qr_detect qr_dete_depth
  DESTINATION lib/${PROJECT_NAME})

 install(DIRECTORY launch/
  DESTINATION share/${PROJECT_NAME}/launch)

+install(DIRECTORY config/
+  DESTINATION share/${PROJECT_NAME}/config)
+
 if(BUILD_TESTING)
  find_package(ament_cmake_gtest REQUIRED)
  find_package(ament_lint_auto REQUIRED)
@@ -43,6 +54,15 @@ if(BUILD_TESTING)
    ament_target_dependencies(test_qr_detect
      rclcpp std_msgs sensor_msgs origincar_msg)
  endif()
+  ament_add_gtest(test_qr_dete_depth test/test_qr_dete_depth.cpp)
+  if(TARGET test_qr_dete_depth)
+    target_include_directories(test_qr_dete_depth PUBLIC
+      ${OpenCV_INCLUDE_DIRS} ${ZBar_INCLUDE_DIRS})
+    target_link_libraries(test_qr_dete_depth
+      ${OpenCV_LIBS} ${ZBar_LIBRARIES})
+    ament_target_dependencies(test_qr_dete_depth
+      rclcpp std_msgs sensor_msgs origincar_msg)
+  endif()
  set(ament_cmake_copyright_FOUND TRUE)
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
--- a/src/qr_detection/config/qr_dete_depth.yaml
+++ b/src/qr_detection/config/qr_dete_depth.yaml
@@ -0,0 +1,5 @@
+qr_dete_depth:
+  ros__parameters:
+    image_topic: /aurora/rgb/image_raw
+    use_buffer: true
+    tts_service: /tts/speak
--- a/src/qr_detection/launch/qr_dete_depth.launch.py
+++ b/src/qr_detection/launch/qr_dete_depth.launch.py
@@ -0,0 +1,31 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    default_params_file = os.path.join(
+        get_package_share_directory("qr_detection"),
+        "config",
+        "qr_dete_depth.yaml",
+    )
+    params_file = LaunchConfiguration("params_file")
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "params_file",
+            default_value=default_params_file,
+            description="Path to qr_dete_depth parameter YAML file",
+        ),
+        Node(
+            package="qr_detection",
+            executable="qr_dete_depth",
+            name="qr_dete_depth",
+            output="screen",
+            parameters=[params_file],
+        ),
+    ])
--- a/src/qr_detection/launch/qr_detection.launch.py
+++ b/src/qr_detection/launch/qr_detection.launch.py
@@ -26,8 +26,8 @@ def generate_launch_description():

    qr_detection_node = Node(
        package='qr_detection',
-        executable='qr_dete_node',
-        name='qr_dete_node',
+        executable='qr_detect',
+        name='qr_detect',
        output='screen'
    )

--- a/src/qr_detection/src/qr_dete_depth.cpp
+++ b/src/qr_detection/src/qr_dete_depth.cpp
@@ -1,173 +1,208 @@
-#include <memory>
-#include <iostream>
-#include <opencv2/opencv.hpp>
-#include <zbar.h>
+#include "origincar_msg/srv/speak.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
 #include "std_msgs/msg/int32.hpp"
 #include "std_msgs/msg/string.hpp"
-#include "sensor_msgs/msg/image.hpp"

-class MinimalHbmemSubscriber : public rclcpp::Node
+#include <opencv2/opencv.hpp>
+#include <zbar.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+class QrDeteDepthNode : public rclcpp::Node
 {
 public:
-  MinimalHbmemSubscriber()
-  : Node("qr_detection"), detect_qr_code_(true)  // 默认检测二维码
+  explicit QrDeteDepthNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+  : Node("qr_dete_depth", options), enabled_(true)
  {
-    use_buffer = this->declare_parameter("use_buffer", true); // 声明参数，默认使用缓存
-    // 订阅 /aurora/rgb/image_raw（sensor_msgs::msg::Image 格式）
-    subscription_image_ =
-      this->create_subscription<sensor_msgs::msg::Image>(
-        "/aurora/rgb/image_raw",
-        10,
-        std::bind(&MinimalHbmemSubscriber::image_callback, this, std::placeholders::_1));
+    image_topic_ = declare_parameter<std::string>("image_topic", "/aurora/rgb/image_raw");
+    use_buffer_ = declare_parameter<bool>("use_buffer", true);
+    tts_service_ = declare_parameter<std::string>("tts_service", "/tts/speak");

-    // 创建订阅器，订阅 'sign4return' 话题
-    // subscription_sign_ =
-    //   this->create_subscription<std_msgs::msg::Int32>(
-    //     "sign4return",
-    //     10,
-    //     std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1));
-    // 创建发布器，发布 'sign4return' 话题
-    sign_publisher_ = this->create_publisher<std_msgs::msg::Int32>("sign4return", 10);
+    image_sub_ = create_subscription<sensor_msgs::msg::Image>(
+      image_topic_, rclcpp::SensorDataQoS(),
+      std::bind(&QrDeteDepthNode::image_callback, this, std::placeholders::_1));

-    // 创建 publisher，topic 为 "qr_results"
-    publisher_ =
-      this->create_publisher<std_msgs::msg::String>("qr_results", 10);
+    const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
+    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
+      "sign4return", sign_qos,
+      std::bind(&QrDeteDepthNode::sign_callback, this, std::placeholders::_1));
+
+    result_pub_ = create_publisher<std_msgs::msg::String>("qr_results", 10);
+    tts_client_ = create_client<origincar_msg::srv::Speak>(tts_service_);
+
+    RCLCPP_INFO(
+      get_logger(), "QR depth detect ready, topic=%s, use_buffer=%s, tts_service=%s",
+      image_topic_.c_str(), use_buffer_ ? "true" : "false", tts_service_.c_str());
  }

 private:
-  // 图像回调函数，处理 /aurora/rgb/image_raw 话题
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
-    if (!detect_qr_code_)
-    {
-      RCLCPP_INFO(this->get_logger(), "QR detection is disabled");
+    if (!enabled_) {
      return;
    }

-    // 创建 Clock 对象并获取当前时间
-    auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
-    auto now = clock->now();
+    log_frame_delay(msg);

-    // 转换消息时间戳为 ROS 时间
-    auto msg_time = rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec);
-
-    // 计算延时（单位：微秒）
-    auto duration = now - msg_time;
-    auto delay_us = duration.nanoseconds() / 1000; // 转换为微秒
-
-    // 打印延迟
-    RCLCPP_INFO(this->get_logger(), "frame_id: %s, time cost %ldus",
-                msg->header.frame_id.c_str(), delay_us);
-
-    // 将 sensor_msgs::Image 转换为 OpenCV 格式（Aurora RGB 为 bgr8 编码，需转为灰度给 ZBar）
-    if (msg->encoding != "bgr8") {
-      RCLCPP_ERROR(this->get_logger(), "Expected bgr8 encoding, got %s", msg->encoding.c_str());
-      return;
-    }
-    cv::Mat rgb_image(msg->height, msg->width, CV_8UC3,
-                      const_cast<uint8_t*>(msg->data.data()));
    cv::Mat gray_image;
-    cv::cvtColor(rgb_image, gray_image, cv::COLOR_BGR2GRAY);
+    if (!image_to_gray(msg, gray_image)) {
+      return;
+    }

-    // 初始化 ZBar 扫描器
+    scan_and_publish(gray_image);
+  }
+
+  void log_frame_delay(const sensor_msgs::msg::Image::SharedPtr msg)
+  {
+    const auto now = get_clock()->now();
+    const auto msg_time = rclcpp::Time(msg->header.stamp);
+    const auto delay_us = (now - msg_time).nanoseconds() / 1000;
+
+    RCLCPP_INFO(
+      get_logger(), "frame_id: %s, time cost %ldus",
+      msg->header.frame_id.c_str(), delay_us);
+  }
+
+  bool image_to_gray(const sensor_msgs::msg::Image::SharedPtr msg, cv::Mat & gray_image)
+  {
+    if (msg->encoding != "bgr8") {
+      static int warn_count = 0;
+      if (warn_count++ < 3) {
+        RCLCPP_ERROR(get_logger(), "Expected bgr8 encoding, got %s", msg->encoding.c_str());
+      }
+      return false;
+    }
+
+    cv::Mat bgr_image(
+      msg->height, msg->width, CV_8UC3,
+      const_cast<uint8_t *>(msg->data.data()));
+    cv::cvtColor(bgr_image, gray_image, cv::COLOR_BGR2GRAY);
+    return true;
+  }
+
+  void scan_and_publish(cv::Mat & gray_image)
+  {
    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);

-    // 将 OpenCV 图像数据包装成 ZBar 图像
-    zbar::Image zbar_image(gray_image.cols, gray_image.rows, "Y800",
-                           gray_image.data, gray_image.cols * gray_image.rows);
+    zbar::Image zbar_image(
+      gray_image.cols, gray_image.rows, "Y800",
+      gray_image.data, gray_image.cols * gray_image.rows);

-    // 扫描图像中的条形码和二维码
    std::string qr_results;
-    int n = scanner.scan(zbar_image);
+    const int n = scanner.scan(zbar_image);
    if (n > 0) {
      for (auto symbol = zbar_image.symbol_begin();
-           symbol != zbar_image.symbol_end(); ++symbol) {
+        symbol != zbar_image.symbol_end(); ++symbol)
+      {
        qr_results += symbol->get_data();
-        RCLCPP_INFO(this->get_logger(), "Decoded %s symbol \"%s\"",
-                    symbol->get_type_name().c_str(), symbol->get_data().c_str());
+        RCLCPP_INFO(
+          get_logger(), "Decoded %s symbol \"%s\"",
+          symbol->get_type_name().c_str(), symbol->get_data().c_str());
      }
-      qr_results_buff_ = qr_results; // 更新缓存
-
-      // 检测字符合法性
-      // if (ResultLegal(qr_results)){
-      //   // 发布sign4return话题
-      //   auto message = std_msgs::msg::Int32();
-      //   message.data = 5; // 5表示检测到二维码
-      //   sign_publisher_->publish(message);
-      //   RCLCPP_INFO(this->get_logger(), "QR Results Legal");
-      // }
-      // else{
-      //   RCLCPP_INFO(this->get_logger(), "\033[31m QR Results Illegal \033[00m");
-      // }
-
-      // 发布sign4return话题
-      auto message = std_msgs::msg::Int32();
-      message.data = 5; // 5表示检测到二维码
-      sign_publisher_->publish(message);
+      qr_results_buff_ = qr_results;
    } else {
      qr_results = "QR Code not detected";
-      RCLCPP_INFO(this->get_logger(), "QR Code not detected");
+      RCLCPP_INFO(get_logger(), "QR Code not detected");
    }

-    // 发布 QR 结果
-    auto message = std_msgs::msg::String();
-    if (use_buffer){
-      message.data = qr_results_buff_;
-    } 
-    else{
-      message.data = qr_results;
+    const std::string result_text = use_buffer_ ? qr_results_buff_ : qr_results;
+    if (result_text.empty()) {
+      return;
    }
-    publisher_->publish(message);
+
+    publish_result(result_text);
  }

-  // 消息回调函数，处理 sign4return 话题
-  // void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
-  // {
-  //   if (msg->data == 0)
-  //   {
-  //     detect_qr_code_ = true; // 启动二维码检测
-  //     RCLCPP_INFO(this->get_logger(), "QR detection started");
-  //   }
-  //   else if (msg->data == 5)
-  //   {
-  //     detect_qr_code_ = false; // 停止二维码检测
-  //     RCLCPP_INFO(this->get_logger(), "QR detection stopped");
-  //   }
-  // }
+  void publish_result(const std::string & qr_data)
+  {
+    auto out = std_msgs::msg::String();
+    if (is_numeric(qr_data)) {
+      out.data = qr_data + " " + (((qr_data.back() - '0') % 2 == 1) ? "顺时针" : "逆时针");
+    } else {
+      out.data = qr_data;
+    }

-  // 识别结果合法性检测
-  bool ResultLegal(std::string& input){
-    // 合法字符串集合
-    static const std::unordered_set<std::string> kValidStrings = {
-        "1", "2", "顺时针", "逆时针", "顺", "逆"
-    };
-    return kValidStrings.find(input) != kValidStrings.end();
+    if (out.data != last_) {
+      RCLCPP_INFO(get_logger(), "QR: %s", out.data.c_str());
+      last_ = out.data;
+    }
+
+    result_pub_->publish(out);
+    speak_once(out.data);
  }

+  bool is_numeric(const std::string & input) const
+  {
+    return !input.empty() && input.find_first_not_of("0123456789") == std::string::npos;
+  }

-  // /aurora/rgb/image_raw 订阅器
-  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_image_;
-  // sign4return 订阅器
-  // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
-  // sign4return 发布器
-  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_publisher_;
-  // QR code results 发布器
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  void speak_once(const std::string & text)
+  {
+    if (text == last_spoken_) {
+      return;
+    }
+    last_spoken_ = text;

-  // 参数
-  bool use_buffer;
-  // 二维码检测标志位
-  bool detect_qr_code_;
-  // 二维码检测结果缓存
+    if (!tts_client_->service_is_ready()) {
+      static int warn_count = 0;
+      if (warn_count++ < 3) {
+        RCLCPP_WARN(get_logger(), "TTS service %s not available", tts_service_.c_str());
+      }
+      return;
+    }
+
+    auto request = std::make_shared<origincar_msg::srv::Speak::Request>();
+    request->text = text;
+    tts_client_->async_send_request(
+      request,
+      [this](rclcpp::Client<origincar_msg::srv::Speak>::SharedFuture future) {
+        try {
+          const auto response = future.get();
+          if (!response->success) {
+            RCLCPP_WARN(get_logger(), "TTS failed: %s", response->message.c_str());
+          }
+        } catch (const std::exception & e) {
+          RCLCPP_ERROR(get_logger(), "TTS call error: %s", e.what());
+        }
+      });
+  }
+
+  void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
+  {
+    if (msg->data == 0) {
+      enabled_ = true;
+      last_.clear();
+      last_spoken_.clear();
+      qr_results_buff_.clear();
+      RCLCPP_INFO(get_logger(), "ON");
+    } else if (msg->data == 5) {
+      enabled_ = false;
+      RCLCPP_INFO(get_logger(), "OFF");
+    }
+  }
+
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sign_sub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr result_pub_;
+  rclcpp::Client<origincar_msg::srv::Speak>::SharedPtr tts_client_;
+
+  bool enabled_;
+  bool use_buffer_;
+  std::string image_topic_;
+  std::string tts_service_;
+  std::string last_;
+  std::string last_spoken_;
  std::string qr_results_buff_;
 };

 int main(int argc, char * argv[])
 {
  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<MinimalHbmemSubscriber>());
+  rclcpp::spin(std::make_shared<QrDeteDepthNode>());
  rclcpp::shutdown();
  return 0;
 }
--- a/src/qr_detection/src/qr_detect.cpp
+++ b/src/qr_detection/src/qr_detect.cpp
@@ -49,8 +49,9 @@ public:
      auto_select_image_subscription();
    }

+    const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
-        "sign4return", 10,
+        "sign4return", sign_qos,
        std::bind(&QrDetectNode::sign_cb, this, std::placeholders::_1));

    pub_ = create_publisher<std_msgs::msg::String>("qr_results", 10);
@@ -243,8 +244,15 @@ private:

  void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
  {
-    if (msg->data == 0)      { enabled_ = true;  RCLCPP_INFO(get_logger(), "ON"); }
-    else if (msg->data == 5) { enabled_ = false; RCLCPP_INFO(get_logger(), "OFF"); }
+    if (msg->data == 0) {
+      enabled_ = true;
+      last_.clear();
+      last_spoken_.clear();
+      RCLCPP_INFO(get_logger(), "ON");
+    } else if (msg->data == 5) {
+      enabled_ = false;
+      RCLCPP_INFO(get_logger(), "OFF");
+    }
  }

  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr             image_sub_;
--- a/src/qr_detection/src/qr_hbmem.cpp
+++ b/src/qr_detection/src/qr_hbmem.cpp
@@ -28,8 +28,9 @@ public:
      std::bind(&QrHbmemNode::callback, this, std::placeholders::_1));

    // sign4return 启停控制
+    const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
    sign_sub_ = this->create_subscription<std_msgs::msg::Int32>(
-      "sign4return", 10,
+      "sign4return", sign_qos,
      std::bind(&QrHbmemNode::sign_cb, this, std::placeholders::_1));

    // 结果发布
@@ -74,8 +75,14 @@ private:

  void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
  {
-    if (msg->data == 0)      { detect_enabled_ = true;  RCLCPP_INFO(this->get_logger(), "ON");  }
-    else if (msg->data == 5) { detect_enabled_ = false; RCLCPP_INFO(this->get_logger(), "OFF"); }
+    if (msg->data == 0) {
+      detect_enabled_ = true;
+      last_found_.clear();
+      RCLCPP_INFO(this->get_logger(), "ON");
+    } else if (msg->data == 5) {
+      detect_enabled_ = false;
+      RCLCPP_INFO(this->get_logger(), "OFF");
+    }
  }

  rclcpp::SubscriptionHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr hbmem_sub_;
--- a/src/qr_detection/src/qr_usb_camera.cpp
+++ b/src/qr_detection/src/qr_usb_camera.cpp
@@ -55,14 +55,15 @@ public:
        std::bind(&QrUsbCameraNode::image_callback, this, std::placeholders::_1));

    // ── 订阅 sign4return (远程控制开关) ──
+    const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
-        "sign4return", 10,
+        "sign4return", sign_qos,
        std::bind(&QrUsbCameraNode::sign_callback, this, std::placeholders::_1));

    // ── 发布者 ──
    qr_pub_      = create_publisher<qr_detection::msg::QrDetection>("qr_detection", 10);
    qr_text_pub_ = create_publisher<std_msgs::msg::String>("qr_text", 10);
-    sign_pub_    = create_publisher<std_msgs::msg::Int32>("sign4return", 10);
+    sign_pub_    = create_publisher<std_msgs::msg::Int32>("sign4return", sign_qos);

    // ── 定时器: 检查缓冲区超时 ──
    buffer_timer_ = create_wall_timer(
@@ -202,6 +203,11 @@ private:
  {
    if (msg->data == 0) {
      detect_enabled_ = true;
+      qr_found_ = false;
+      qr_lost_count_ = 0;
+      prev_qr_text_.clear();
+      buffer_text_.clear();
+      buffer_time_acc_ = 0.0;
      RCLCPP_INFO(get_logger(), "QR detection ENABLED");
    } else if (msg->data == 5) {
      detect_enabled_ = false;
--- a/src/qr_detection/test/test_qr_dete_depth.cpp
+++ b/src/qr_detection/test/test_qr_dete_depth.cpp
@@ -0,0 +1,71 @@
+#include <algorithm>
+#include <chrono>
+#include <string>
+
+#include <gtest/gtest.h>
+#include <origincar_msg/srv/speak.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#define private public
+#define main qr_dete_depth_node_main
+#include "../src/qr_dete_depth.cpp"
+#undef main
+#undef private
+
+namespace
+{
+
+bool has_endpoint_type(
+  const std::vector<rclcpp::TopicEndpointInfo> & endpoints,
+  const std::string & topic_type)
+{
+  return std::any_of(
+    endpoints.begin(), endpoints.end(),
+    [&](const rclcpp::TopicEndpointInfo & endpoint) {
+      return endpoint.topic_type() == topic_type;
+    });
+}
+
+}  // namespace
+
+TEST(QrDeteDepthNode, exposesQrResultAndControlEndpoints)
+{
+  if (!rclcpp::ok()) {
+    rclcpp::init(0, nullptr);
+  }
+
+  rclcpp::NodeOptions options;
+  options.append_parameter_override("image_topic", "/depth_qr_test_image");
+  auto qr_node = std::make_shared<QrDeteDepthNode>(options);
+  auto graph_node = std::make_shared<rclcpp::Node>("qr_dete_depth_graph_test");
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(qr_node);
+  executor.add_node(graph_node);
+
+  std::vector<rclcpp::TopicEndpointInfo> image_subscribers;
+  std::vector<rclcpp::TopicEndpointInfo> sign_subscribers;
+  std::vector<rclcpp::TopicEndpointInfo> result_publishers;
+  const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
+  while (std::chrono::steady_clock::now() < deadline) {
+    executor.spin_some();
+    image_subscribers = graph_node->get_subscriptions_info_by_topic("/depth_qr_test_image");
+    sign_subscribers = graph_node->get_subscriptions_info_by_topic("/sign4return");
+    result_publishers = graph_node->get_publishers_info_by_topic("/qr_results");
+    if (has_endpoint_type(image_subscribers, "sensor_msgs/msg/Image") &&
+      has_endpoint_type(sign_subscribers, "std_msgs/msg/Int32") &&
+      has_endpoint_type(result_publishers, "std_msgs/msg/String"))
+    {
+      break;
+    }
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  EXPECT_TRUE(has_endpoint_type(image_subscribers, "sensor_msgs/msg/Image"));
+  EXPECT_TRUE(has_endpoint_type(sign_subscribers, "std_msgs/msg/Int32"));
+  EXPECT_TRUE(has_endpoint_type(result_publishers, "std_msgs/msg/String"));
+  ASSERT_NE(qr_node->tts_client_, nullptr);
+  EXPECT_STREQ(qr_node->tts_client_->get_service_name(), "/tts/speak");
+}
--- a/src/racing_control/AGENTS.md
+++ b/src/racing_control/AGENTS.md
@@ -0,0 +1,41 @@
+# 项目说明
+
+## 项目用途
+
+- 这是 RDKx5 赛车机器人的 ROS2 Humble 工作区。
+- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和备用轨迹保护节点，不在包内重新实现这些功能。
+
+## 环境准备
+
+- 通过 `ssh sunrise@192.168.10.210` 连接机器人。
+- 构建或运行前先 source ROS 和工作区：
+  `source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash`。
+- 部分相机/Hobot 流程可能还需要 source `/opt/tros/humble/setup.bash`。
+
+## 构建与测试
+
+- 只构建比赛总控：
+  `cd /home/sunrise/yiliao_ws && source /opt/ros/humble/setup.bash && source install/setup.bash && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON`。
+- 只测试比赛总控：
+  `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`。
+- `ament_xmllint` 会从 `download.ros.org` 读取 package schema；如果出现临时资源或网络错误，先重跑 xmllint 测试，不要急着修改 XML。
+
+## 架构
+
+- `/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`，属于备用链路；默认比赛流程不依赖它。
+- 比赛总控使用 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`。
+
+## 外部系统
+
+- `vlm_detect` 会调用由 launch 参数配置的外部 VLM 服务，该服务可能运行在另一台机器上。
+- `vlm_detect` 同时通过 `origincar_msg/srv/Speak` 提供 `/tts/speak` 语音服务。
+
+## 已知注意事项
+
+- 不要让多个节点同时发布 `/cmd_vel`，避免与 Nav2 controller 输出互相抢控制权。
+- 实际比赛点位依赖现场标定；不要把代码或 yaml 默认值当作真实赛场坐标。
+- 机器人底盘也订阅 `/sign4return`；总调度节点不要发布未明确约定的负数复位指令。
--- a/src/racing_control/CMakeLists.txt
+++ b/src/racing_control/CMakeLists.txt
@@ -1,17 +1,70 @@
 cmake_minimum_required(VERSION 3.8)
 project(racing_control)

+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()

 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_library(racing_control_core
+  src/candidate_waypoint_selector.cpp
+  src/final_home_fallback.cpp
+)
+target_include_directories(racing_control_core PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(racing_control_core
+  geometry_msgs
+  nav_msgs
+)
+
+add_executable(racing_control
+  src/racing_control.cpp
+  src/candidate_waypoint_selector.cpp
+  src/final_home_fallback.cpp
+)
+target_include_directories(racing_control PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(racing_control
+  geometry_msgs
+  nav_msgs
+  nav2_msgs
+  rclcpp
+  rclcpp_action
+  sensor_msgs
+  std_msgs
+)
+
+install(TARGETS
+  racing_control_core
+  racing_control
+  DESTINATION lib/${PROJECT_NAME}
+)
+install(DIRECTORY include/ DESTINATION include)
+install(DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}
+)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)

 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
@@ -20,6 +73,23 @@ if(BUILD_TESTING)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
+
+  ament_add_gtest(test_racing_control_helpers
+    test/test_racing_control_helpers.cpp
+  )
+  target_include_directories(test_racing_control_helpers PRIVATE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  )
+  ament_target_dependencies(test_racing_control_helpers
+    geometry_msgs
+    nav_msgs
+    rclcpp
+  )
+  target_sources(test_racing_control_helpers PRIVATE
+    src/candidate_waypoint_selector.cpp
+    src/final_home_fallback.cpp
+  )
+
  ament_lint_auto_find_test_dependencies()
 endif()

--- a/src/racing_control/config/racing_control.yaml
+++ b/src/racing_control/config/racing_control.yaml
@@ -0,0 +1,125 @@
+racing_control:
+  ros__parameters:
+    # Startup
+    auto_start: false
+    frame_id: odom
+    use_post_qr_pose: false
+    split_qr_to_vlm_segment: true
+    enable_vlm_image_relay: false
+    enable_dynamic_replanning: false
+    enable_recovery: true
+    enable_candidate_waypoint_selection: true
+    vlm_image_input_topic: /image
+    vlm_image_output_topic: /vlm_image
+
+    # Shared coordination topics
+    sign_topic: /sign4return
+    qr_result_topic: /qr_results
+    vlm_result_topic: /vlm_result
+    odom_topic: /odom_combined
+    recovery_cmd_vel_topic: /cmd_vel
+    global_costmap_topic: /global_costmap/costmap
+    trajectory_guard_input_topic: /trajectory_guard/input_path
+
+    # Nav2 actions and plugin IDs
+    navigate_action: /navigate_to_pose
+    compute_path_action: /compute_path_through_poses
+    follow_path_action: /follow_path
+    planner_id: GridBased
+    controller_id: FollowPath
+    goal_checker_id: ""
+    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: 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
+    # Used only when the initial ComputePathThroughPoses request fails.
+    planning_failure_backup_speed: -0.1
+    planning_failure_backup_distance: 0.03
+    planning_failure_backup_timeout_sec: 0.3
+    recovery_clear_wait_sec: 1.0
+    recovery_search_radius_m: 1.0
+    recovery_rear_clear_distance_m: 0.5
+    # 最终回家保护：普通 Nav2/恢复失败后，低速直控回 home，同时定时探测 Nav2 是否恢复。
+    enable_final_home_fallback: true
+    final_home_distance_tolerance: 0.2
+    final_home_yaw_tolerance: 0.3
+    final_home_forward_speed: 0.12
+    final_home_yaw_gain: 0.5
+    final_home_max_turn_speed: 0.7
+    final_home_nav_retry_interval_sec: 0.8
+    global_costmap_clear_service: global_costmap/clear_entirely_global_costmap
+    local_costmap_clear_service: local_costmap/clear_entirely_local_costmap
+    max_recovery_attempts: 2
+    circle_goal_tolerance: 0.50
+
+    # VLM capture:
+    #   stop         - stop at the VLM waypoint, trigger capture, then continue immediately.
+    #   pass_through - keep following the full route and trigger capture while passing nearby.
+    vlm_capture_mode: stop
+    pass_through_vlm_trigger_radius: 0.35
+
+    # /sign4return command values used by existing packages
+    sign_qr_enable: 0
+    sign_qr_disable: 5
+    sign_vlm_trigger: 9
+    sign_profile_normal: 10
+    sign_profile_task2: 11
+    vlm_trigger_repeat_count: 5
+    vlm_trigger_interval_sec: 0.5
+
+    # Candidate waypoint JSONs reuse the saved-point format.
+    # home is intentionally never replaced.
+    candidate_waypoint_json_dir: /home/sunrise/yiliao_ws/src/racing_control/config/waypoints
+    qr_candidate_group_name: qr
+    entry_candidate_group_name: entry
+    # Task-two route candidates are selected by point name only:
+    #   clockwise waypoint N        -> goal_NNN_1
+    #   counterclockwise waypoint N -> goal_NNN_2
+    # The VLM capture point is still selected by vlm_waypoint_number.
+
+    # Pose parameters are flat x/y/yaw-radians triples in frame_id.
+    qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392]
+    post_qr_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
+    entry_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
+
+    # The route's Nth waypoint is used as the VLM capture point.
+    # For the current point set, the 2nd waypoint is the VLM point.
+    vlm_waypoint_number: 2
+
+    # main_1.json: clockwise route, excluding home.
+    clockwise_waypoints:
+      - 0.7534956931109804
+      - 3.766501234923627
+      - 1.5592359900104606
+      - 3.8567885105264077
+      - 4.329424195458402
+      - 0.03997856199797794
+      - 2.5144339572664096
+      - 2.4337692660037766
+      - -1.6101462328054222
+    clockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
+
+    # main_2.json: counterclockwise route, excluding home.
+    counterclockwise_waypoints:
+      - 4.2128251001861265
+      - 3.7376334819031842
+      - 1.5450283923222128
+      - 1.1720794040064113
+      - 4.319801449605877
+      - 3.1112984779040144
+      - 2.4951887885861144
+      - 2.318297930897253
+      - -1.539556591616333
+    counterclockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
--- a/src/racing_control/config/waypoints/main.json
+++ b/src/racing_control/config/waypoints/main.json
@@ -0,0 +1,106 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-08-07T14:42:49.402Z",
+  "count": 2,
+  "points": [
+    {
+      "name": "qr",
+      "captured_at": "2026-08-07T14:42:33.118Z",
+      "received_at": null,
+      "yaw_degrees": 65.22491304455252,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.333107888975101,
+              "y": 1.028867429995691,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.5389539275964809,
+              "w": 0.8423352443821446
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 65.22491304455252
+      }
+    },
+    {
+      "name": "entry",
+      "captured_at": "2026-08-07T14:42:45.961Z",
+      "received_at": null,
+      "yaw_degrees": 87.09617569507752,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.504811372926262,
+              "y": 2.2798075935366624,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6889631335573172,
+              "w": 0.7247963856138373
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 87.09617569507752
+      }
+    }
+  ]
+}
--- a/src/racing_control/config/waypoints/main_1.json
+++ b/src/racing_control/config/waypoints/main_1.json
@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-08-07T14:44:10.775Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_001_1",
+      "captured_at": "2026-08-07T14:43:55.328Z",
+      "received_at": null,
+      "yaw_degrees": 89.33764149250207,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 0.7534956931109804,
+              "y": 3.766501234923627,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.7030077953706314,
+              "w": 0.7111821423855668
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 89.33764149250207
+      }
+    },
+    {
+      "name": "goal_002_1",
+      "captured_at": "2026-08-07T14:44:02.099Z",
+      "received_at": null,
+      "yaw_degrees": 2.2906028734862383,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 3.8567885105264077,
+              "y": 4.329424195458402,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.019987949834902125,
+              "w": 0.9998002209748693
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 2.2906028734862383
+      }
+    },
+    {
+      "name": "goal_003_1",
+      "captured_at": "2026-08-07T14:44:09.250Z",
+      "received_at": null,
+      "yaw_degrees": -92.25458353863968,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.5144339572664096,
+              "y": 2.4337692660037766,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": -0.720881318872454,
+              "w": 0.6930585286256215
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": -92.25458353863968
+      }
+    }
+  ]
+}
--- a/src/racing_control/config/waypoints/main_2.json
+++ b/src/racing_control/config/waypoints/main_2.json
@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-08-07T14:43:29.485Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_001_2",
+      "captured_at": "2026-08-07T14:43:14.866Z",
+      "received_at": null,
+      "yaw_degrees": 88.52360610794565,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.2128251001861265,
+              "y": 3.7376334819031842,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6979380047775942,
+              "w": 0.7161581818893582
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 88.52360610794565
+      }
+    },
+    {
+      "name": "goal_002_2",
+      "captured_at": "2026-08-07T14:43:20.968Z",
+      "received_at": null,
+      "yaw_degrees": 178.26427158937722,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 1.1720794040064113,
+              "y": 4.319801449605877,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.99988528505826,
+              "w": 0.015146508639358486
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 178.26427158937722
+      }
+    },
+    {
+      "name": "goal_003_2",
+      "captured_at": "2026-08-07T14:43:28.294Z",
+      "received_at": null,
+      "yaw_degrees": -88.21009502116203,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.4951887885861144,
+              "y": 2.318297930897253,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": -0.6959760577153663,
+              "w": 0.7180649880665239
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": -88.21009502116203
+      }
+    }
+  ]
+}
--- a/src/racing_control/config/waypoints/rest.json
+++ b/src/racing_control/config/waypoints/rest.json
@@ -0,0 +1,204 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-08-12T07:47:27.220Z",
+  "count": 4,
+  "points": [
+    {
+      "name": "qr",
+      "captured_at": "2026-08-12T07:13:09.418Z",
+      "received_at": null,
+      "yaw_degrees": 74.93151184050782,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.507415254237288,
+              "y": 0.7213320974576272,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6082871552778868,
+              "w": 0.7937170381968224
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 74.93151184050782
+      }
+    },
+    {
+      "name": "qr",
+      "captured_at": "2026-08-12T07:13:22.097Z",
+      "received_at": null,
+      "yaw_degrees": 59.82647997035565,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.375,
+              "y": 1.2403998940677967,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.4986880501001949,
+              "w": 0.8667815345790804
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 59.82647997035565
+      }
+    },
+    {
+      "name": "entry",
+      "captured_at": "2026-08-12T07:13:32.736Z",
+      "received_at": null,
+      "yaw_degrees": 87.99044618697887,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.791313559322034,
+              "y": 2.50099311440678,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6945983947098336,
+              "w": 0.7193977134148553
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 87.99044618697887
+      }
+    },
+    {
+      "name": "entry",
+      "captured_at": "2026-08-12T07:13:41.872Z",
+      "received_at": null,
+      "yaw_degrees": 87.79740183823421,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.3146186440677967,
+              "y": 2.8028998940677967,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6933854908702647,
+              "w": 0.7205668331602574
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 87.79740183823421
+      }
+    }
+  ]
+}
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Orange	bdddcc63cc	good	2026-08-12 21:07:21 +08:00
Orange	c30a810b12	test for gc	2026-08-11 19:38:10 +08:00
Orange	94609d938d	改了下地图和部分规划参数	2026-08-10 19:46:09 +08:00
Orange	b3d06e1ba8	40~60s完赛	2026-08-10 15:48:25 +08:00
Orange	07598f4b11	222	2026-08-09 21:02:42 +08:00
Orange	173670e194	gc更改前的推送	2026-08-06 18:57:54 +08:00
Orange	8991e5208e	modified: CLAUDE.md modified: src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp modified: src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py modified: src/navigation/obstacle_nav2/src/trajectory_guard.cpp modified: src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp modified: src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp modified: src/origincar_base/CMakeLists.txt modified: src/origincar_base/config/ekf.yaml new file: src/origincar_base/include/origincar_base/log.hpp modified: src/origincar_base/include/origincar_base/origincar_base.h modified: src/origincar_base/launch/base_serial.launch.py modified: src/origincar_base/launch/base_serial.launch.py.bak new file: src/origincar_base/src/log.cpp modified: src/origincar_base/src/origincar_base.cpp modified: src/origincar_base/src/origincar_base.cpp.bak new file: src/origincar_base/test/scan_odom_timing_logger_test.cpp modified: src/planner/REAL_ROBOT_RUNBOOK.md modified: src/planner/launch/odom_hybrid_astar.launch.py modified: src/planner/launch/real_hybrid_astar.launch.py modified: "src/planner/\346\223\215\344\275\234\346\211\213\345\206\214.md" modified: src/racing_control/CMakeLists.txt modified: src/racing_control/config/racing_control.yaml modified: src/racing_control/include/racing_control/racing_control.hpp modified: src/racing_control/launch/racing_control.launch.py modified: src/racing_control/package.xml new file: src/racing_control/src/racing_control copy.cpp modified: src/racing_control/src/racing_control.cpp modified: src/racing_control/test/test_racing_control_helpers.cpp modified: "src/racing_control/\347\202\271\344\275\215\346\240\274\345\274\217\350\275\254\346\215\242.md"	2026-08-06 16:34:53 +08:00
Orange	71bae6c68c	planner升级	2026-08-05 22:18:26 +08:00
Orange	ce49a0c5d3	vlm	2026-08-05 22:18:14 +08:00
Orange	b0e52bdb6f	111	2026-08-05 22:17:53 +08:00
Orange	3880d0265c	预选点数组的添加和发布节点	2026-08-05 22:17:05 +08:00
Orange	66acea5e87	给轨迹+MPPI版导航加入了障碍物检测	2026-08-05 22:15:35 +08:00
Orange	b4689b940f	给导航添加了动态调参的节点	2026-08-05 22:13:57 +08:00
Orange	1219a40077	添加了全套任务的包	2026-08-05 22:12:02 +08:00