zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加了全套任务的包

Browse Source
This commit is contained in:
Orange
2026-08-05 22:12:02 +08:00
parent b53f203f2d
commit 1219a40077
10 changed files with 1405 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
src/racing_control/AGENTS.md Normal file
View File

@@ -0,0 +1,40 @@
# 项目说明
## 项目用途
- 这是 RDKx5 赛车机器人的 ROS2 Humble 工作区。
- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和轨迹保护节点，不在包内重新实现这些功能。
## 环境准备
- 通过 `ssh sunrise@192.168.10.210` 连接机器人。
- 构建或运行前先 source ROS 和工作区：
`source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash`。
- 部分相机/Hobot 流程可能还需要 source `/opt/tros/humble/setup.bash`。
## 构建与测试
- 只构建比赛总控：
`cd /home/sunrise/yiliao_ws && source /opt/ros/humble/setup.bash && source install/setup.bash && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON`。
- 只测试比赛总控：
`cd /home/sunrise/yiliao_ws && source /opt/ros/humble/setup.bash && source install/setup.bash && colcon test --packages-select racing_control && colcon test-result --verbose --test-result-base build/racing_control`。
- `ament_xmllint` 会从 `download.ros.org` 读取 package schema；如果出现临时资源或网络错误，先重跑 xmllint 测试，不要急着修改 XML。
## 架构
- `/sign4return` 是共享的 `std_msgs/msg/Int32` 控制话题：
`0` 开启二维码检测，`5` 关闭二维码检测，`9` 触发 VLM 拍照识别，`10` 切换普通 Nav2 参数，`11` 切换任务二 Nav2 参数。
- `obstacle_nav2/nav2_profile_tuner` 监听 `/sign4return`，并根据 10 或 11 应用对应的 Nav2 profile。
- `obstacle_nav2/trajectory_guard_node` 订阅 `/trajectory_guard/input_path`；当 `execute_follow_path` 为 true 时，它会把保护后的路径发送给 Nav2 `/follow_path`。
- 比赛总控使用 Nav2 actions（`/navigate_to_pose`、`/compute_path_through_poses`、`/follow_path`），比赛点位应保持为可通过 ROS 参数配置。
## 外部系统
- `vlm_detect` 会调用由 launch 参数配置的外部 VLM 服务，该服务可能运行在另一台机器上。
- `vlm_detect` 同时通过 `origincar_msg/srv/Speak` 提供 `/tts/speak` 语音服务。
## 已知注意事项
- 不要让多个节点同时发布 `/cmd_vel`，避免与 Nav2 controller 输出互相抢控制权。
- 实际比赛点位依赖现场标定；不要把代码或 yaml 默认值当作真实赛场坐标。
- 机器人底盘也订阅 `/sign4return`；总调度节点不要发布未明确约定的负数复位指令。

44
src/racing_control/CMakeLists.txt
View File

@@ -7,11 +7,43 @@ endif()
# find dependencies # find dependencies
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(racing_control
src/racing_control.cpp
)
target_include_directories(racing_control PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
ament_target_dependencies(racing_control
geometry_msgs
nav_msgs
nav2_msgs
rclcpp
rclcpp_action
std_msgs
)
install(TARGETS
racing_control
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY config launch
DESTINATION share/${PROJECT_NAME}
)
# uncomment the following section in order to fill in # uncomment the following section in order to fill in
# further dependencies manually. # further dependencies manually.
# find_package(<dependency> REQUIRED) # find_package(<dependency> REQUIRED)
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights # the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files # comment the line when a copyright and license is added to all source files
@@ -20,6 +52,18 @@ if(BUILD_TESTING)
# comment the line when this package is in a git repo and when # comment the line when this package is in a git repo and when
# a copyright and license is added to all source files # a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE)
ament_add_gtest(test_racing_control_helpers
test/test_racing_control_helpers.cpp
)
target_include_directories(test_racing_control_helpers PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
)
ament_target_dependencies(test_racing_control_helpers
geometry_msgs
rclcpp
)
ament_lint_auto_find_test_dependencies() ament_lint_auto_find_test_dependencies()
endif() endif()

103
src/racing_control/config/racing_control.yaml Normal file
View File

@@ -0,0 +1,103 @@
racing_control:
ros__parameters:
# Startup
auto_start: false
frame_id: odom
# Shared coordination topics
sign_topic: /sign4return
qr_result_topic: /qr_results
vlm_result_topic: /vlm_result
odom_topic: /odom_combined
trajectory_guard_input_topic: /trajectory_guard/input_path
# Nav2 actions and plugin IDs
navigate_action: /navigate_to_pose
compute_path_action: /compute_path_through_poses
follow_path_action: /follow_path
planner_id: GridBased
controller_id: FollowPath
goal_checker_id: ""
use_trajectory_guard: true
# Timeouts and settling waits, in seconds
navigation_timeout_sec: 120.0
path_planning_timeout_sec: 30.0
circle_timeout_sec: 120.0
qr_result_timeout_sec: 8.0
profile_switch_wait_sec: 1.0
post_qr_wait_sec: 1.0
vlm_capture_wait_sec: 0.5
circle_goal_tolerance: 0.30
# VLM capture:
# stop - stop at the VLM waypoint, trigger capture, then continue immediately.
# pass_through - keep following the full route and trigger capture while passing nearby.
vlm_capture_mode: stop
pass_through_vlm_trigger_radius: 0.35
# /sign4return command values used by existing packages
sign_qr_enable: 0
sign_qr_disable: 5
sign_vlm_trigger: 9
sign_profile_normal: 10
sign_profile_task2: 11
# Pose parameters are flat x/y/yaw-radians triples in frame_id.
qr_pose: [4.5303713524852558, 1.226131216530598, 1.2983334871583534]
entry_pose: [2.4951887885861144, 2.2268832181534743, 1.5626000867375398]
# The route's Nth waypoint is used as the VLM capture point.
# With the saved JSON files, the 4th point is goal_011 for main_1
# and goal_004 for main_2.
vlm_waypoint_number: 4
# main_1.json: clockwise route, excluding home.
clockwise_waypoints:
- 1.542550031688728
- 3.1698990676859604
- 3.1415926535897918
- 0.71981664792046363
- 3.7665012349236271
- 1.5495230091246759
- 1.3164186536457545
- 4.3294241954584018
- -0.016392199787162172
- 3.659525047016253
- 4.324612741775951
- 4.6883147711881255e-16
- 4.2320702688664218
- 3.7761238192637752
- -1.5784882007253702
- 3.510374505206836
- 3.1650879370282627
- -3.0916338672023889
- 2.5288679952890067
- 2.4578258883665218
- -1.6718998381125814
clockwise_home_pose: [0.51774173072785878, 0.17726629320698351, -2.2032146659725105]
# main_2.json: counterclockwise route, excluding home.
counterclockwise_waypoints:
- 3.3612239633974195
- 3.1747105213684104
- 0.010869470292991894
- 4.2272591382087246
- 3.7280108975630366
- 1.5707963267948966
- 3.4237709231207547
- 4.3486693641386971
- 3.123412893984987
- 1.2731168626027138
- 4.334235487628475
- -3.1165979438417697
- 0.74868440094090649
- 3.7376334819031838
- -1.5593026199905524
- 1.3933994898793114
- 3.1795219750508603
- -0.01136349401017399
- 2.4663210355656715
- 2.4145240973234814
- -1.5556465256578393
counterclockwise_home_pose: [0.51774173072785878, 0.15802112452668779, -2.1396781415005068]

121
src/racing_control/include/racing_control/racing_control.hpp
View File

@@ -1,33 +1,102 @@
/* #ifndef RACING_CONTROL__RACING_CONTROL_HPP_
功能描述： #define RACING_CONTROL__RACING_CONTROL_HPP_
1. 状态指令：
1 - 比赛开始，进行任务一寻找二维码
2 - 找到二维码，执行任务一到任务二过渡阶段导航
3 - 到达任务二起始阶段，执行顺/逆时针绕圈（可集成图生文）
4 - 减速拍照，图像传入图生文节点，随后回到3（备用）
5 - 完整走完一圈，执行任务三
6 - 到达终点，停止
2. 全部流程
启动小车->slamtoolbox开始建图，并开始发布导航命令（这时候小车还不能动）->打开电机开关，小车开始行动->走到一半扫到二维码，发布在/qr_results上->停掉二维码节点（sign=5）和导航1，同时开始导航2到任务二入口并顺逆时针转圈->到达指定位置触发一次vlm请求->开始语音播报同时完成任务二、三
*/
#include "rclcpp/rclcpp.hpp" #include <cmath>
#include <cctype>
#include <stdexcept>
#include <string>
#include <vector>
static int QR_SEARCHING = 1; #include "geometry_msgs/msg/pose_stamped.hpp"
static int ENTRY = 2;
static int CIRCLE = 3;
static int VLM = 4;
static int TASK3 = 5;
static int END = 6;
namespace racing_control
class RacingControl : public rclcpp::Node
{ {
public:
RacingControl(); enum class RouteDirection
private: {
// 状态指令变量 Unknown,
int state_command_; Clockwise,
Counterclockwise
}; };
enum class VlmCaptureMode
{
Unknown,
Stop,
PassThrough
};
inline VlmCaptureMode vlmCaptureModeFromString(const std::string & value)
{
std::string normalized;
normalized.reserve(value.size());
for (const auto ch : value) {
normalized.push_back(static_cast<char>(std::tolower(static_cast<unsigned char>(ch))));
}
if (normalized == "stop") {
return VlmCaptureMode::Stop;
}
if (normalized == "pass_through") {
return VlmCaptureMode::PassThrough;
}
return VlmCaptureMode::Unknown;
}
inline RouteDirection directionFromQrResult(const std::string & text)
{
if (text.find("逆") != std::string::npos) {
return RouteDirection::Counterclockwise;
}
if (text.find("顺") != std::string::npos) {
return RouteDirection::Clockwise;
}
for (auto it = text.rbegin(); it != text.rend(); ++it) {
const auto ch = static_cast<unsigned char>(*it);
if (std::isdigit(ch)) {
return ((*it - '0') % 2 == 1) ? RouteDirection::Clockwise :
RouteDirection::Counterclockwise;
}
}
return RouteDirection::Unknown;
}
inline bool shouldAcceptQrDirection(
const RouteDirection current_direction, const RouteDirection incoming_direction)
{
return current_direction == RouteDirection::Unknown &&
incoming_direction != RouteDirection::Unknown;
}
inline geometry_msgs::msg::PoseStamped poseFromXYYaw(
const double x, const double y, const double yaw, const std::string & frame_id)
{
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = frame_id;
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.position.z = 0.0;
pose.pose.orientation.z = std::sin(yaw * 0.5);
pose.pose.orientation.w = std::cos(yaw * 0.5);
return pose;
}
inline std::vector<geometry_msgs::msg::PoseStamped> posesFromFlatDoubles(
const std::vector<double> & values, const std::string & frame_id)
{
if (values.size() % 3 != 0) {
throw std::invalid_argument("pose arrays must contain x/y/yaw triples");
}
std::vector<geometry_msgs::msg::PoseStamped> poses;
poses.reserve(values.size() / 3);
for (std::size_t i = 0; i < values.size(); i += 3) {
poses.push_back(poseFromXYYaw(values[i], values[i + 1], values[i + 2], frame_id));
}
return poses;
}
} // namespace racing_control
#endif // RACING_CONTROL__RACING_CONTROL_HPP_

47
src/racing_control/launch/racing_control.launch.py Normal file
View File

@@ -0,0 +1,47 @@
#!/usr/bin/env python3
"""Launch the racing_control orchestration node."""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
pkg_dir = get_package_share_directory("racing_control")
default_params = os.path.join(pkg_dir, "config", "racing_control.yaml")
params_file = LaunchConfiguration("params_file")
auto_start = LaunchConfiguration("auto_start")
racing_control = Node(
package="racing_control",
executable="racing_control",
name="racing_control",
output="screen",
parameters=[
params_file,
{
"auto_start": auto_start,
},
],
)
return LaunchDescription(
[
DeclareLaunchArgument(
"params_file",
default_value=default_params,
description="YAML parameter file for the racing_control node",
),
DeclareLaunchArgument(
"auto_start",
default_value="false",
description="Start the race immediately instead of waiting for SPACE",
),
racing_control,
]
)

12
src/racing_control/package.xml
View File

@@ -9,6 +9,18 @@
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>nav2_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>std_msgs</depend>
<exec_depend>ament_index_python</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

19
src/racing_control/prompt.md Normal file
View File

@@ -0,0 +1,19 @@
# 一、功能描述
### 1. 全部流程
启动所有节点 - 发布第一个点寻找二维码 - tts - 发布第二个点进入通道 - 切换到任务二的导航参数 - 通过固定点生成轨迹交给MPPI绕圈（） - 到指定位置后获取图像（后续优化：多拍几张，使用最清晰的一张） - 图像传入图生文并tts - 继续导航 - 回到原点
### 2. 其它相关包
1. 摄像头：`car_usb_cam`中的hobot_usb_cam，调用它来获取/image
2. 二维码检测：`qr_detection`中的`qr_detection.launch.py`，通过/sign4return控制开关
3. 导航：`obstacle_nav2`，包括nav2节点启动、origincar_base启动、雷达启动等。它的主要功能是将obstacle_scan检测到的/obstacles转化为costmap，并启动nav2相关服务
4. 障碍物检测：`obstacle_scanner`
5. 图生文和tts：`vlm_detect`
6. 参数传递节点：`obstacle_nav2\nav2_profile_tuner.launch.py`
> #### 存在于其它设备中
> 1. 关于任务二路径规划的部分后端代码在TOU-DU这个电脑的`D:\Programme\agent\projects\yiliao\scanner_debug_web\run_odom_snapshot.sh`中有用到，效果是将一系列点规划为路径再传输给/path
> 2. 关于vlm-server: 它实际运行于另一台电脑上，先不用管
### 特别说明
在本项目中，一些特殊的功能都由/sign4return的值来决定是否启动，比如vlm的识别、qr的检测等等

908
src/racing_control/src/racing_control.cpp
View File

@@ -1,7 +1,907 @@
#include "racing_control/racing_control.hpp" #include "racing_control/racing_control.hpp"
RacingControl : Node("racing_control") #include <atomic>
#include <chrono>
#include <cmath>
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <sstream>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include <sys/select.h>
#include <termios.h>
#include <unistd.h>
#include "nav2_msgs/action/compute_path_through_poses.hpp"
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
namespace racing_control
{
namespace
{
enum class Stage
{
Idle,
NavigateToQr,
WaitForQr,
NavigateToEntry,
SwitchToTask2Profile,
ComputeCirclePath,
ExecuteCirclePath,
SwitchToNormalProfile,
WaitForVlm,
ReturnOrigin,
Finished,
Failed
};
enum class RouteSegment
{
None,
ToVlm,
AfterVlm,
FullRoute
};
struct RouteConfig
{
std::string label;
std::vector<geometry_msgs::msg::PoseStamped> waypoints;
geometry_msgs::msg::PoseStamped home_pose;
};
const char * stageName(const Stage stage)
{
switch (stage) {
case Stage::Idle:
return "等待启动";
case Stage::NavigateToQr:
return "二维码点导航";
case Stage::WaitForQr:
return "二维码识别/TTS";
case Stage::NavigateToEntry:
return "通道入口导航";
case Stage::SwitchToTask2Profile:
return "任务二参数切换";
case Stage::ComputeCirclePath:
return "任务二轨迹规划";
case Stage::ExecuteCirclePath:
return "任务二轨迹执行";
case Stage::SwitchToNormalProfile:
return "恢复导航参数";
case Stage::WaitForVlm:
return "图生文/TTS";
case Stage::ReturnOrigin:
return "返回原点";
case Stage::Finished:
return "比赛完成";
case Stage::Failed:
return "比赛失败";
}
return "未知阶段";
}
double distance2d(
const geometry_msgs::msg::PoseStamped & a,
const geometry_msgs::msg::PoseStamped & b)
{
const double dx = a.pose.position.x - b.pose.position.x;
const double dy = a.pose.position.y - b.pose.position.y;
return std::hypot(dx, dy);
}
std::string poseSummary(const geometry_msgs::msg::PoseStamped & pose)
{
std::ostringstream out;
out << "(" << pose.pose.position.x << ", " << pose.pose.position.y << ")";
return out.str();
}
} // namespace
class RacingControl : public rclcpp::Node
{
public:
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using ComputePathThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
using FollowPath = nav2_msgs::action::FollowPath;
using NavigateGoalHandle = rclcpp_action::ClientGoalHandle<NavigateToPose>;
using ComputeGoalHandle = rclcpp_action::ClientGoalHandle<ComputePathThroughPoses>;
using FollowGoalHandle = rclcpp_action::ClientGoalHandle<FollowPath>;
RacingControl()
: Node("racing_control")
{
loadParameters();
sign_pub_ = create_publisher<std_msgs::msg::Int32>(sign_topic_, 10);
guard_path_pub_ = create_publisher<nav_msgs::msg::Path>(guard_input_topic_, 1);
qr_sub_ = create_subscription<std_msgs::msg::String>(
qr_result_topic_, 10,
[this](std_msgs::msg::String::SharedPtr msg) {onQrResult(std::move(msg));});
vlm_sub_ = create_subscription<std_msgs::msg::String>(
vlm_result_topic_, 10,
[this](std_msgs::msg::String::SharedPtr msg) {onVlmResult(std::move(msg));});
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
odom_topic_, 10,
[this](nav_msgs::msg::Odometry::SharedPtr msg) {onOdom(std::move(msg));});
navigate_client_ = rclcpp_action::create_client<NavigateToPose>(this, navigate_action_);
compute_path_client_ =
rclcpp_action::create_client<ComputePathThroughPoses>(this, compute_path_action_);
follow_path_client_ = rclcpp_action::create_client<FollowPath>(this, follow_path_action_);
tick_timer_ = create_wall_timer(200ms, [this]() {tick();});
startKeyboardThread();
RCLCPP_INFO(
get_logger(),
"racing_control ready. Press SPACE to start, or set auto_start:=true.");
if (auto_start_) {
startRace();
}
}
~RacingControl() override
{
stop_keyboard_.store(true);
if (keyboard_thread_.joinable()) {
keyboard_thread_.join();
}
}
private:
void loadParameters()
{
frame_id_ = declare_parameter<std::string>("frame_id", "odom");
sign_topic_ = declare_parameter<std::string>("sign_topic", "/sign4return");
qr_result_topic_ = declare_parameter<std::string>("qr_result_topic", "/qr_results");
vlm_result_topic_ = declare_parameter<std::string>("vlm_result_topic", "/vlm_result");
odom_topic_ = declare_parameter<std::string>("odom_topic", "/odom_combined");
navigate_action_ = declare_parameter<std::string>("navigate_action", "/navigate_to_pose");
compute_path_action_ =
declare_parameter<std::string>("compute_path_action", "/compute_path_through_poses");
follow_path_action_ = declare_parameter<std::string>("follow_path_action", "/follow_path");
guard_input_topic_ =
declare_parameter<std::string>(
"trajectory_guard_input_topic",
"/trajectory_guard/input_path");
planner_id_ = declare_parameter<std::string>("planner_id", "GridBased");
controller_id_ = declare_parameter<std::string>("controller_id", "FollowPath");
goal_checker_id_ = declare_parameter<std::string>("goal_checker_id", "");
auto_start_ = declare_parameter<bool>("auto_start", false);
use_trajectory_guard_ = declare_parameter<bool>("use_trajectory_guard", true);
navigation_timeout_sec_ = declare_parameter<double>("navigation_timeout_sec", 120.0);
path_planning_timeout_sec_ = declare_parameter<double>("path_planning_timeout_sec", 30.0);
circle_timeout_sec_ = declare_parameter<double>("circle_timeout_sec", 120.0);
qr_result_timeout_sec_ = declare_parameter<double>("qr_result_timeout_sec", 8.0);
profile_switch_wait_sec_ = declare_parameter<double>("profile_switch_wait_sec", 1.0);
post_qr_wait_sec_ = declare_parameter<double>("post_qr_wait_sec", 1.0);
vlm_capture_wait_sec_ = declare_parameter<double>("vlm_capture_wait_sec", 0.5);
pass_through_vlm_trigger_radius_ =
declare_parameter<double>("pass_through_vlm_trigger_radius", 0.35);
circle_goal_tolerance_ = declare_parameter<double>("circle_goal_tolerance", 0.30);
const auto vlm_capture_mode =
declare_parameter<std::string>("vlm_capture_mode", "stop");
vlm_capture_mode_ = vlmCaptureModeFromString(vlm_capture_mode);
if (vlm_capture_mode_ == VlmCaptureMode::Unknown) {
throw std::invalid_argument("vlm_capture_mode must be 'stop' or 'pass_through'");
}
sign_qr_enable_ = declare_parameter<int>("sign_qr_enable", 0);
sign_qr_disable_ = declare_parameter<int>("sign_qr_disable", 5);
sign_vlm_trigger_ = declare_parameter<int>("sign_vlm_trigger", 9);
sign_profile_normal_ = declare_parameter<int>("sign_profile_normal", 10);
sign_profile_task2_ = declare_parameter<int>("sign_profile_task2", 11);
qr_pose_ = singlePoseFromParameter("qr_pose", {0.80, 0.20, 0.0});
entry_pose_ = singlePoseFromParameter("entry_pose", {1.20, 0.20, 0.0});
vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 4);
const auto clockwise_defaults = std::vector<double>{
1.20, 0.80, 1.5708,
2.20, 0.80, 0.0,
2.20, 1.40, 1.5708,
1.20, 1.40, 3.1416,
1.20, 0.80, -1.5708};
const auto counterclockwise_defaults = std::vector<double>{
1.20, 0.80, -1.5708,
1.20, 1.40, 3.1416,
2.20, 1.40, 1.5708,
2.20, 0.80, 0.0,
1.20, 0.80, 1.5708};
clockwise_route_.label = "顺时针";
clockwise_route_.waypoints = posesFromFlatDoubles(
declare_parameter<std::vector<double>>("clockwise_waypoints", clockwise_defaults), frame_id_);
clockwise_route_.home_pose =
singlePoseFromParameter("clockwise_home_pose", {0.54, 0.20, 0.0});
counterclockwise_route_.label = "逆时针";
counterclockwise_route_.waypoints = posesFromFlatDoubles(
declare_parameter<std::vector<double>>(
"counterclockwise_waypoints",
counterclockwise_defaults),
frame_id_);
counterclockwise_route_.home_pose =
singlePoseFromParameter("counterclockwise_home_pose", {0.54, 0.20, 0.0});
}
geometry_msgs::msg::PoseStamped singlePoseFromParameter(
const std::string & name, const std::vector<double> & defaults)
{
const auto poses = posesFromFlatDoubles(
declare_parameter<std::vector<double>>(name, defaults), frame_id_);
if (poses.size() != 1) {
throw std::invalid_argument(name + " must contain exactly one x/y/yaw triple");
}
return poses.front();
}
void startKeyboardThread()
{
if (!isatty(STDIN_FILENO)) {
RCLCPP_WARN(
get_logger(),
"stdin is not a TTY; use auto_start:=true to start without keyboard");
return;
}
keyboard_thread_ = std::thread(
[this]() {
termios old_termios {};
if (tcgetattr(STDIN_FILENO, &old_termios) != 0) {
return;
}
termios raw = old_termios;
raw.c_lflag &= static_cast<unsigned int>(~(ICANON | ECHO));
tcsetattr(STDIN_FILENO, TCSANOW, &raw);
while (!stop_keyboard_.load()) {
fd_set read_set;
FD_ZERO(&read_set);
FD_SET(STDIN_FILENO, &read_set);
timeval timeout {};
timeout.tv_sec = 0;
timeout.tv_usec = 200000;
const int ready = select(STDIN_FILENO + 1, &read_set, nullptr, nullptr, &timeout);
if (ready > 0 && FD_ISSET(STDIN_FILENO, &read_set)) {
char c = 0;
if (read(STDIN_FILENO, &c, 1) == 1 && c == ' ') {
start_requested_.store(true);
}
}
}
tcsetattr(STDIN_FILENO, TCSANOW, &old_termios);
});
}
void tick()
{
if (start_requested_.exchange(false)) {
startRace();
}
if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
return;
}
const auto elapsed = (now() - stage_start_).seconds();
if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) {
if (stage_ == Stage::WaitForQr) {
finishStage("QR wait timeout");
failRace("QR wait timed out before route direction was selected");
return;
}
if (stage_ == Stage::WaitForVlm) {
RCLCPP_WARN(get_logger(), "VLM capture wait timed out; continuing route");
finishStage("VLM capture wait timeout");
runRemainingRouteSegment();
return;
}
failRace("stage timed out: " + std::string(stageName(stage_)));
return;
}
if (stage_ == Stage::SwitchToTask2Profile && elapsed >= profile_switch_wait_sec_) {
finishStage("profile switch wait complete");
runFirstRouteSegment();
return;
}
if (stage_ == Stage::SwitchToNormalProfile && elapsed >= profile_switch_wait_sec_) {
finishStage("normal profile restored");
runReturnOrigin();
return;
}
if (stage_ == Stage::WaitForQr && selected_direction_ != RouteDirection::Unknown &&
(now() - qr_result_time_).seconds() >= post_qr_wait_sec_)
{ {
// 初始化状态指令 finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label);
state_command_ = QR_SEARCHING; runEntryNavigation();
} return;
}
if (stage_ == Stage::WaitForVlm && elapsed >= vlm_capture_wait_sec_) {
finishStage("VLM capture window elapsed");
runRemainingRouteSegment();
return;
}
if (stage_ == Stage::ExecuteCirclePath) {
maybeTriggerPassThroughVlmCapture();
}
if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) {
finishStage("route segment final pose reached");
if (active_segment_ == RouteSegment::ToVlm) {
runVlmWait();
} else if (active_segment_ == RouteSegment::AfterVlm ||
active_segment_ == RouteSegment::FullRoute)
{
runSwitchToNormalProfile();
}
}
}
void startRace()
{
if (race_started_) {
RCLCPP_WARN(get_logger(), "race already started");
return;
}
race_started_ = true;
race_start_ = now();
RCLCPP_INFO(get_logger(), "race started");
runQrNavigation();
}
void startStage(const Stage stage, const double timeout_sec)
{
stage_ = stage;
stage_start_ = now();
stage_timeout_sec_ = timeout_sec;
RCLCPP_INFO(get_logger(), "%s task started", stageName(stage_));
}
void finishStage(const std::string & detail)
{
const auto stage_elapsed = (now() - stage_start_).seconds();
const auto total_elapsed = (now() - race_start_).seconds();
RCLCPP_INFO(
get_logger(), "%s task finished: %s | task %.2fs | total %.2fs",
stageName(stage_), detail.c_str(), stage_elapsed, total_elapsed);
}
void failRace(const std::string & reason)
{
stage_ = Stage::Failed;
publishSign(sign_qr_disable_);
RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
}
void finishRace()
{
finishStage("origin reached");
stage_ = Stage::Finished;
publishSign(sign_qr_disable_);
RCLCPP_INFO(get_logger(), "race finished | total %.2fs", (now() - race_start_).seconds());
}
void publishSign(const int value)
{
std_msgs::msg::Int32 msg;
msg.data = value;
for (int i = 0; i < 3; ++i) {
sign_pub_->publish(msg);
}
RCLCPP_INFO(get_logger(), "published %s=%d", sign_topic_.c_str(), value);
}
void runQrNavigation()
{
latest_qr_result_.clear();
selected_direction_ = RouteDirection::Unknown;
publishSign(sign_profile_normal_);
publishSign(sign_qr_enable_);
startStage(Stage::NavigateToQr, navigation_timeout_sec_);
sendNavigateGoal(
qr_pose_, [this](const bool ok) {
if (stage_ != Stage::NavigateToQr) {
RCLCPP_DEBUG(get_logger(), "stale QR navigation result ignored");
return;
}
finishStage(ok ? "reached " + poseSummary(qr_pose_) : "navigation failed");
if (!ok) {
failRace("failed to reach QR pose");
return;
}
runQrWait();
});
}
void runQrWait()
{
startStage(Stage::WaitForQr, qr_result_timeout_sec_);
if (!latest_qr_result_.empty()) {
qr_result_time_ = now();
}
if (post_qr_wait_sec_ > 0.0) {
stage_timeout_sec_ += post_qr_wait_sec_;
}
}
void runEntryNavigation()
{
publishSign(sign_qr_disable_);
startStage(Stage::NavigateToEntry, navigation_timeout_sec_);
sendNavigateGoal(
entry_pose_, [this](const bool ok) {
if (stage_ != Stage::NavigateToEntry) {
RCLCPP_DEBUG(get_logger(), "stale entry navigation result ignored");
return;
}
finishStage(ok ? "reached " + poseSummary(entry_pose_) : "navigation failed");
if (!ok) {
failRace("failed to reach entry pose");
return;
}
runSwitchToTask2Profile();
});
}
void runSwitchToTask2Profile()
{
if (selected_direction_ == RouteDirection::Unknown) {
failRace("cannot switch to task two before QR route direction is known");
return;
}
publishSign(sign_profile_task2_);
startStage(Stage::SwitchToTask2Profile, profile_switch_wait_sec_ + 2.0);
}
void runFirstRouteSegment()
{
const auto & route = selectedRoute();
const auto vlm_index = vlmWaypointIndex(route);
latest_vlm_result_.clear();
vlm_capture_triggered_ = false;
if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) {
active_segment_ = RouteSegment::FullRoute;
active_segment_waypoints_ = route.waypoints;
runRouteSegmentPlanning("full route with pass-through VLM capture");
return;
}
active_segment_ = RouteSegment::ToVlm;
active_segment_waypoints_.assign(
route.waypoints.begin(),
route.waypoints.begin() + vlm_index + 1);
runRouteSegmentPlanning("to VLM waypoint");
}
void runRemainingRouteSegment()
{
const auto & route = selectedRoute();
const auto vlm_index = vlmWaypointIndex(route);
if (vlm_index + 1 >= route.waypoints.size()) {
runSwitchToNormalProfile();
return;
}
active_segment_ = RouteSegment::AfterVlm;
active_segment_waypoints_.assign(
route.waypoints.begin() + vlm_index + 1,
route.waypoints.end());
runRouteSegmentPlanning("after VLM waypoint");
}
void runRouteSegmentPlanning(const std::string & label)
{
if (active_segment_waypoints_.empty()) {
failRace("route segment must contain at least one pose");
return;
}
startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_);
if (!compute_path_client_->wait_for_action_server(2s)) {
failRace("ComputePathThroughPoses action server is not available");
return;
}
ComputePathThroughPoses::Goal goal;
goal.goals = stampPoses(active_segment_waypoints_);
goal.planner_id = planner_id_;
goal.use_start = false;
auto options = rclcpp_action::Client<ComputePathThroughPoses>::SendGoalOptions();
options.goal_response_callback =
[this](ComputeGoalHandle::SharedPtr goal_handle) {
if (!goal_handle) {
failRace("circle path planning goal was rejected");
}
};
options.result_callback =
[this](const ComputeGoalHandle::WrappedResult & result) {
if (stage_ != Stage::ComputeCirclePath) {
return;
}
if (result.code != rclcpp_action::ResultCode::SUCCEEDED ||
result.result->path.poses.empty())
{
finishStage("planning failed");
failRace("circle path planning failed");
return;
}
active_path_ = result.result->path;
finishStage(
"planned " + std::to_string(active_path_.poses.size()) + " path poses");
runRouteSegmentExecution();
};
compute_path_client_->async_send_goal(goal, options);
RCLCPP_INFO(get_logger(), "planning selected route segment: %s", label.c_str());
}
void runRouteSegmentExecution()
{
startStage(Stage::ExecuteCirclePath, circle_timeout_sec_);
if (use_trajectory_guard_) {
auto path = stampPath(active_path_);
guard_path_pub_->publish(path);
RCLCPP_INFO(
get_logger(), "published route path poses=%zu to %s",
path.poses.size(), guard_input_topic_.c_str());
return;
}
sendFollowPath(
active_path_, [this](const bool ok) {
finishStage(ok ? "FollowPath succeeded" : "FollowPath failed");
if (!ok) {
failRace("route segment FollowPath failed");
return;
}
if (active_segment_ == RouteSegment::ToVlm) {
runVlmWait();
} else if (active_segment_ == RouteSegment::AfterVlm ||
active_segment_ == RouteSegment::FullRoute)
{
runSwitchToNormalProfile();
}
});
}
void runSwitchToNormalProfile()
{
publishSign(sign_profile_normal_);
startStage(Stage::SwitchToNormalProfile, profile_switch_wait_sec_ + 2.0);
}
void runVlmWait()
{
triggerVlmCaptureOnce("stopped at VLM waypoint");
startStage(Stage::WaitForVlm, vlm_capture_wait_sec_ + 2.0);
}
void triggerVlmCaptureOnce(const std::string & reason)
{
if (vlm_capture_triggered_) {
return;
}
vlm_capture_triggered_ = true;
publishSign(sign_vlm_trigger_);
RCLCPP_INFO(get_logger(), "VLM capture triggered: %s", reason.c_str());
}
void maybeTriggerPassThroughVlmCapture()
{
if (vlm_capture_mode_ != VlmCaptureMode::PassThrough ||
active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_)
{
return;
}
const auto & route = selectedRoute();
const auto vlm_index = vlmWaypointIndex(route);
geometry_msgs::msg::PoseStamped current;
{
std::lock_guard<std::mutex> lock(odom_mutex_);
if (!latest_odom_) {
return;
}
current.header = latest_odom_->header;
current.pose = latest_odom_->pose.pose;
}
if (distance2d(current, route.waypoints[vlm_index]) <= pass_through_vlm_trigger_radius_) {
triggerVlmCaptureOnce("passing VLM waypoint");
}
}
void runReturnOrigin()
{
startStage(Stage::ReturnOrigin, navigation_timeout_sec_);
sendNavigateGoal(
selectedRoute().home_pose, [this](const bool ok) {
if (!ok) {
finishStage("navigation failed");
failRace("failed to return origin");
return;
}
finishRace();
});
}
void sendNavigateGoal(
const geometry_msgs::msg::PoseStamped & pose,
std::function<void(bool)> on_done)
{
if (!navigate_client_->wait_for_action_server(2s)) {
failRace("NavigateToPose action server is not available");
return;
}
NavigateToPose::Goal goal;
goal.pose = stampPose(pose);
auto options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
options.goal_response_callback =
[this](NavigateGoalHandle::SharedPtr goal_handle) {
if (!goal_handle) {
failRace("NavigateToPose goal was rejected");
}
};
options.result_callback =
[callback = std::move(on_done)](const NavigateGoalHandle::WrappedResult & result) {
callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
};
navigate_client_->async_send_goal(goal, options);
}
void sendFollowPath(const nav_msgs::msg::Path & path, std::function<void(bool)> on_done)
{
if (!follow_path_client_->wait_for_action_server(2s)) {
failRace("FollowPath action server is not available");
return;
}
FollowPath::Goal goal;
goal.path = stampPath(path);
goal.controller_id = controller_id_;
goal.goal_checker_id = goal_checker_id_;
auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
options.goal_response_callback =
[this](FollowGoalHandle::SharedPtr goal_handle) {
if (!goal_handle) {
failRace("FollowPath goal was rejected");
}
};
options.result_callback =
[callback = std::move(on_done)](const FollowGoalHandle::WrappedResult & result) {
callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
};
follow_path_client_->async_send_goal(goal, options);
}
geometry_msgs::msg::PoseStamped stampPose(geometry_msgs::msg::PoseStamped pose)
{
pose.header.stamp = now();
if (pose.header.frame_id.empty()) {
pose.header.frame_id = frame_id_;
}
return pose;
}
std::vector<geometry_msgs::msg::PoseStamped> stampPoses(
std::vector<geometry_msgs::msg::PoseStamped> poses)
{
for (auto & pose : poses) {
pose = stampPose(pose);
}
return poses;
}
nav_msgs::msg::Path stampPath(nav_msgs::msg::Path path)
{
path.header.frame_id = path.header.frame_id.empty() ? frame_id_ : path.header.frame_id;
path.header.stamp = now();
for (auto & pose : path.poses) {
pose.header.stamp = path.header.stamp;
if (pose.header.frame_id.empty()) {
pose.header.frame_id = path.header.frame_id;
}
}
return path;
}
std::size_t vlmWaypointIndex(const RouteConfig & route) const
{
if (vlm_waypoint_number_ <= 0) {
throw std::runtime_error("vlm_waypoint_number must be >= 1");
}
const auto index = static_cast<std::size_t>(vlm_waypoint_number_ - 1);
if (index >= route.waypoints.size()) {
throw std::runtime_error("vlm_waypoint_number exceeds selected route waypoint count");
}
return index;
}
const RouteConfig & selectedRoute() const
{
if (selected_direction_ == RouteDirection::Clockwise) {
return clockwise_route_;
}
if (selected_direction_ == RouteDirection::Counterclockwise) {
return counterclockwise_route_;
}
throw std::runtime_error("route direction is not selected");
}
bool routeSegmentReached() const
{
std::lock_guard<std::mutex> lock(odom_mutex_);
if (!latest_odom_ || active_segment_waypoints_.empty()) {
return false;
}
geometry_msgs::msg::PoseStamped current;
current.header = latest_odom_->header;
current.pose = latest_odom_->pose.pose;
return distance2d(current, active_segment_waypoints_.back()) <= circle_goal_tolerance_;
}
void onOdom(nav_msgs::msg::Odometry::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(odom_mutex_);
latest_odom_ = std::move(msg);
}
void onQrResult(std_msgs::msg::String::SharedPtr msg)
{
if (msg->data.empty()) {
return;
}
const auto direction = directionFromQrResult(msg->data);
if (direction == RouteDirection::Unknown) {
RCLCPP_WARN(
get_logger(), "QR result received but route direction is unknown: %s",
msg->data.c_str());
return;
}
if (!shouldAcceptQrDirection(selected_direction_, direction)) {
RCLCPP_DEBUG(
get_logger(), "QR result ignored after route direction was selected: %s",
msg->data.c_str());
return;
}
latest_qr_result_ = msg->data;
qr_result_time_ = now();
selected_direction_ = direction;
RCLCPP_INFO(
get_logger(), "QR result received: %s -> %s",
latest_qr_result_.c_str(), selectedRoute().label.c_str());
if (stage_ == Stage::NavigateToQr || stage_ == Stage::WaitForQr) {
finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label);
runEntryNavigation();
}
}
void onVlmResult(std_msgs::msg::String::SharedPtr msg)
{
if (msg->data.empty()) {
return;
}
latest_vlm_result_ = msg->data;
vlm_result_time_ = now();
RCLCPP_INFO(get_logger(), "VLM result received: %s", latest_vlm_result_.c_str());
}
Stage stage_{Stage::Idle};
bool race_started_{false};
bool auto_start_{false};
bool use_trajectory_guard_{true};
rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME};
double stage_timeout_sec_{0.0};
std::string frame_id_;
std::string sign_topic_;
std::string qr_result_topic_;
std::string vlm_result_topic_;
std::string odom_topic_;
std::string navigate_action_;
std::string compute_path_action_;
std::string follow_path_action_;
std::string guard_input_topic_;
std::string planner_id_;
std::string controller_id_;
std::string goal_checker_id_;
double navigation_timeout_sec_{120.0};
double path_planning_timeout_sec_{30.0};
double circle_timeout_sec_{120.0};
double qr_result_timeout_sec_{8.0};
double profile_switch_wait_sec_{1.0};
double post_qr_wait_sec_{1.0};
double vlm_capture_wait_sec_{0.5};
double pass_through_vlm_trigger_radius_{0.35};
double circle_goal_tolerance_{0.30};
int sign_qr_enable_{0};
int sign_qr_disable_{5};
int sign_vlm_trigger_{9};
int sign_profile_normal_{10};
int sign_profile_task2_{11};
int vlm_waypoint_number_{4};
geometry_msgs::msg::PoseStamped qr_pose_;
geometry_msgs::msg::PoseStamped entry_pose_;
RouteConfig clockwise_route_;
RouteConfig counterclockwise_route_;
RouteDirection selected_direction_{RouteDirection::Unknown};
VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop};
RouteSegment active_segment_{RouteSegment::None};
std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
nav_msgs::msg::Path active_path_;
bool vlm_capture_triggered_{false};
std::string latest_qr_result_;
std::string latest_vlm_result_;
rclcpp::Time qr_result_time_{0, 0, RCL_ROS_TIME};
rclcpp::Time vlm_result_time_{0, 0, RCL_ROS_TIME};
nav_msgs::msg::Odometry::SharedPtr latest_odom_;
mutable std::mutex odom_mutex_;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_pub_;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr guard_path_pub_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr qr_sub_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr vlm_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;
rclcpp_action::Client<ComputePathThroughPoses>::SharedPtr compute_path_client_;
rclcpp_action::Client<FollowPath>::SharedPtr follow_path_client_;
rclcpp::TimerBase::SharedPtr tick_timer_;
std::atomic<bool> start_requested_{false};
std::atomic<bool> stop_keyboard_{false};
std::thread keyboard_thread_;
};
} // namespace racing_control
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
try {
rclcpp::spin(std::make_shared<racing_control::RacingControl>());
} catch (const std::exception & e) {
RCLCPP_FATAL(rclcpp::get_logger("racing_control"), "%s", e.what());
rclcpp::shutdown();
return 1;
}
rclcpp::shutdown();
return 0;
}

87
src/racing_control/test/test_racing_control_helpers.cpp Normal file
View File

@@ -0,0 +1,87 @@
#include <cmath>
#include <stdexcept>
#include "gtest/gtest.h"
#include "racing_control/racing_control.hpp"
namespace
{
constexpr double kTolerance = 1e-6;
} // namespace
TEST(RacingControlHelpers, ConvertsFlatTriplesToStampedPoses)
{
const auto poses = racing_control::posesFromFlatDoubles(
{1.0, 2.0, M_PI_2, -0.5, 0.25, -M_PI},
"odom");
ASSERT_EQ(poses.size(), 2U);
EXPECT_EQ(poses[0].header.frame_id, "odom");
EXPECT_DOUBLE_EQ(poses[0].pose.position.x, 1.0);
EXPECT_DOUBLE_EQ(poses[0].pose.position.y, 2.0);
EXPECT_NEAR(poses[0].pose.orientation.z, std::sin(M_PI_4), kTolerance);
EXPECT_NEAR(poses[0].pose.orientation.w, std::cos(M_PI_4), kTolerance);
EXPECT_DOUBLE_EQ(poses[1].pose.position.x, -0.5);
EXPECT_DOUBLE_EQ(poses[1].pose.position.y, 0.25);
}
TEST(RacingControlHelpers, RejectsIncompletePoseTriples)
{
EXPECT_THROW(
racing_control::posesFromFlatDoubles({1.0, 2.0, 0.0, 3.0}, "odom"),
std::invalid_argument);
}
TEST(RacingControlHelpers, ParsesQrDirectionFromText)
{
EXPECT_EQ(
racing_control::directionFromQrResult("7 顺时针"),
racing_control::RouteDirection::Clockwise);
EXPECT_EQ(
racing_control::directionFromQrResult("8 逆时针"),
racing_control::RouteDirection::Counterclockwise);
EXPECT_EQ(
racing_control::directionFromQrResult("5"),
racing_control::RouteDirection::Clockwise);
EXPECT_EQ(
racing_control::directionFromQrResult("6"),
racing_control::RouteDirection::Counterclockwise);
EXPECT_EQ(
racing_control::directionFromQrResult("未识别"),
racing_control::RouteDirection::Unknown);
}
TEST(RacingControlHelpers, LatchesFirstKnownQrDirection)
{
EXPECT_TRUE(
racing_control::shouldAcceptQrDirection(
racing_control::RouteDirection::Unknown,
racing_control::RouteDirection::Clockwise));
EXPECT_FALSE(
racing_control::shouldAcceptQrDirection(
racing_control::RouteDirection::Unknown,
racing_control::RouteDirection::Unknown));
EXPECT_FALSE(
racing_control::shouldAcceptQrDirection(
racing_control::RouteDirection::Clockwise,
racing_control::RouteDirection::Clockwise));
EXPECT_FALSE(
racing_control::shouldAcceptQrDirection(
racing_control::RouteDirection::Clockwise,
racing_control::RouteDirection::Counterclockwise));
}
TEST(RacingControlHelpers, ParsesVlmCaptureMode)
{
EXPECT_EQ(
racing_control::vlmCaptureModeFromString("stop"),
racing_control::VlmCaptureMode::Stop);
EXPECT_EQ(
racing_control::vlmCaptureModeFromString("pass_through"),
racing_control::VlmCaptureMode::PassThrough);
EXPECT_EQ(
racing_control::vlmCaptureModeFromString("unknown"),
racing_control::VlmCaptureMode::Unknown);
}

54
src/racing_control/点位格式转换.md Normal file
View File

@@ -0,0 +1,54 @@
# 点位格式转换
## 用途
把采点工具导出的 odom JSON 转成 `config/racing_control.yaml` 中比赛总控可直接读取的 ROS 参数。
## 输入文件约定
- `main_.json`：只保存 `qr` 和 `entry` 两个点。
- `main_1.json`：顺时针路线点，最后一个点名为 `home`。
- `main_2.json`：逆时针路线点，最后一个点名为 `home`。
- `main_1.json` 和 `main_2.json` 的第 4 个路线点是 VLM 拍摄点。正式参数用 `vlm_waypoint_number: 4` 表示，不依赖原始点名。
- 默认 `vlm_capture_mode: stop`：总控先规划到第 4 点，停车触发 `/sign4return=9`，等待 `vlm_capture_wait_sec` 后立即继续规划剩余路线。
- 备选 `vlm_capture_mode: pass_through`：总控规划整条路线，车辆经过第 4 点附近 `pass_through_vlm_trigger_radius` 范围时触发 `/sign4return=9`，不停车。
## 输出 YAML 字段
`racing_control.yaml` 中所有点位均使用扁平数组：
```yaml
[x, y, yaw_radians]
```
- `x`：`point.odom.pose.pose.position.x`
- `y`：`point.odom.pose.pose.position.y`
- `yaw_radians`：`point.yaw_degrees * pi / 180`
字段映射：
- `main_.json` 中 `qr` -> `qr_pose`
- `main_.json` 中 `entry` -> `entry_pose`
- `main_1.json` 中除 `home` 外的所有点 -> `clockwise_waypoints`
- `main_1.json` 中 `home` -> `clockwise_home_pose`
- `main_2.json` 中除 `home` 外的所有点 -> `counterclockwise_waypoints`
- `main_2.json` 中 `home` -> `counterclockwise_home_pose`
## 转换流程
1. 读取 JSON 的 `points` 数组，保持原始顺序。
2. 对每个点提取 `x`、`y` 和 `yaw_degrees`。
3. 把 `yaw_degrees` 转成弧度。
4. 写入 `config/racing_control.yaml` 对应字段。
5. 路线点不要包含 `home`；`home` 单独写入对应的 `*_home_pose`。
6. 转换后运行：
`colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON`
7. 再运行：
`colcon test --packages-select racing_control && colcon test-result --verbose --test-result-base build/racing_control`
## 注意事项
- 不要把 `saved_at`、`captured_at`、`twist`、`covariance` 等采集元数据写入正式 YAML。
- 不要依赖 `goal_004`、`goal_011` 这类采点名称判断 VLM 点；使用第几个路线点更稳定。
- 如果现场重新采点，只替换 YAML 中的点位参数，不需要改 C++ 流程代码。
- 如果要从停车拍照改成经过拍照，只改 `vlm_capture_mode` 和触发半径，点位格式不用变化。