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