good

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`、参数名和阶段名在计划内一致。

src/map/nav2_costmap_map.yaml

@@ -1,4 +1,4 @@
-image: nav2_costmap_binary_01.png
+image: map06.png
 mode: trinary
 resolution: 0.01
 origin: [0.0, 0.0, 0.0]

src/navigation/obstacle_nav2/config/nav2_profile_10.yaml

@@ -74,12 +74,12 @@ controller_server:
     controller_frequency: 10.0
     FollowPath:
       plugin: "nav2_mppi_controller::MPPIController"
-      time_steps: 36
+      time_steps: 30
-      model_dt: 0.10
+      model_dt: 0.1
       batch_size: 700
-      vx_std: 0.22
+      vx_std: 0.50
       vy_std: 0.0
-      wz_std: 0.45
+      wz_std: 0.80
       vx_max: 1.00
       vx_min: -0.75
       vy_max: 0.0
@@ -117,8 +117,8 @@ controller_server:
       CostCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 3.81
+        cost_weight: 1.8
-        critical_cost: 300.0
+        critical_cost: 250.0
         consider_footprint: true
         collision_cost: 100000.0
         near_goal_distance: 1.0
@@ -166,8 +166,13 @@ local_costmap:
       resolution: 0.05
       footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
       footprint_padding: 0.02
-      track_unknown_space: false
+      track_unknown_space: true
-      plugins: ["obstacle_array_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        enabled: true
+        map_subscribe_transient_local: true
+        subscribe_to_updates: false
       obstacle_array_layer:
         plugin: "obstacle_nav2::ObstacleArrayLayer"
         enabled: true
@@ -181,7 +186,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.35
+        inflation_radius: 0.25
       always_send_full_costmap: True
   local_costmap_client:
     ros__parameters:
@@ -253,9 +258,9 @@ planner_server:
       analytic_expansion_max_length: 3.0
       minimum_turning_radius: 0.40
       reverse_penalty: 3.0
-      change_penalty: 0.0
+      change_penalty: 1.0
       non_straight_penalty: 1.2
-      cost_penalty: 2.0
+      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.
@@ -315,7 +320,7 @@ robot_state_publisher:
 
 waypoint_follower:
   ros__parameters:
-    loop_rate: 20
+    loop_rate: 10
     use_sim_time: False
     stop_on_failure: false
     waypoint_task_executor_plugin: "wait_at_waypoint"
@@ -330,7 +335,7 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [1.00, 0.0, 2.0]
+    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]

src/navigation/obstacle_nav2/config/nav2_profile_11.yaml

@@ -1,12 +1,12 @@
 # ============================================================================
-# nav2_params_basic_tracking.yaml
+# nav2_params.yaml — Odometry-only obstacle navigation
 #
-# Basic stable trajectory-following profile for obstacle_nav2.
+# No static map, no AMCL, no SLAM.
-# This file is intentionally conservative: low speed, longer MPPI horizon,
+# Both costmaps are rolling windows in odom. The launch file can rewrite every
-# smoother acceleration, moderate path tracking weights, and Ackermann limits.
+# global_frame leaf when a different connected odometry frame is required.
-#
+# Global planner: Smac Hybrid A* (Reeds-Shepp)
-# It does not replace nav2_params.yaml. To use it, launch Nav2 with this params
+# Local controller: MPPI (Ackermann)
-# file or add a launch argument later.
+# 速度快但是雷达容易掉
 # ============================================================================
 
 bt_navigator:
@@ -17,6 +17,7 @@ bt_navigator:
     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
@@ -70,99 +71,82 @@ bt_navigator_rclcpp_node:
 controller_server:
   ros__parameters:
     use_sim_time: False
-    controller_frequency: 20.0
+    controller_frequency: 10.0
-
     FollowPath:
       plugin: "nav2_mppi_controller::MPPIController"
-
+      time_steps: 30
-      # Longer horizon and moderate sampling make turns easier to discover.
+      model_dt: 0.1
-      time_steps: 48
+      batch_size: 700
-      model_dt: 0.05
+      vx_std: 0.40
-      batch_size: 1000
-      iteration_count: 1
-
-      # Conservative velocity exploration. Speed is deliberately not optimized.
-      vx_std: 0.20
       vy_std: 0.0
-      wz_std: 0.85
+      wz_std: 0.70
-      vx_max: 0.50
+      vx_max: 1.00
-      vx_min: -0.15
+      vx_min: -0.75
       vy_max: 0.0
-      wz_max: 1.8
+      wz_max: 1.5
-
+      iteration_count: 1
-      # Lower temperature makes selection steadier; gamma damps rough controls.
+      temperature: 0.3
-      temperature: 0.25
+      gamma: 0.015
-      gamma: 0.02
-
       motion_model: "Ackermann"
       visualize: false
       TrajectoryVisualizer:
         trajectory_step: 5
         time_step: 3
       AckermannConstraints:
-        min_turning_r: 0.60
+        min_turning_r: 0.4
-
       critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
-
       ConstraintCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 3.0
+        cost_weight: 4.0
-
-      CostCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 6.0
-        critical_cost: 250.0
-        consider_footprint: true
-        collision_cost: 1000000.0
-        near_goal_distance: 0.8
-        trajectory_point_step: 2
-
       GoalCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 2.0
+        cost_weight: 5.0
-        threshold_to_consider: 1.0
+        threshold_to_consider: 1.4
-
       GoalAngleCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 2.0
+        cost_weight: 3.0
-        threshold_to_consider: 0.45
+        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: 8.0
+        cost_weight: 10.0
-        max_path_occupancy_ratio: 0.07
+        max_path_occupancy_ratio: 0.05
         trajectory_point_step: 4
-        threshold_to_consider: 0.45
+        threshold_to_consider: 0.5
-        offset_from_furthest: 8
+        offset_from_furthest: 20
         use_path_orientations: false
-
       PathFollowCritic:
         enabled: true
         cost_power: 1
         cost_weight: 4.0
-        offset_from_furthest: 4
+        offset_from_furthest: 10
-        threshold_to_consider: 1.0
+        threshold_to_consider: 1.4
-
       PathAngleCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 5.0
+        cost_weight: 2.0
         offset_from_furthest: 5
-        threshold_to_consider: 0.45
+        threshold_to_consider: 0.5
-        max_angle_to_furthest: 1.2
+        max_angle_to_furthest: 1.0
         forward_preference: false
 
-      PreferForwardCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 1.0
-        threshold_to_consider: 0.4
-
 controller_server_rclcpp_node:
   ros__parameters:
     use_sim_time: False
@@ -170,31 +154,35 @@ controller_server_rclcpp_node:
 local_costmap:
   local_costmap:
     ros__parameters:
-      update_frequency: 8.0
+      update_frequency: 5.0
-      publish_frequency: 4.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: 4
+      width: 3
-      height: 4
+      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
+      track_unknown_space: true
-      plugins: ["obstacle_array_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        enabled: true
+        map_subscribe_transient_local: true
+        subscribe_to_updates: false
       obstacle_array_layer:
         plugin: "obstacle_nav2::ObstacleArrayLayer"
         enabled: true
         topic: /obstacles
-        obstacle_timeout: 1.0
+        obstacle_timeout: 0.5
         transform_tolerance: 0.2
         default_obstacle_radius: 0.05
         minimum_obstacle_radius: 0.02
-        maximum_obstacle_radius: 0.06
+        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
@@ -210,8 +198,8 @@ local_costmap:
 global_costmap:
   global_costmap:
     ros__parameters:
-      update_frequency: 5.0
+      update_frequency: 1.0
-      publish_frequency: 3.0
+      publish_frequency: 1.0
       transform_tolerance: 0.5
       global_frame: map
       robot_base_frame: base_footprint
@@ -233,17 +221,16 @@ global_costmap:
         plugin: "obstacle_nav2::ObstacleArrayLayer"
         enabled: true
         topic: /obstacles
-        obstacle_timeout: 1.0
+        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
-        retain_previous_on_empty_snapshot: true
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
+        cost_scaling_factor: 2.0
-        inflation_radius: 0.45
+        inflation_radius: 0.3
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
@@ -260,7 +247,7 @@ planner_server:
       plugin: "nav2_smac_planner/SmacPlannerHybrid"
       downsample_costmap: false
       downsampling_factor: 1
-      tolerance: 0.20
+      tolerance: 0.15
       allow_unknown: false
       max_iterations: 1000000
       max_on_approach_iterations: 1000
@@ -268,21 +255,23 @@ planner_server:
       motion_model_for_search: "REEDS_SHEPP"
       angle_quantization_bins: 72
       analytic_expansion_ratio: 3.5
-      analytic_expansion_max_length: 2.0
+      analytic_expansion_max_length: 3.0
-      minimum_turning_radius: 0.60
+      minimum_turning_radius: 0.40
-      reverse_penalty: 2.2
+      reverse_penalty: 3.0
-      change_penalty: 0.0
+      change_penalty: 1.0
-      non_straight_penalty: 0.8
+      non_straight_penalty: 1.2
-      cost_penalty: 5.0
+      cost_penalty: 6.0
-      retrospective_penalty: 0.01
+      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.35
+        w_smooth: 0.3
-        w_data: 0.25
+        w_data: 0.2
         tolerance: 1.0e-10
         do_refinement: true
         refinement_num: 2
@@ -311,8 +300,8 @@ behavior_server:
       plugin: "nav2_behaviors/Spin"
     backup:
       plugin: "nav2_behaviors/BackUp"
-      backup_dist: 0.30
+      backup_dist: 0.8
-      backup_speed: 0.08
+      backup_speed: 0.18
     wait:
       plugin: "nav2_behaviors/Wait"
       wait_duration: 0.5
@@ -321,9 +310,9 @@ behavior_server:
     transform_tolerance: 0.5
     use_sim_time: False
     simulate_ahead_time: 2.0
-    max_rotational_vel: 0.8
+    max_rotational_vel: 1.0
-    min_rotational_vel: 0.2
+    min_rotational_vel: 0.4
-    rotational_acc_lim: 1.5
+    rotational_acc_lim: 3.2
 
 robot_state_publisher:
   ros__parameters:
@@ -331,7 +320,7 @@ robot_state_publisher:
 
 waypoint_follower:
   ros__parameters:
-    loop_rate: 20
+    loop_rate: 10
     use_sim_time: False
     stop_on_failure: false
     waypoint_task_executor_plugin: "wait_at_waypoint"
@@ -346,18 +335,11 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.50, 0.0, 1.8]
+    max_velocity: [1.25, 0.0, 2.0]
-    min_velocity: [-0.15, 0.0, -1.8]
+    min_velocity: [-0.75, 0.0, -2.0]
-    max_accel: [0.50, 0.0, 1.50]
+    max_accel: [3.73, 0.0, 3.2]
-    max_decel: [-0.60, 0.0, -1.80]
+    max_decel: [-1.1, 0.0, -4.5]
     odom_topic: /odom_combined
     odom_duration: 0.1
-    deadband_velocity: [0.02, 0.0, 0.02]
+    deadband_velocity: [0.03, 0.0, 0.03]
     velocity_timeout: 1.0
-
-lifecycle_manager_navigation:
-  ros__parameters:
-    use_sim_time: False
-    autostart: True
-    service_timeout: 10.0
-    bond_timeout: 8.0

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()

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

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],
+        ),
+    ])

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',
+        executable='qr_detect',
-        name='qr_dete_node',
+        name='qr_detect',
         output='screen'
     )
 

src/qr_detection/src/qr_dete_depth.cpp

@@ -1,173 +1,208 @@
-#include <memory>
+#include "origincar_msg/srv/speak.hpp"
-#include <iostream>
-#include <opencv2/opencv.hpp>
-#include <zbar.h>
 #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()
+  explicit QrDeteDepthNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
-  : Node("qr_detection"), detect_qr_code_(true)  // 默认检测二维码
+  : Node("qr_dete_depth", options), enabled_(true)
   {
-    use_buffer = this->declare_parameter("use_buffer", true); // 声明参数，默认使用缓存
+    image_topic_ = declare_parameter<std::string>("image_topic", "/aurora/rgb/image_raw");
-    // 订阅 /aurora/rgb/image_raw（sensor_msgs::msg::Image 格式）
+    use_buffer_ = declare_parameter<bool>("use_buffer", true);
-    subscription_image_ =
+    tts_service_ = declare_parameter<std::string>("tts_service", "/tts/speak");
-      this->create_subscription<sensor_msgs::msg::Image>(
-        "/aurora/rgb/image_raw",
-        10,
-        std::bind(&MinimalHbmemSubscriber::image_callback, this, std::placeholders::_1));
 
-    // 创建订阅器，订阅 'sign4return' 话题
+    image_sub_ = create_subscription<sensor_msgs::msg::Image>(
-    // subscription_sign_ =
+      image_topic_, rclcpp::SensorDataQoS(),
-    //   this->create_subscription<std_msgs::msg::Int32>(
+      std::bind(&QrDeteDepthNode::image_callback, this, std::placeholders::_1));
-    //     "sign4return",
-    //     10,
-    //     std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1));
-    // 创建发布器，发布 'sign4return' 话题
-    sign_publisher_ = this->create_publisher<std_msgs::msg::Int32>("sign4return", 10);
 
-    // 创建 publisher，topic 为 "qr_results"
+    const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
-    publisher_ =
+    sign_sub_ = create_subscription<std_msgs::msg::Int32>(
-      this->create_publisher<std_msgs::msg::String>("qr_results", 10);
+      "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_)
+    if (!enabled_) {
-    {
-      RCLCPP_INFO(this->get_logger(), "QR detection is disabled");
       return;
     }
 
-    // 创建 Clock 对象并获取当前时间
+    log_frame_delay(msg);
-    auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
-    auto now = clock->now();
 
-    // 转换消息时间戳为 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(
-    zbar::Image zbar_image(gray_image.cols, gray_image.rows, "Y800",
+      gray_image.cols, gray_image.rows, "Y800",
-                           gray_image.data, gray_image.cols * gray_image.rows);
+      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\"",
+        RCLCPP_INFO(
-                    symbol->get_type_name().c_str(), symbol->get_data().c_str());
+          get_logger(), "Decoded %s symbol \"%s\"",
+          symbol->get_type_name().c_str(), symbol->get_data().c_str());
       }
-      qr_results_buff_ = qr_results; // 更新缓存
+      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);
     } 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 结果
+    const std::string result_text = use_buffer_ ? qr_results_buff_ : qr_results;
-    auto message = std_msgs::msg::String();
+    if (result_text.empty()) {
-    if (use_buffer){
+      return;
-      message.data = qr_results_buff_;
-    } 
-    else{
-      message.data = qr_results;
     }
-    publisher_->publish(message);
+
+    publish_result(result_text);
   }
 
-  // 消息回调函数，处理 sign4return 话题
+  void publish_result(const std::string & qr_data)
-  // void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
+  {
-  // {
+    auto out = std_msgs::msg::String();
-  //   if (msg->data == 0)
+    if (is_numeric(qr_data)) {
-  //   {
+      out.data = qr_data + " " + (((qr_data.back() - '0') % 2 == 1) ? "顺时针" : "逆时针");
-  //     detect_qr_code_ = true; // 启动二维码检测
+    } else {
-  //     RCLCPP_INFO(this->get_logger(), "QR detection started");
+      out.data = qr_data;
-  //   }
+    }
-  //   else if (msg->data == 5)
-  //   {
-  //     detect_qr_code_ = false; // 停止二维码检测
-  //     RCLCPP_INFO(this->get_logger(), "QR detection stopped");
-  //   }
-  // }
 
-  // 识别结果合法性检测
+    if (out.data != last_) {
-  bool ResultLegal(std::string& input){
+      RCLCPP_INFO(get_logger(), "QR: %s", out.data.c_str());
-    // 合法字符串集合
+      last_ = out.data;
-    static const std::unordered_set<std::string> kValidStrings = {
+    }
-        "1", "2", "顺时针", "逆时针", "顺", "逆"
+
-    };
+    result_pub_->publish(out);
-    return kValidStrings.find(input) != kValidStrings.end();
+    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 订阅器
+  void speak_once(const std::string & text)
-  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_image_;
+  {
-  // sign4return 订阅器
+    if (text == last_spoken_) {
-  // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
+      return;
-  // sign4return 发布器
+    }
-  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_publisher_;
+    last_spoken_ = text;
-  // QR code results 发布器
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
 
-  // 参数
+    if (!tts_client_->service_is_ready()) {
-  bool use_buffer;
+      static int warn_count = 0;
-  // 二维码检测标志位
+      if (warn_count++ < 3) {
-  bool detect_qr_code_;
+        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;
 }

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"); }
+    if (msg->data == 0) {
-    else if (msg->data == 5) { enabled_ = false; RCLCPP_INFO(get_logger(), "OFF"); }
+      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_;

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");  }
+    if (msg->data == 0) {
-    else if (msg->data == 5) { detect_enabled_ = false; RCLCPP_INFO(this->get_logger(), "OFF"); }
+      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_;

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;

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");
+}

src/racing_control/CMakeLists.txt

@@ -20,6 +20,7 @@ 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>
@@ -33,6 +34,7 @@ ament_target_dependencies(racing_control_core
 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>
@@ -85,6 +87,7 @@ if(BUILD_TESTING)
   )
   target_sources(test_racing_control_helpers PRIVATE
     src/candidate_waypoint_selector.cpp
+    src/final_home_fallback.cpp
   )
 
   ament_lint_auto_find_test_dependencies()

src/racing_control/config/racing_control.yaml

@@ -51,6 +51,14 @@ racing_control:
     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
@@ -76,9 +84,10 @@ racing_control:
     candidate_waypoint_json_dir: /home/sunrise/yiliao_ws/src/racing_control/config/waypoints
     qr_candidate_group_name: qr
     entry_candidate_group_name: entry
-    vlm_candidate_group_name: goal_005
+    # Task-two route candidates are selected by point name only:
-    clockwise_candidate_source_file: main_1.json
+    #   clockwise waypoint N        -> goal_NNN_1
-    counterclockwise_candidate_source_file: main_2.json
+    #   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]

src/racing_control/config/waypoints/main_1.json

@@ -5,7 +5,7 @@
   "count": 3,
   "points": [
     {
-      "name": "goal_004",
+      "name": "goal_001_1",
       "captured_at": "2026-08-07T14:43:55.328Z",
       "received_at": null,
       "yaw_degrees": 89.33764149250207,
@@ -54,7 +54,7 @@
       }
     },
     {
-      "name": "goal_005",
+      "name": "goal_002_1",
       "captured_at": "2026-08-07T14:44:02.099Z",
       "received_at": null,
       "yaw_degrees": 2.2906028734862383,
@@ -103,7 +103,7 @@
       }
     },
     {
-      "name": "goal_006",
+      "name": "goal_003_1",
       "captured_at": "2026-08-07T14:44:09.250Z",
       "received_at": null,
       "yaw_degrees": -92.25458353863968,

src/racing_control/config/waypoints/main_2.json

@@ -5,7 +5,7 @@
   "count": 3,
   "points": [
     {
-      "name": "goal_001",
+      "name": "goal_001_2",
       "captured_at": "2026-08-07T14:43:14.866Z",
       "received_at": null,
       "yaw_degrees": 88.52360610794565,
@@ -54,7 +54,7 @@
       }
     },
     {
-      "name": "goal_002",
+      "name": "goal_002_2",
       "captured_at": "2026-08-07T14:43:20.968Z",
       "received_at": null,
       "yaw_degrees": 178.26427158937722,
@@ -103,7 +103,7 @@
       }
     },
     {
-      "name": "goal_003",
+      "name": "goal_003_2",
       "captured_at": "2026-08-07T14:43:28.294Z",
       "received_at": null,
       "yaw_degrees": -88.21009502116203,

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
+      }
+    }
+  ]
+}

src/racing_control/include/racing_control/final_home_fallback.hpp

@@ -0,0 +1,33 @@
+#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_

src/racing_control/include/racing_control/racing_control.hpp

@@ -4,6 +4,8 @@
 #include <cmath>
 #include <cctype>
 #include <cstddef>
+#include <iomanip>
+#include <sstream>
 #include <stdexcept>
 #include <string>
 #include <vector>
@@ -139,6 +141,31 @@ inline QrTransitTarget qrTransitTargetAfterRecognition(const bool use_post_qr_po
   return use_post_qr_pose ? QrTransitTarget::PostQr : QrTransitTarget::Entry;
 }
 
+inline std::string routeCandidateSuffix(const RouteDirection direction)
+{
+  if (direction == RouteDirection::Clockwise) {
+    return "_1";
+  }
+  if (direction == RouteDirection::Counterclockwise) {
+    return "_2";
+  }
+  return "";
+}
+
+inline std::string routeCandidateGroupName(
+  const std::size_t route_waypoint_index,
+  const RouteDirection direction)
+{
+  const auto suffix = routeCandidateSuffix(direction);
+  if (suffix.empty()) {
+    return "";
+  }
+
+  std::ostringstream out;
+  out << "goal_" << std::setw(3) << std::setfill('0') << (route_waypoint_index + 1) << suffix;
+  return out.str();
+}
+
 inline bool shouldPublishVlmImageFrame(
   const bool enable_vlm_image_relay, const bool has_latest_image)
 {

src/racing_control/src/final_home_fallback.cpp

@@ -0,0 +1,61 @@
+#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

src/racing_control/src/racing_control.cpp

@@ -30,6 +30,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "racing_control/candidate_waypoint_selector.hpp"
+#include "racing_control/final_home_fallback.hpp"
 #include "sensor_msgs/msg/compressed_image.hpp"
 #include "std_msgs/msg/int32.hpp"
 #include "std_msgs/msg/string.hpp"
@@ -55,6 +56,7 @@ enum class Stage
   SwitchToNormalProfile,
   WaitForVlm,
   ReturnOrigin,
+  FinalHomeFallback,
   Finished,
   Failed
 };
@@ -100,6 +102,8 @@ const char * stageName(const Stage stage)
       return "图生文/TTS";
     case Stage::ReturnOrigin:
       return "返回原点";
+    case Stage::FinalHomeFallback:
+      return "最终回家保护";
     case Stage::Finished:
       return "比赛完成";
     case Stage::Failed:
@@ -142,7 +146,8 @@ public:
   {
     loadParameters();
 
-    sign_pub_ = create_publisher<std_msgs::msg::Int32>(sign_topic_, 10);
+    const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
+    sign_pub_ = create_publisher<std_msgs::msg::Int32>(sign_topic_, sign_qos);
     startStartupQrEnablePublisher();
     recovery_cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
       recovery_cmd_vel_topic_, 10);
@@ -237,6 +242,8 @@ private:
     enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
     enable_dynamic_replanning_ = declare_parameter<bool>("enable_dynamic_replanning", true);
     enable_recovery_ = declare_parameter<bool>("enable_recovery", true);
+    enable_final_home_fallback_ =
+      declare_parameter<bool>("enable_final_home_fallback", true);
     enable_candidate_waypoint_selection_ =
       declare_parameter<bool>("enable_candidate_waypoint_selection", true);
     vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
@@ -273,6 +280,18 @@ private:
     recovery_search_radius_m_ = declare_parameter<double>("recovery_search_radius_m", 1.0);
     recovery_rear_clear_distance_m_ =
       declare_parameter<double>("recovery_rear_clear_distance_m", 0.5);
+    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);
 
     const auto vlm_capture_mode =
       declare_parameter<std::string>("vlm_capture_mode", "stop");
@@ -328,12 +347,6 @@ private:
     qr_candidate_group_name_ = declare_parameter<std::string>("qr_candidate_group_name", "qr");
     entry_candidate_group_name_ =
       declare_parameter<std::string>("entry_candidate_group_name", "entry");
-    vlm_candidate_group_name_ =
-      declare_parameter<std::string>("vlm_candidate_group_name", "goal_005");
-    clockwise_candidate_source_file_ = declare_parameter<std::string>(
-      "clockwise_candidate_source_file", "main_1.json");
-    counterclockwise_candidate_source_file_ = declare_parameter<std::string>(
-      "counterclockwise_candidate_source_file", "main_2.json");
     candidate_selector_.setGroups(
       loadCandidateWaypointGroups(candidate_waypoint_json_dir_, frame_id_));
   }
@@ -413,6 +426,11 @@ private:
       return;
     }
 
+    if (stage_ == Stage::FinalHomeFallback) {
+      tickFinalHomeFallback();
+      return;
+    }
+
     if (recovery_in_progress_) {
       return;
     }
@@ -509,6 +527,11 @@ private:
   }
 
   void failRace(const std::string & reason)
+  {
+    startFinalHomeFallback(reason);
+  }
+
+  void startFinalHomeFallback(const std::string & reason)
   {
     stage_ = Stage::Failed;
     recovery_in_progress_ = false;
@@ -523,7 +546,17 @@ private:
     cancelActiveFollowGoal();
     publishRecoveryVelocity(0.0);
     publishSign(sign_qr_disable_);
-    RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
+    if (!enable_final_home_fallback_) {
+      RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
+      return;
+    }
+
+    final_home_nav_retry_in_flight_ = false;
+    last_final_home_nav_retry_time_ =
+      now() - rclcpp::Duration::from_seconds(final_home_nav_retry_interval_sec_);
+    startStage(Stage::FinalHomeFallback, 0.0);
+    RCLCPP_ERROR(
+      get_logger(), "entering final home fallback instead of race failed: %s", reason.c_str());
   }
 
   void finishRace()
@@ -682,15 +715,9 @@ private:
       active_segment_waypoints_.push_back(post_qr_pose_);
     }
 
-    auto route_waypoints = route.waypoints;
+    auto route_waypoints = routeWaypointsWithCandidates(route.waypoints);
-    if (vlm_index < route_waypoints.size()) {
-      route_waypoints[vlm_index] = selectCandidatePose(
-        vlm_candidate_group_name_, preferredCandidateSourceFile(),
-        route_waypoints[vlm_index]).value_or(route_waypoints[vlm_index]);
-    }
     const auto selected_entry_pose = selectCandidatePose(
-      entry_candidate_group_name_, preferredCandidateSourceFile(),
+      entry_candidate_group_name_, "", entry_pose_).value_or(entry_pose_);
-      entry_pose_).value_or(entry_pose_);
     const auto qr_segment = routeWaypointsAfterQr(
       selected_entry_pose, route_waypoints, vlm_index, split_at_vlm);
     active_segment_waypoints_.insert(
@@ -708,7 +735,8 @@ private:
     const auto vlm_index = vlmWaypointIndex(route);
     resetRouteRecoveryState();
     active_segment_ = RouteSegment::AfterVlm;
-    active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1);
+    active_segment_waypoints_ =
+      remainingWaypoints(routeWaypointsWithCandidates(route.waypoints), vlm_index + 1);
     active_segment_waypoints_.push_back(route.home_pose);
     active_segment_next_waypoint_index_ = 0;
     publishSign(sign_profile_normal_);
@@ -1269,6 +1297,40 @@ private:
     recovery_cmd_vel_pub_->publish(cmd);
   }
 
+  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);
+  }
+
   void cancelActiveFollowGoal()
   {
     ++follow_goal_generation_;
@@ -1282,7 +1344,7 @@ private:
   {
     startStage(Stage::ReturnOrigin, navigation_timeout_sec_);
     sendNavigateGoal(
-      selectedRoute().home_pose, [this](const bool ok) {
+      fallbackHomePose(), [this](const bool ok) {
         if (!ok) {
           finishStage("navigation failed");
           failRace("failed to return origin");
@@ -1345,6 +1407,30 @@ private:
     navigate_client_->async_send_goal(goal, options);
   }
 
+  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();
+    publishRecoveryVelocity(0.0);
+    RCLCPP_INFO(
+      get_logger(), "final home fallback switching back to Nav2 home navigation: %s",
+      poseSummary(home).c_str());
+    runReturnOrigin();
+  }
+
   void retryNavigateGoalOrFinish(
     const geometry_msgs::msg::PoseStamped & pose,
     std::function<void(bool)> on_done,
@@ -1474,15 +1560,30 @@ private:
     throw std::runtime_error("route direction is not selected");
   }
 
-  std::string preferredCandidateSourceFile() const
+  geometry_msgs::msg::PoseStamped fallbackHomePose() const
   {
-    if (selected_direction_ == RouteDirection::Clockwise) {
+    if (selected_direction_ == RouteDirection::Clockwise ||
-      return clockwise_candidate_source_file_;
+      selected_direction_ == RouteDirection::Counterclockwise)
+    {
+      return selectedRoute().home_pose;
     }
-    if (selected_direction_ == RouteDirection::Counterclockwise) {
+    return clockwise_route_.home_pose;
-      return counterclockwise_candidate_source_file_;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> routeWaypointsWithCandidates(
+    const std::vector<geometry_msgs::msg::PoseStamped> & route_waypoints)
+  {
+    auto selected_waypoints = route_waypoints;
+    for (std::size_t index = 0; index < selected_waypoints.size(); ++index) {
+      const auto group_name = routeCandidateGroupName(index, selected_direction_);
+      if (group_name.empty()) {
+        continue;
+      }
+      selected_waypoints[index] =
+        selectCandidatePose(group_name, "", selected_waypoints[index]).value_or(
+        selected_waypoints[index]);
     }
-    return "";
+    return selected_waypoints;
   }
 
   bool routeSegmentReached() const
@@ -1561,14 +1662,17 @@ private:
   bool enable_vlm_image_relay_{false};
   bool enable_dynamic_replanning_{true};
   bool enable_recovery_{true};
+  bool enable_final_home_fallback_{true};
   bool enable_candidate_waypoint_selection_{true};
   bool qr_detection_disabled_{false};
   bool dynamic_replan_in_flight_{false};
   bool recovery_in_progress_{false};
+  bool final_home_nav_retry_in_flight_{false};
   rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
   rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME};
   rclcpp::Time last_dynamic_replan_time_{0, 0, RCL_ROS_TIME};
   rclcpp::Time recovery_start_time_{0, 0, RCL_ROS_TIME};
+  rclcpp::Time last_final_home_nav_retry_time_{0, 0, RCL_ROS_TIME};
   double stage_timeout_sec_{0.0};
 
   std::string frame_id_;
@@ -1592,9 +1696,6 @@ private:
   std::string candidate_waypoint_json_dir_;
   std::string qr_candidate_group_name_;
   std::string entry_candidate_group_name_;
-  std::string vlm_candidate_group_name_;
-  std::string clockwise_candidate_source_file_;
-  std::string counterclockwise_candidate_source_file_;
 
   double navigation_timeout_sec_{120.0};
   double path_planning_timeout_sec_{30.0};
@@ -1617,6 +1718,12 @@ private:
   double recovery_active_timeout_sec_{0.0};
   double recovery_search_radius_m_{1.0};
   double recovery_rear_clear_distance_m_{0.5};
+  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};
   double pass_through_vlm_trigger_radius_{0.35};
   double circle_goal_tolerance_{0.30};
 

src/racing_control/test/test_racing_control_helpers.cpp

@@ -6,6 +6,7 @@
 #include "gtest/gtest.h"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "racing_control/candidate_waypoint_selector.hpp"
+#include "racing_control/final_home_fallback.hpp"
 #include "racing_control/racing_control.hpp"
 
 namespace
@@ -87,6 +88,30 @@ TEST(RacingControlHelpers, SelectsPostQrTransitTargetWhenEnabled)
     racing_control::QrTransitTarget::Entry);
 }
 
+TEST(RacingControlHelpers, BuildsDirectionSpecificRouteCandidateNames)
+{
+  EXPECT_EQ(
+    racing_control::routeCandidateSuffix(racing_control::RouteDirection::Clockwise),
+    "_1");
+  EXPECT_EQ(
+    racing_control::routeCandidateSuffix(racing_control::RouteDirection::Counterclockwise),
+    "_2");
+  EXPECT_EQ(
+    racing_control::routeCandidateGroupName(0, racing_control::RouteDirection::Clockwise),
+    "goal_001_1");
+  EXPECT_EQ(
+    racing_control::routeCandidateGroupName(1, racing_control::RouteDirection::Clockwise),
+    "goal_002_1");
+  EXPECT_EQ(
+    racing_control::routeCandidateGroupName(1, racing_control::RouteDirection::Counterclockwise),
+    "goal_002_2");
+  EXPECT_EQ(
+    racing_control::routeCandidateGroupName(11, racing_control::RouteDirection::Counterclockwise),
+    "goal_012_2");
+  EXPECT_TRUE(
+    racing_control::routeCandidateGroupName(0, racing_control::RouteDirection::Unknown).empty());
+}
+
 TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable)
 {
   EXPECT_TRUE(racing_control::shouldPublishVlmImageFrame(true, true));
@@ -386,3 +411,51 @@ TEST(CandidateWaypointSelector, FindsNearestFreeRecoveryCommand)
   EXPECT_GT(command->linear_x, 0.0);
   EXPECT_DOUBLE_EQ(command->angular_z, 0.0);
 }
+
+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);
+}

技术报告.md

@@ -0,0 +1,164 @@
+# 一、性质
+
+该技术报告面向2026年智能汽车竞赛的智慧医疗子项目，全称为**第二十一届全国大学生智能汽车竞赛—地瓜机器人创意组智慧医疗赛项**
+
+## 内容要求（规则规定）
+
+- 规则适配与系统调整情况
+- 关键技术实现与工程经验总结
+- 比赛过程中遇到的主要问题及解决思路
+
+
+
+# 二、内容框架
+
+## 1. 队伍简介
+
+### 1.1 队伍成员
+
+曾博文（大二）、郭诚（大二）、段华祺（大二）、陈有源（大三）
+
+### 1.2 参赛用车及其改装
+
+使用了origincar pro作为我们的比赛用车，拆卸了深度相机，加装了**镭神N10雷达-串口版**、外接扬声器、外置网卡
+
+## 2. 系统架构
+
+### 2.1 流程图
+
+![草稿](./草稿.assets/草稿.png)
+
+> 注：该流程图仅为示例，请兼顾常用机器人系统流程图制作规范以及美观性、可读性，利用合适的工具重新绘制一份流程图/架构图。
+
+### 2.2 关键技术模块实现
+
+#### 2.2.1 利用雷达点云实现的四周墙壁拟合的重定位技术
+
+解释技术关键要点、如何确定墙壁方程、如何规定前后左右语义等等
+
+#### 2.2.2 利用雷达点云与卡尔曼滤波实现的障碍物检测系统
+
+在点云观测中，障碍物的外接圆拟合半径通常在4cm~6cm之间，由于场上仅有圆锥一种障碍物，因此直接将外接圆适配的目标标记为“障碍物”，并发布在/obstacles话题中
+
+由于比赛时速度较快且雷达扫描与处理频率限制，很容易出现车移动但障碍物位置未更新的情况。于是我们将障碍物坐标建立为一个观测系统，基于一些数据……，从而能实时预测高速运动状态下的障碍物实际位置
+
+#### 2.2.3 基于扩展卡尔曼滤波的定位系统
+
+解释origincar_base中如何将imu、电机编码器、雷达拟合位置融合并实现实时定位
+
+#### 2.2.4 二维码识别
+
+解释qr_detection中如何识别二维码
+
+#### 2.2.5 图生文-vlm大模型与tts语音模块
+![alt text](技术报告.Assets/vlm流程.png)
+##### 概述
+在 RDK X5 上部署视·觉语言模型推理系统，用于诊疗区（C区）观察到人形立牌时，通过大模型图生文反馈识别结果（病人状态描述），再经由 TTS 语音播报。系统由两块 RDK X5 板子协同完成：
+- **服务端** (192.168.175.64): 运行 BPU 推理引擎，负责模型推理
+- **客户端** (192.168.175.65): 运行 ROS2 小车控制 + 图像采集 + TTS 播报
+客户端通过相机采集人形立牌图片，发送给服务端进行 VLM 推理，返回中文病人状态描述，再通过 TTS 语音模块播报。
+
+##### 模型选择
+采用 **InternVL2.5 + Qwen2.5-0.5B** 组合方案：
+选型依据：
+Qwen2.5-0.5B 是轻量级中文大模型，0.5B 参数适合 RDK X5 4GB 内存限制
+Q4_0 量化格式在 ARM aarch64 上有 NEON 指令集优化，推理效率最高
+ViT 模型经 hbDNN 工具链编译为 BPU int16 格式，图像编码仅需约 0.1s
+医疗场景描述任务相对简单，0.5B 配合合适的 prompt 即可满足要求
+
+##### 架构设计
+
+服务端采用 **BPU 持久化方案**，模型加载一次后常驻内存，循环等待推理请求：
+
+```
+服务端 (192.168.175.64)
+systemd: vlm-services.service (开机自启)
+  |-- llama-intern2vl-bpu-server  (BPU 持久化, -t 8)
+  |-- tcp_bridge.py               (TCP :9216, 二进制协议)
+
+客户端 (192.168.175.65)  
+ROS2 launch vlm_detect
+  |-- vlm_node     (订阅图片 + 触发信号, TCP 推理)
+  |-- tts_server   (语音合成播报)
+```
+
+通信采用网线直连 + TCP 自定义二进制协议，替代 HTTP 的 JSON/base64 开销：
+
+```
+每帧: [0xAB 0xCD] [Flag:1B] [Length:4B BE] [Data:N bytes]
+Flag: C=命令(JSON), I=图片(JPEG), R=结果(JSON)
+
+Client --CMD(prompt+size)--> Server
+Client --IMG(JPEG bytes)--> Server
+Client <-RESP(answer)------ Server
+```
+
+##### VLM 推理流程
+
+1. 小车导航至人形立牌前，`racing_control` 节点发布 `Int32(9)` 到 `/sign4return`
+2. `vlm_detect` 节点收到触发信号，取最新一帧相机图片
+3. 图片预处理：缩放到 96px，JPEG 质量 60 压缩（约 2KB）
+4. 通过 TCP 发送图片 + prompt 到服务端
+5. 服务端 BPU 推理引擎执行 ViT 图像编码 + LLM 文本生成
+6. 返回中文描述到客户端，发布到 `/vlm_result`
+7. `tts_server` 订阅结果，语音合成播报
+
+为防止小车在路标点附近反复触发，加入一次性锁定机制：首次收到信号后置 `_vlm_done = True`，后续所有触发信号直接忽略。
+
+##### 性能优化
+
+| 阶段 | 优化措施 | 效果 |
+|------|----------|------|
+| 模型加载 | BPU 常驻内存，不释放 hbDNN handle | 消除每次推理的加载开销 |
+| 上下文 | `llama_kv_cache_clear()` 清空而非重载 | 支持连续多次推理 |
+| 冷启动 | 启动脚本中加入暖启动步骤 | 首次推理从 22s 降至正常 |
+| 线程 | `-t 8` 线程并行推理 | 5.6s（最优值） |
+| 传输 | 网线直连 TCP，延迟 0.3ms | 替代 WiFi (25-77ms) |
+| 图片 | 缩放到 96px，减少视觉 token | 间接提升推理速度 |
+
+最终推理耗时 **5.6s**，满足比赛实时性要求。
+
+##### TTS 语音模块
+
+`tts_server` 节点订阅 `/vlm_result` 话题，收到 VLM 推理结果后，调用语音合成引擎将中文文本转为语音播报。支持调整语速（默认 1.5x），通过外接扬声器输出。
+
+##### 关键参数
+
+| 参数 | 默认值 | 说明 |
+|------|--------|------|
+| image_max_dim | 96 | 图片最长边缩放大小 |
+| prompt_text | "请描述这个病人的状态…" | 推理提示词 |
+| temperature | 0.1 | 低温度确保输出确定性 |
+| max_tokens | 30 | 限制输出长度 |
+| trigger_sign | 9 | 触发信号值 |
+| tcp_host | 192.168.127.1 | 服务端有线 IP
+
+
+#### 2.2.6 导航模块
+
+- costmap层插件：基于/obstacles定制的一款轻量化插件，以减轻算力
+- 取消重定位系统：在base的定位中已经实现实时重定位（滤波矫正），因此计算量庞大的重定位对于本项目来说没有必要
+- 其它
+
+### 2.3 由于规则定制的适配方案
+
+#### 2.3.1 墙线拟合模块
+
+在实际比赛场地中，官方会在地图周围摆设一个矩形挡板，因此我们能够根据雷达观测到前后左右墙的位置来间接计算小车位于场地的位置
+
+#### 2.3.2 小车的比赛行为
+
+由于轨迹较为固定且对时间要求较高，我们并没有采用目标+行为树的逻辑方法，而是针对比赛情况定制了一个基于多点生成轨迹与适配的恢复行为，从而在不影响比赛时长的前提下快速恢复导航
+
+### 2.4 备赛时遇到的问题及解决办法
+
+#### 2.4.1 算力问题
+
+初版方案是slamtoolbox+nav2进行的slam导航，结果发现开启后出现整车运行卡顿、tf常常滞后等情况，于是探索了经典的点云建图、避障的方法，优化效果微乎其微。最终通过建立二维方程的方法将重定位
+
+
+
+
+
+
+