diff --git a/src/racing_control/AGENTS.md b/src/racing_control/AGENTS.md
new file mode 100644
index 0000000..913337f
--- /dev/null
+++ b/src/racing_control/AGENTS.md
@@ -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`；总调度节点不要发布未明确约定的负数复位指令。
diff --git a/src/racing_control/CMakeLists.txt b/src/racing_control/CMakeLists.txt
index 301be46..9d87a47 100644
--- a/src/racing_control/CMakeLists.txt
+++ b/src/racing_control/CMakeLists.txt
@@ -7,11 +7,43 @@ endif()
 
 # find dependencies
 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
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
   # 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
   # a copyright and license is added to all source files
   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()
 endif()
 
diff --git a/src/racing_control/config/racing_control.yaml b/src/racing_control/config/racing_control.yaml
new file mode 100644
index 0000000..56bfcea
--- /dev/null
+++ b/src/racing_control/config/racing_control.yaml
@@ -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]
diff --git a/src/racing_control/include/racing_control/racing_control.hpp b/src/racing_control/include/racing_control/racing_control.hpp
index cb1d3db..9fc9af8 100644
--- a/src/racing_control/include/racing_control/racing_control.hpp
+++ b/src/racing_control/include/racing_control/racing_control.hpp
@@ -1,33 +1,102 @@
-/*
-功能描述：
-    1. 状态指令：
-        1 - 比赛开始，进行任务一寻找二维码
-        2 - 找到二维码，执行任务一到任务二过渡阶段导航
-        3 - 到达任务二起始阶段，执行顺/逆时针绕圈（可集成图生文）
-        4 - 减速拍照，图像传入图生文节点，随后回到3（备用）
-        5 - 完整走完一圈，执行任务三
-        6 - 到达终点，停止
-    2. 全部流程
-        启动小车->slamtoolbox开始建图，并开始发布导航命令（这时候小车还不能动）->打开电机开关，小车开始行动->走到一半扫到二维码，发布在/qr_results上->停掉二维码节点（sign=5）和导航1，同时开始导航2到任务二入口并顺逆时针转圈->到达指定位置触发一次vlm请求->开始语音播报同时完成任务二、三
-*/
+#ifndef RACING_CONTROL__RACING_CONTROL_HPP_
+#define RACING_CONTROL__RACING_CONTROL_HPP_
 
-#include "rclcpp/rclcpp.hpp"
+#include <cmath>
+#include <cctype>
+#include <stdexcept>
+#include <string>
+#include <vector>
 
-static int QR_SEARCHING = 1;
-static int ENTRY = 2;
-static int CIRCLE = 3;
-static int VLM = 4;
-static int TASK3 = 5;
-static int END = 6;
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
-
-class RacingControl : public rclcpp::Node
+namespace racing_control
 {
-public:
-    RacingControl();
-private:
-    // 状态指令变量
-    int state_command_;
+
+enum class RouteDirection
+{
+  Unknown,
+  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_
diff --git a/src/racing_control/launch/racing_control.launch.py b/src/racing_control/launch/racing_control.launch.py
new file mode 100644
index 0000000..91fca79
--- /dev/null
+++ b/src/racing_control/launch/racing_control.launch.py
@@ -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,
+        ]
+    )
diff --git a/src/racing_control/package.xml b/src/racing_control/package.xml
index e5f978f..812e9da 100644
--- a/src/racing_control/package.xml
+++ b/src/racing_control/package.xml
@@ -9,6 +9,18 @@
 
   <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_common</test_depend>
 
diff --git a/src/racing_control/prompt.md b/src/racing_control/prompt.md
new file mode 100644
index 0000000..a0416f6
--- /dev/null
+++ b/src/racing_control/prompt.md
@@ -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的检测等等
\ No newline at end of file
diff --git a/src/racing_control/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp
index 8c04fbe..60d4e36 100644
--- a/src/racing_control/src/racing_control.cpp
+++ b/src/racing_control/src/racing_control.cpp
@@ -1,7 +1,907 @@
 #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_)
     {
-        // 初始化状态指令
-        state_command_ = QR_SEARCHING;
-    }  
\ No newline at end of file
+      finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label);
+      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;
+}
diff --git a/src/racing_control/test/test_racing_control_helpers.cpp b/src/racing_control/test/test_racing_control_helpers.cpp
new file mode 100644
index 0000000..80615ea
--- /dev/null
+++ b/src/racing_control/test/test_racing_control_helpers.cpp
@@ -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);
+}
diff --git a/src/racing_control/点位格式转换.md b/src/racing_control/点位格式转换.md
new file mode 100644
index 0000000..2ee3f0c
--- /dev/null
+++ b/src/racing_control/点位格式转换.md
@@ -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` 和触发半径，点位格式不用变化。