zbw/yiliao2026

Files

Orange bdddcc63cc good

2026-08-12 21:07:21 +08:00

20 KiB

Raw Blame History

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 区域加入：

#include "racing_control/final_home_fallback.hpp"

Step 2: 添加失败测试

把下面测试追加到 /home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.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: 运行测试并确认失败

运行：

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：

#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：

#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 改为：

add_library(racing_control_core
  src/candidate_waypoint_selector.cpp
  src/final_home_fallback.cpp
)

把 add_executable(racing_control ...) 改为包含新文件：

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 ...) 改为：

  target_sources(test_racing_control_helpers PRIVATE
    src/candidate_waypoint_selector.cpp
    src/final_home_fallback.cpp
  )

Step 4: 运行 helper 测试

运行：

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 区域加入：

#include "racing_control/final_home_fallback.hpp"

Step 2: 增加阶段枚举和名称

在 enum class Stage 中加入：

  FinalHomeFallback,

在 stageName(...) 中加入：

    case Stage::FinalHomeFallback:
      return "最终回家保护";

Step 3: 加载参数

在 loadParameters() 里，恢复参数附近加入：

    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 成员变量区加入：

  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 的恢复参数附近加入：

    # 最终回家保护：普通 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: 编译

运行：

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(...) 附近加入：

  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(...) 函数体替换为：

  void failRace(const std::string & reason)
  {
    startFinalHomeFallback(reason);
  }

Step 3: 修改 tick 入口

在 tick() 中，把终止保护后面加入最终保护 tick：

    if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
      return;
    }

    if (stage_ == Stage::FinalHomeFallback) {
      tickFinalHomeFallback();
      return;
    }

Step 4: 编译确认缺口

运行：

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 附近加入：

  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

在恢复相关函数附近加入：

  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(...) 附近加入：

  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: 编译

运行：

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 测试

运行：

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: 查看测试结果

运行：

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: 检查改动范围

运行：

ssh sunrise@192.168.10.210 'cd /home/sunrise/yiliao_ws && git status --short src/racing_control'

预期：只出现 racing_control 的源码、头文件、CMake、yaml、测试改动。

Step 4: 审查最终 diff

运行：

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

20 KiB Raw Blame History Unescape Escape

Racing Control 最终回家保护实现计划

文件结构

Task 1: 写最终保护算法的失败测试

Task 2: 新建最终保护算法文件

Task 3: 增加最终保护参数和状态

Task 4: 把最终失败切入 FinalHomeFallback

Task 5: 接入算法、发布直控速度、定时重试 Nav2

Task 6: 验证和检查

自检

20 KiB

Raw Blame History