good

2026-08-12 21:07:21 +08:00
parent c30a810b12
commit bdddcc63cc
26 changed files with 1774 additions and 292 deletions
--- 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`、参数名和阶段名在计划内一致。