modified: CLAUDE.md

modified:   src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp
	modified:   src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py
	modified:   src/navigation/obstacle_nav2/src/trajectory_guard.cpp
	modified:   src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp
	modified:   src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp
	modified:   src/origincar_base/CMakeLists.txt
	modified:   src/origincar_base/config/ekf.yaml
	new file:   src/origincar_base/include/origincar_base/log.hpp
	modified:   src/origincar_base/include/origincar_base/origincar_base.h
	modified:   src/origincar_base/launch/base_serial.launch.py
	modified:   src/origincar_base/launch/base_serial.launch.py.bak
	new file:   src/origincar_base/src/log.cpp
	modified:   src/origincar_base/src/origincar_base.cpp
	modified:   src/origincar_base/src/origincar_base.cpp.bak
	new file:   src/origincar_base/test/scan_odom_timing_logger_test.cpp
	modified:   src/planner/REAL_ROBOT_RUNBOOK.md
	modified:   src/planner/launch/odom_hybrid_astar.launch.py
	modified:   src/planner/launch/real_hybrid_astar.launch.py
	modified:   "src/planner/\346\223\215\344\275\234\346\211\213\345\206\214.md"
	modified:   src/racing_control/CMakeLists.txt
	modified:   src/racing_control/config/racing_control.yaml
	modified:   src/racing_control/include/racing_control/racing_control.hpp
	modified:   src/racing_control/launch/racing_control.launch.py
	modified:   src/racing_control/package.xml
	new file:   src/racing_control/src/racing_control copy.cpp
	modified:   src/racing_control/src/racing_control.cpp
	modified:   src/racing_control/test/test_racing_control_helpers.cpp
	modified:   "src/racing_control/\347\202\271\344\275\215\346\240\274\345\274\217\350\275\254\346\215\242.md"

CLAUDE.md

@@ -151,7 +151,7 @@ World 文件在 `src/origincar_description/world/`:
 
 ## 实车底盘驱动包 (origincar_base)
 
-- `origincar_base_node`（C++）: 串口读写（/dev/ttyACM0, 115200bps）、航迹推算、四元数姿态解算（Mahony AHRS）
+- `origincar_base_node`（C++）: 串口读写（/dev/ttyACM0, 921600bps）、航迹推算、四元数姿态解算（Mahony AHRS）
 - `cmd_vel_to_ackermann_drive.py`（Python）: Twist → AckermannDriveStamped 转换，wheelbase=0.143
 - 帧协议: 24 字节收（帧头0x7B/帧尾0x7D）/ 11 字节发
 - EKF 融合: `/odom` + IMU → `/odom_combined`，`two_d_mode=true`

src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp

@@ -59,6 +59,13 @@ std::optional<std::size_t> findClearRejoinIndex(
   const nav_msgs::msg::OccupancyGrid & costmap,
   const GuardSettings & settings);
 
+std::optional<std::size_t> findClearRejoinIndexWithPlannerCostmap(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & local_costmap,
+  const nav_msgs::msg::OccupancyGrid * planner_costmap,
+  const GuardSettings & settings);
+
 bool shouldRetryBlockedRepair(
   const std::optional<std::size_t> & last_repair_nearest_index,
   std::size_t nearest_index,

src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py

@@ -49,7 +49,7 @@ def generate_launch_description():
     start_lidar = LaunchConfiguration('start_lidar', default='true')
     start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
 
-    nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_params.yaml')
+    nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
     fastdds_profile_path = os.path.join(pkg_dir, 'config', 'fastdds_udp_only.xml')
     nav_to_pose_bt_path = os.path.join(
         pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')

src/navigation/obstacle_nav2/src/trajectory_guard.cpp

@@ -213,6 +213,19 @@ std::optional<std::size_t> findClearRejoinIndex(
   return std::nullopt;
 }
 
+std::optional<std::size_t> findClearRejoinIndexWithPlannerCostmap(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & local_costmap,
+  const nav_msgs::msg::OccupancyGrid * planner_costmap,
+  const GuardSettings & settings)
+{
+  if (planner_costmap != nullptr) {
+    return findClearRejoinIndex(path, start_index, *planner_costmap, settings);
+  }
+  return findClearRejoinIndex(path, start_index, local_costmap, settings);
+}
+
 bool shouldRetryBlockedRepair(
   const std::optional<std::size_t> & last_repair_nearest_index,
   std::size_t nearest_index,

src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp

@@ -34,6 +34,7 @@ public:
   {
     declare_parameter("input_path_topic", "/trajectory_guard/input_path");
     declare_parameter("patched_path_topic", "/trajectory_guard/patched_path");
+    declare_parameter("reject_path_topic", "/reject_path");
     declare_parameter("costmap_topic", "/local_costmap/costmap");
     declare_parameter("planner_costmap_topic", "/global_costmap/costmap");
     declare_parameter("odom_topic", "/odom_combined");
@@ -85,6 +86,8 @@ public:
 
     patched_path_pub_ = create_publisher<nav_msgs::msg::Path>(
       get_parameter("patched_path_topic").as_string(), 1);
+    reject_path_pub_ = create_publisher<nav_msgs::msg::Path>(
+      get_parameter("reject_path_topic").as_string(), 1);
     cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
       get_parameter("cmd_vel_topic").as_string(), 1);
 
@@ -236,8 +239,8 @@ private:
       search_start = std::max(search_start, advanced);
     }
 
-    const auto rejoin = findClearRejoinIndex(
+    const auto rejoin = findClearRejoinIndexWithPlannerCostmap(
-      active_path_, search_start, *latest_costmap_, settings_);
+      active_path_, search_start, *latest_costmap_, latest_planner_costmap_.get(), settings_);
     if (!rejoin.has_value()) {
       RCLCPP_WARN(get_logger(), "blocked replay path but no clear rejoin point found");
       publishStop();
@@ -362,6 +365,7 @@ private:
 
         const auto patched = stitchPaths(result.result->path, active_path_, rejoin_index);
         if (!patchedPathIsClear(patched)) {
+          reject_path_pub_->publish(patched);
           retry_without_planner_costmap_update_ = true;
           waiting_for_planner_costmap_update_ = false;
           RCLCPP_WARN(
@@ -437,6 +441,7 @@ private:
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr planner_costmap_sub_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr patched_path_pub_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr reject_path_pub_;
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
   rclcpp_action::Client<ComputePathToPose>::SharedPtr planner_client_;
   rclcpp_action::Client<FollowPath>::SharedPtr follow_client_;

src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp

@@ -141,6 +141,34 @@ TEST(TrajectoryGuard, FindClearRejoinIndexSkipsBlockedArea)
   EXPECT_GE(*rejoin, 5u);
 }
 
+TEST(TrajectoryGuard, FindClearRejoinIndexCanUsePlannerCostmapBeyondLocalWindow)
+{
+  auto path = straightPath();
+  auto local_grid = gridWithOrigin(-0.5, -1.0);
+  local_grid.info.width = 15;
+  local_grid.data.assign(local_grid.info.width * local_grid.info.height, 0);
+
+  auto planner_grid = gridWithOrigin(-1.0, -1.0);
+  planner_grid.info.width = 80;
+  planner_grid.data.assign(planner_grid.info.width * planner_grid.info.height, 0);
+
+  obstacle_nav2::GuardSettings settings;
+  settings.rejoin_min_distance = 1.5;
+  settings.rejoin_max_distance = 3.0;
+  settings.occupied_threshold = 50;
+  settings.treat_unknown_as_occupied = true;
+  settings.footprint_half_length = 0.01;
+  settings.footprint_half_width = 0.01;
+  settings.footprint_padding = 0.0;
+
+  EXPECT_FALSE(obstacle_nav2::findClearRejoinIndex(path, 0, local_grid, settings).has_value());
+
+  const auto rejoin = obstacle_nav2::findClearRejoinIndexWithPlannerCostmap(
+    path, 0, local_grid, &planner_grid, settings);
+  ASSERT_TRUE(rejoin.has_value());
+  EXPECT_GE(*rejoin, 8u);
+}
+
 TEST(TrajectoryGuard, RetryBlockedRepairWaitsForCostmapUpdateAndRetryDelay)
 {
   const std::optional<std::size_t> last_repair_index = 10u;

src/origincar_base/CMakeLists.txt

@@ -64,6 +64,12 @@ set(origincar_base_node_SRCS
   src/origincar_base.cpp
 )
 
+add_library(origincar_base_log STATIC src/log.cpp)
+target_include_directories(origincar_base_log PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 add_executable(origincar_base_node src/origincar_base.cpp)
 ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description)
 
@@ -80,7 +86,7 @@ target_include_directories(wall_kalman_filter PUBLIC
 )
 target_link_libraries(wall_kalman_filter wall_fit_core)
 
-target_link_libraries(origincar_base_node wall_kalman_filter wall_fit_core)
+target_link_libraries(origincar_base_node origincar_base_log wall_kalman_filter wall_fit_core)
 
 add_executable(wall_fit_calibrator src/wall_fit_calibrator.cpp)
 target_link_libraries(wall_fit_calibrator wall_fit_core)
@@ -102,6 +108,10 @@ if(BUILD_TESTING)
   add_executable(wall_kalman_filter_test test/wall_kalman_filter_test.cpp)
   target_link_libraries(wall_kalman_filter_test wall_kalman_filter)
   add_test(NAME wall_kalman_filter_test COMMAND wall_kalman_filter_test)
+
+  add_executable(scan_odom_timing_logger_test test/scan_odom_timing_logger_test.cpp)
+  target_link_libraries(scan_odom_timing_logger_test origincar_base_log)
+  add_test(NAME scan_odom_timing_logger_test COMMAND scan_odom_timing_logger_test)
 endif()
 
 #add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)

src/origincar_base/config/ekf.yaml

@@ -1,7 +1,7 @@
 ### ekf config file ###
 ekf_filter_node:
     ros__parameters:
-        frequency: 20.0
+        frequency: 50.0
         sensor_timeout: 2.0
         two_d_mode: true
         transform_time_offset: 0.0

src/origincar_base/include/origincar_base/log.hpp

@@ -0,0 +1,56 @@
+#ifndef ORIGINCAR_BASE_LOG_HPP_
+#define ORIGINCAR_BASE_LOG_HPP_
+
+#include <chrono>
+#include <cstdint>
+#include <fstream>
+#include <limits>
+#include <string>
+#include <unordered_map>
+
+namespace origincar_base_logging
+{
+class ScanOdomTimingLogger
+{
+public:
+  using Clock = std::chrono::steady_clock;
+  using TimePoint = Clock::time_point;
+
+  explicit ScanOdomTimingLogger(
+      const std::string &node_name,
+      const std::string &base_log_dir = "/home/sunrise/yiliao_ws/running_logs",
+      TimePoint start_time = Clock::now());
+
+  uint64_t startFrame(TimePoint now = Clock::now());
+  bool finishFrame(uint64_t frame_id, TimePoint now = Clock::now());
+  bool cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now = Clock::now());
+  bool makeSummaryLineIfDue(TimePoint now, std::string *line);
+
+  const std::string &logPath() const;
+  bool isOpen() const;
+
+private:
+  struct Stats
+  {
+    uint64_t count{0};
+    double total_ms{0.0};
+    double min_ms{std::numeric_limits<double>::max()};
+    double max_ms{0.0};
+    double last_ms{0.0};
+  };
+
+  std::string makeLogPath(const std::string &node_name, const std::string &base_log_dir) const;
+  void writeFinishedFrame(uint64_t frame_id, double duration_ms);
+  void writeCanceledFrame(uint64_t frame_id, double elapsed_ms, const std::string &reason);
+  std::string formatStatsLine(const std::string &prefix) const;
+
+  std::unordered_map<uint64_t, TimePoint> active_frames_;
+  uint64_t next_frame_id_{1};
+  Stats stats_;
+  TimePoint last_summary_time_;
+  std::string log_path_;
+  std::ofstream log_file_;
+};
+}  // namespace origincar_base_logging
+
+#endif  // ORIGINCAR_BASE_LOG_HPP_

src/origincar_base/include/origincar_base/origincar_base.h

@@ -4,10 +4,12 @@
 #include <memory>
 #include <inttypes.h>
 #include <array>
+#include <cstdint>
 #include <mutex>
 #include <vector>
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "origincar_base/log.hpp"
 #include "origincar_base/wall_fit_core.hpp"
 #include "origincar_base/wall_kalman_filter.hpp"
 #include <csignal>
@@ -151,6 +153,7 @@ private:
 	auto createQuaternionMsgFromYaw(double yaw);
 	void Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
 	void Apply_Wall_Update();
+	void Print_Timing_Log_If_Due();
 
 	bool Get_Sensor_Data();
 	unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode);
@@ -252,10 +255,15 @@ private:
 	::origincar_wall::WallFitConfig wall_fit_config_;
 	std::unique_ptr<::origincar_wall::WallKalmanFilter> wall_filter_;
 	::origincar_wall::Pose2D laser_pose_;
+	::origincar_base_logging::ScanOdomTimingLogger scan_odom_timing_logger_;
 	std::mutex wall_scan_mutex_;
 	std::vector<::origincar_wall::WallPoint> latest_scan_points_;
+	uint64_t latest_scan_timing_frame_id_;
+	uint64_t pending_odom_timing_frame_id_;
 	bool has_latest_scan_;
 	bool latest_scan_consumed_;
+	bool has_latest_scan_timing_frame_;
+	bool has_pending_odom_timing_frame_;
 };
 
 #endif //_ORIGINCAR_BASE_H_

src/origincar_base/launch/base_serial.launch.py

@@ -5,7 +5,7 @@ import launch_ros.actions
 def generate_launch_description():
     robot_parameters = [
         {'usart_port_name': '/dev/ttyACM0',
-         'serial_baud_rate': 115200,
+         'serial_baud_rate': 921600,
          'robot_frame_id': 'base_footprint',
          'odom_frame_id': 'odom',
          'combined_odom_topic': 'odom_combined',

src/origincar_base/launch/base_serial.launch.py.bak

@@ -9,7 +9,7 @@ def generate_launch_description():
 
     robot_parameters = [
         {'usart_port_name': '/dev/ttyACM0',
-         'serial_baud_rate': 115200,
+         'serial_baud_rate': 921600,
          'robot_frame_id': 'base_link',
          'odom_frame_id': 'odom',
          'cmd_vel': 'cmd_vel',

src/origincar_base/src/log.cpp

@@ -0,0 +1,232 @@
+#include "origincar_base/log.hpp"
+
+#include <cerrno>
+#include <cctype>
+#include <cmath>
+#include <ctime>
+#include <iomanip>
+#include <sstream>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+namespace origincar_base_logging
+{
+namespace
+{
+std::string sanitizeNodeName(const std::string &node_name)
+{
+  std::string result;
+  for (char ch : node_name)
+  {
+    const unsigned char value = static_cast<unsigned char>(ch);
+    if (std::isalnum(value) || ch == '_' || ch == '-')
+    {
+      result.push_back(ch);
+    }
+    else if (!result.empty() && result.back() != '_')
+    {
+      result.push_back('_');
+    }
+  }
+  return result.empty() ? "node" : result;
+}
+
+bool makeDirectory(const std::string &path)
+{
+  if (path.empty())
+  {
+    return false;
+  }
+  if (::mkdir(path.c_str(), 0755) == 0 || errno == EEXIST)
+  {
+    return true;
+  }
+  return false;
+}
+
+bool makeDirectories(const std::string &path)
+{
+  if (path.empty())
+  {
+    return false;
+  }
+
+  std::string current;
+  for (std::size_t index = 0; index < path.size(); ++index)
+  {
+    current.push_back(path[index]);
+    if ((path[index] == '/' && current.size() > 1) || index + 1 == path.size())
+    {
+      if (current.size() > 1 && current.back() == '/')
+      {
+        current.pop_back();
+      }
+      if (!current.empty() && !makeDirectory(current))
+      {
+        return false;
+      }
+      if (index + 1 < path.size() && path[index] == '/')
+      {
+        current.push_back('/');
+      }
+    }
+  }
+  return true;
+}
+
+std::string formatSystemTime()
+{
+  const std::time_t now = std::time(nullptr);
+  std::tm local_time;
+  localtime_r(&now, &local_time);
+  std::ostringstream stream;
+  stream << std::put_time(&local_time, "%Y-%m-%d %H:%M:%S");
+  return stream.str();
+}
+
+std::string formatFileTimestamp()
+{
+  const std::time_t now = std::time(nullptr);
+  std::tm local_time;
+  localtime_r(&now, &local_time);
+  std::ostringstream stream;
+  stream << std::put_time(&local_time, "%Y%m%d_%H%M%S");
+  return stream.str();
+}
+
+double elapsedMs(ScanOdomTimingLogger::TimePoint start, ScanOdomTimingLogger::TimePoint end)
+{
+  return std::chrono::duration_cast<std::chrono::duration<double, std::milli>>(end - start).count();
+}
+}  // namespace
+
+ScanOdomTimingLogger::ScanOdomTimingLogger(
+    const std::string &node_name,
+    const std::string &base_log_dir,
+    TimePoint start_time)
+    : last_summary_time_(start_time),
+      log_path_(makeLogPath(node_name, base_log_dir)),
+      log_file_(log_path_.c_str(), std::ios::out | std::ios::app)
+{
+}
+
+uint64_t ScanOdomTimingLogger::startFrame(TimePoint now)
+{
+  const uint64_t frame_id = next_frame_id_++;
+  active_frames_[frame_id] = now;
+  return frame_id;
+}
+
+bool ScanOdomTimingLogger::finishFrame(uint64_t frame_id, TimePoint now)
+{
+  const auto frame = active_frames_.find(frame_id);
+  if (frame == active_frames_.end())
+  {
+    return false;
+  }
+
+  const double duration_ms = elapsedMs(frame->second, now);
+  active_frames_.erase(frame);
+  stats_.count += 1;
+  stats_.total_ms += duration_ms;
+  stats_.last_ms = duration_ms;
+  stats_.max_ms = std::max(stats_.max_ms, duration_ms);
+  stats_.min_ms = std::min(stats_.min_ms, duration_ms);
+  writeFinishedFrame(frame_id, duration_ms);
+  return true;
+}
+
+bool ScanOdomTimingLogger::cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now)
+{
+  const auto frame = active_frames_.find(frame_id);
+  if (frame == active_frames_.end())
+  {
+    return false;
+  }
+
+  const double elapsed_ms = elapsedMs(frame->second, now);
+  active_frames_.erase(frame);
+  writeCanceledFrame(frame_id, elapsed_ms, reason);
+  return true;
+}
+
+bool ScanOdomTimingLogger::makeSummaryLineIfDue(TimePoint now, std::string *line)
+{
+  if (now - last_summary_time_ < std::chrono::seconds(1))
+  {
+    return false;
+  }
+  last_summary_time_ = now;
+  if (line)
+  {
+    *line = formatStatsLine("scan_to_odom timing");
+  }
+  return true;
+}
+
+const std::string &ScanOdomTimingLogger::logPath() const
+{
+  return log_path_;
+}
+
+bool ScanOdomTimingLogger::isOpen() const
+{
+  return log_file_.is_open();
+}
+
+std::string ScanOdomTimingLogger::makeLogPath(
+    const std::string &node_name,
+    const std::string &base_log_dir) const
+{
+  const std::string node_dir = base_log_dir + "/" + sanitizeNodeName(node_name);
+  makeDirectories(node_dir);
+  return node_dir + "/" + formatFileTimestamp() + ".log";
+}
+
+void ScanOdomTimingLogger::writeFinishedFrame(uint64_t frame_id, double duration_ms)
+{
+  if (!log_file_.is_open())
+  {
+    return;
+  }
+  log_file_ << formatSystemTime()
+            << " frame_id=" << frame_id
+            << " status=finished"
+            << std::fixed << std::setprecision(3)
+            << " duration_ms=" << duration_ms
+            << " " << formatStatsLine("stats")
+            << '\n';
+  log_file_.flush();
+}
+
+void ScanOdomTimingLogger::writeCanceledFrame(uint64_t frame_id, double elapsed_ms, const std::string &reason)
+{
+  if (!log_file_.is_open())
+  {
+    return;
+  }
+  log_file_ << formatSystemTime()
+            << " frame_id=" << frame_id
+            << " status=canceled"
+            << " reason=" << reason
+            << std::fixed << std::setprecision(3)
+            << " elapsed_ms=" << elapsed_ms
+            << '\n';
+  log_file_.flush();
+}
+
+std::string ScanOdomTimingLogger::formatStatsLine(const std::string &prefix) const
+{
+  const double average_ms = stats_.count == 0 ? 0.0 : stats_.total_ms / static_cast<double>(stats_.count);
+  const double min_ms = stats_.count == 0 ? 0.0 : stats_.min_ms;
+  std::ostringstream stream;
+  stream << prefix
+         << " frames=" << stats_.count
+         << std::fixed << std::setprecision(3)
+         << " last_ms=" << stats_.last_ms
+         << " avg_ms=" << average_ms
+         << " max_ms=" << stats_.max_ms
+         << " min_ms=" << min_ms;
+  return stream.str();
+}
+}  // namespace origincar_base_logging

src/origincar_base/src/origincar_base.cpp

@@ -5,6 +5,7 @@
 #include "robot_localization/srv/set_pose.hpp"
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <algorithm>
+#include <chrono>
 #include <cmath>
 
 using std::placeholders::_1;
@@ -278,18 +279,36 @@ void origincar_base::Publish_Odom()
     tf_broadcaster_->sendTransform(t);
   }
   odom_publisher->publish(odom);
+  if (has_pending_odom_timing_frame_)
+  {
+    scan_odom_timing_logger_.finishFrame(pending_odom_timing_frame_id_);
+    has_pending_odom_timing_frame_ = false;
+  }
+  Print_Timing_Log_If_Due();
   robotpose_publisher->publish(robotpose);
   robotvel_publisher->publish(robotvel);
 }
 
 void origincar_base::Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
 {
+  {
+    std::lock_guard<std::mutex> lock(wall_scan_mutex_);
+    if (has_latest_scan_timing_frame_ && !latest_scan_consumed_)
+    {
+      scan_odom_timing_logger_.cancelFrame(latest_scan_timing_frame_id_, "replaced_by_new_scan");
+      has_latest_scan_timing_frame_ = false;
+    }
+  }
+
+  const uint64_t timing_frame_id = scan_odom_timing_logger_.startFrame();
   const auto scan_points = scanMsgToPoints(*scan, wall_scan_stride_);
   const auto pose_frame_points = ::origincar_wall::transformScanPointsToPoseFrame(scan_points, laser_pose_);
   std::lock_guard<std::mutex> lock(wall_scan_mutex_);
   latest_scan_points_ = pose_frame_points;
+  latest_scan_timing_frame_id_ = timing_frame_id;
   has_latest_scan_ = true;
   latest_scan_consumed_ = false;
+  has_latest_scan_timing_frame_ = true;
 }
 
 void origincar_base::Apply_Wall_Update()
@@ -299,6 +318,8 @@ void origincar_base::Apply_Wall_Update()
     return;
   }
   std::vector<::origincar_wall::WallPoint> scan_points;
+  uint64_t timing_frame_id = 0;
+  bool has_timing_frame = false;
   {
     std::lock_guard<std::mutex> lock(wall_scan_mutex_);
     if (!has_latest_scan_ || latest_scan_consumed_)
@@ -306,8 +327,16 @@ void origincar_base::Apply_Wall_Update()
       return;
     }
     scan_points = latest_scan_points_;
+    timing_frame_id = latest_scan_timing_frame_id_;
+    has_timing_frame = has_latest_scan_timing_frame_;
+    has_latest_scan_timing_frame_ = false;
     latest_scan_consumed_ = true;
   }
+  if (has_timing_frame)
+  {
+    pending_odom_timing_frame_id_ = timing_frame_id;
+    has_pending_odom_timing_frame_ = true;
+  }
 
   const auto result = ::origincar_wall::localizeFromScan(scan_points, wall_fit_config_, wall_filter_->pose());
   if (!result.ok)
@@ -325,6 +354,15 @@ void origincar_base::Apply_Wall_Update()
   wall_filter_->correct(result.pose, noise);
 }
 
+void origincar_base::Print_Timing_Log_If_Due()
+{
+  std::string timing_summary;
+  if (scan_odom_timing_logger_.makeSummaryLineIfDue(std::chrono::steady_clock::now(), &timing_summary))
+  {
+    RCLCPP_INFO(this->get_logger(), "%s", timing_summary.c_str());
+  }
+}
+
 void origincar_base::Publish_Voltage()
 {
   std_msgs::msg::Float32 voltage_msgs;
@@ -530,7 +568,8 @@ void origincar_base::Control()
 }
 
 origincar_base::origincar_base()
-    : rclcpp::Node("origincar_base")
+    : rclcpp::Node("origincar_base"),
+      scan_odom_timing_logger_(this->get_name())
 {
   memset(&Robot_Pos, 0, sizeof(Robot_Pos));
   memset(&Robot_Vel, 0, sizeof(Robot_Vel));
@@ -550,11 +589,15 @@ origincar_base::origincar_base()
   gyro_z_low_pass_initialized_ = false;
   has_latest_scan_ = false;
   latest_scan_consumed_ = true;
+  latest_scan_timing_frame_id_ = 0;
+  pending_odom_timing_frame_id_ = 0;
+  has_latest_scan_timing_frame_ = false;
+  has_pending_odom_timing_frame_ = false;
 
-  int serial_baud_rate = 115200;
+  int serial_baud_rate = 921600;
 
   this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
-  this->declare_parameter<int>("serial_baud_rate", 115200);
+  this->declare_parameter<int>("serial_baud_rate", 921600);
   this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
   this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
   this->declare_parameter<std::string>("odom_frame_id", "odom");
@@ -660,6 +703,14 @@ origincar_base::origincar_base()
   {
     RCLCPP_INFO(this->get_logger(), "origincar_base serial port opened");
   }
+  if (scan_odom_timing_logger_.isOpen())
+  {
+    RCLCPP_INFO(this->get_logger(), "scan_to_odom timing log: %s", scan_odom_timing_logger_.logPath().c_str());
+  }
+  else
+  {
+    RCLCPP_WARN(this->get_logger(), "scan_to_odom timing log file could not be opened: %s", scan_odom_timing_logger_.logPath().c_str());
+  }
 }
 
 void sigintHandler(int sig)
@@ -668,7 +719,7 @@ void sigintHandler(int sig)
   printf("OriginBot shutdown...\n");
   serial::Serial Stm32_Serial;
   Stm32_Serial.setPort("/dev/ttyACM0");
-  Stm32_Serial.setBaudrate(115200);
+  Stm32_Serial.setBaudrate(921600);
   serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
   Stm32_Serial.setTimeout(_time);
   Stm32_Serial.open();
@@ -703,7 +754,7 @@ void sigintHandler(int sig)
     {
     }
   }
-  // Shutdown ROS2 and release resources.
+  // Shutdown ROS2 and release resources.
   rclcpp::shutdown();
 }
 
@@ -711,4 +762,4 @@ origincar_base::~origincar_base()
 {
   RCLCPP_INFO(this->get_logger(), "Shutting down");
 }
-
+

src/origincar_base/src/origincar_base.cpp.bak

@@ -373,7 +373,7 @@ origincar_base::origincar_base()
   Robot_Pos.X = 0.54;
   Robot_Pos.Y = 0.2;
 
-  int serial_baud_rate = 115200;
+  int serial_baud_rate = 921600;
 
   this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
   this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
@@ -436,7 +436,7 @@ void sigintHandler(int sig)
       printf("OriginBot shutdown...\n");
     serial::Serial Stm32_Serial;
     Stm32_Serial.setPort("/dev/ttyACM0");
-    Stm32_Serial.setBaudrate(115200);
+    Stm32_Serial.setBaudrate(921600);
     serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
     Stm32_Serial.setTimeout(_time);
     Stm32_Serial.open();                                       

src/origincar_base/test/scan_odom_timing_logger_test.cpp

@@ -0,0 +1,103 @@
+#include "origincar_base/log.hpp"
+
+#include <chrono>
+#include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+#include <unistd.h>
+
+namespace
+{
+using Clock = std::chrono::steady_clock;
+
+void require(bool condition, const std::string &message)
+{
+  if (!condition)
+  {
+    throw std::runtime_error(message);
+  }
+}
+
+void requireContains(const std::string &text, const std::string &expected, const std::string &message)
+{
+  if (text.find(expected) == std::string::npos)
+  {
+    throw std::runtime_error(message + " missing=" + expected + " text=" + text);
+  }
+}
+
+std::string readFile(const std::string &path)
+{
+  std::ifstream input(path.c_str());
+  return std::string((std::istreambuf_iterator<char>(input)), std::istreambuf_iterator<char>());
+}
+
+std::string makeTempDir()
+{
+  const std::string path = "/tmp/origincar_timing_logger_test_" + std::to_string(getpid());
+  std::string command = "rm -rf " + path + " && mkdir -p " + path;
+  require(std::system(command.c_str()) == 0, "create temp log dir");
+  return path;
+}
+
+void testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate()
+{
+  const auto root = makeTempDir();
+  const auto start = Clock::time_point(std::chrono::seconds(0));
+  origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root, start);
+
+  const auto first = logger.startFrame(start);
+  require(logger.finishFrame(first, start + std::chrono::milliseconds(15)), "finish first frame");
+  std::string contents = readFile(logger.logPath());
+  requireContains(contents, "frame_id=1", "first frame id");
+  requireContains(contents, "duration_ms=15.000", "first frame duration");
+  requireContains(contents, "avg_ms=15.000", "first frame average");
+  requireContains(contents, "max_ms=15.000", "first frame max");
+  requireContains(contents, "min_ms=15.000", "first frame min");
+
+  const auto second = logger.startFrame(start + std::chrono::milliseconds(20));
+  require(logger.finishFrame(second, start + std::chrono::milliseconds(50)), "finish second frame");
+  contents = readFile(logger.logPath());
+  requireContains(contents, "frame_id=2", "second frame id");
+  requireContains(contents, "duration_ms=30.000", "second frame duration");
+  requireContains(contents, "avg_ms=22.500", "second frame average");
+  requireContains(contents, "max_ms=30.000", "second frame max");
+  requireContains(contents, "min_ms=15.000", "second frame min");
+}
+
+void testSummaryPrintIsThrottledToOneHz()
+{
+  const auto root = makeTempDir();
+  const auto start = Clock::time_point(std::chrono::seconds(0));
+  origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root, start);
+  std::string summary;
+
+  const auto frame = logger.startFrame(start);
+  require(logger.finishFrame(frame, start + std::chrono::milliseconds(10)), "finish frame");
+  require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(999), &summary),
+          "summary should not be due before one second");
+  require(logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1000), &summary),
+          "summary should be due at one second");
+  requireContains(summary, "frames=1", "summary frame count");
+  requireContains(summary, "avg_ms=10.000", "summary average");
+  require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1500), &summary),
+          "summary should be throttled after printing");
+}
+}  // namespace
+
+int main()
+{
+  try
+  {
+    testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate();
+    testSummaryPrintIsThrottledToOneHz();
+  }
+  catch (const std::exception &error)
+  {
+    std::cerr << "scan_odom_timing_logger_test failed: " << error.what() << std::endl;
+    return 1;
+  }
+  return 0;
+}

src/planner/REAL_ROBOT_RUNBOOK.md

@@ -19,7 +19,7 @@ must be replaced by a map created from the real environment before driving.
 2. The LSLIDAR model, interface and serial device match the selected parameter
    file. The example N10 configuration uses `/dev/ttyCH343USB0`.
 3. The STM32 serial device and baud rate are correct. The default is
-   `/dev/ttyACM0` and 115200.
+   `/dev/ttyACM0` and 921600.
 4. The robot is Ackermann configured and the measured minimum turning radius is
    close to 0.40 m.
 5. The initial robot pose in the map is known and the start cell is free.
@@ -59,7 +59,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
   lidar_model:=N10 \
   start_lidar:=true \
   serial_port:=/dev/ttyACM0 \
-  serial_baud:=115200 \
+  serial_baud:=921600 \
   enable_motion:=false
 ```
 

src/planner/launch/odom_hybrid_astar.launch.py

@@ -331,7 +331,7 @@ def generate_launch_description():
         DeclareLaunchArgument("start_base", default_value="true"),
         DeclareLaunchArgument("start_robot_state_publisher", default_value="true"),
         DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
-        DeclareLaunchArgument("serial_baud", default_value="115200"),
+        DeclareLaunchArgument("serial_baud", default_value="921600"),
         DeclareLaunchArgument(
             "enable_motion",
             default_value="false",

src/planner/launch/real_hybrid_astar.launch.py

@@ -281,7 +281,7 @@ def generate_launch_description():
             description="Start LSLIDAR. Set false when /scan is provided externally.",
         ),
         DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
-        DeclareLaunchArgument("serial_baud", default_value="115200"),
+        DeclareLaunchArgument("serial_baud", default_value="921600"),
         DeclareLaunchArgument(
             "enable_motion",
             default_value="false",

src/planner/操作手册.md

@@ -70,7 +70,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
   lidar_model:=N10 \
   start_lidar:=true \
   serial_port:=/dev/实际底盘串口 \
-  serial_baud:=115200 \
+  serial_baud:=921600 \
   enable_motion:=false
 ```
 
@@ -84,7 +84,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
   lidar_model:=N10 \
   start_lidar:=true \
   serial_port:=/dev/实际底盘串口 \
-  serial_baud:=115200 \
+  serial_baud:=921600 \
   enable_motion:=false
 ```
 
@@ -183,7 +183,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
   lidar_model:=N10 \
   start_lidar:=true \
   serial_port:=/dev/实际底盘串口 \
-  serial_baud:=115200 \
+  serial_baud:=921600 \
   wheelbase:=0.143 \
   max_steering_angle:=0.60 \
   enable_motion:=true

src/racing_control/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(nav_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 
 add_executable(racing_control
@@ -27,6 +28,7 @@ ament_target_dependencies(racing_control
   nav2_msgs
   rclcpp
   rclcpp_action
+  sensor_msgs
   std_msgs
 )
 

src/racing_control/config/racing_control.yaml

@@ -3,6 +3,10 @@ racing_control:
     # Startup
     auto_start: false
     frame_id: odom
+    use_post_qr_pose: true
+    enable_vlm_image_relay: false
+    vlm_image_input_topic: /image
+    vlm_image_output_topic: /vlm_image
 
     # Shared coordination topics
     sign_topic: /sign4return
@@ -44,8 +48,9 @@ racing_control:
     sign_profile_task2: 11
 
     # Pose parameters are flat x/y/yaw-radians triples in frame_id.
-    qr_pose: [4.5303713524852558, 1.226131216530598, 1.2983334871583534]
+    qr_pose: [4.3860322643582883, 1.2934897914487595, 0.94658891138326606]
-    entry_pose: [2.4951887885861144, 2.2268832181534743, 1.5626000867375398]
+    post_qr_pose: [4.4004663023808863, 0.46113350031559491, 1.7514152174101529]
+    entry_pose: [2.499999919243812, 2.1932039710724882, 1.5174118916273021]
 
     # 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

src/racing_control/include/racing_control/racing_control.hpp

@@ -26,6 +26,12 @@ enum class VlmCaptureMode
   PassThrough
 };
 
+enum class QrTransitTarget
+{
+  Entry,
+  PostQr
+};
+
 inline VlmCaptureMode vlmCaptureModeFromString(const std::string & value)
 {
   std::string normalized;
@@ -69,6 +75,17 @@ inline bool shouldAcceptQrDirection(
          incoming_direction != RouteDirection::Unknown;
 }
 
+inline QrTransitTarget qrTransitTargetAfterRecognition(const bool use_post_qr_pose)
+{
+  return use_post_qr_pose ? QrTransitTarget::PostQr : QrTransitTarget::Entry;
+}
+
+inline bool shouldPublishVlmImageFrame(
+  const bool enable_vlm_image_relay, const bool has_latest_image)
+{
+  return enable_vlm_image_relay && has_latest_image;
+}
+
 inline geometry_msgs::msg::PoseStamped poseFromXYYaw(
   const double x, const double y, const double yaw, const std::string & frame_id)
 {

src/racing_control/launch/racing_control.launch.py

@@ -16,6 +16,9 @@ def generate_launch_description():
 
     params_file = LaunchConfiguration("params_file")
     auto_start = LaunchConfiguration("auto_start")
+    enable_vlm_image_relay = LaunchConfiguration("enable_vlm_image_relay")
+    vlm_image_input_topic = LaunchConfiguration("vlm_image_input_topic")
+    vlm_image_output_topic = LaunchConfiguration("vlm_image_output_topic")
 
     racing_control = Node(
         package="racing_control",
@@ -26,6 +29,9 @@ def generate_launch_description():
             params_file,
             {
                 "auto_start": auto_start,
+                "enable_vlm_image_relay": enable_vlm_image_relay,
+                "vlm_image_input_topic": vlm_image_input_topic,
+                "vlm_image_output_topic": vlm_image_output_topic,
             },
         ],
     )
@@ -42,6 +48,21 @@ def generate_launch_description():
                 default_value="false",
                 description="Start the race immediately instead of waiting for SPACE",
             ),
+            DeclareLaunchArgument(
+                "enable_vlm_image_relay",
+                default_value="false",
+                description="Publish one cached image frame to /vlm_image before VLM trigger",
+            ),
+            DeclareLaunchArgument(
+                "vlm_image_input_topic",
+                default_value="/image",
+                description="CompressedImage input topic used for one-frame VLM relay",
+            ),
+            DeclareLaunchArgument(
+                "vlm_image_output_topic",
+                default_value="/vlm_image",
+                description="CompressedImage output topic for one-frame VLM relay",
+            ),
             racing_control,
         ]
     )

src/racing_control/package.xml

@@ -14,6 +14,7 @@
   <depend>nav2_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
+  <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
 
   <exec_depend>ament_index_python</exec_depend>

src/racing_control/src/racing_control.cpp

@@ -24,6 +24,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
+#include "sensor_msgs/msg/compressed_image.hpp"
 #include "std_msgs/msg/int32.hpp"
 #include "std_msgs/msg/string.hpp"
 
@@ -39,6 +40,7 @@ enum class Stage
 {
   Idle,
   NavigateToQr,
+  NavigatePostQr,
   WaitForQr,
   NavigateToEntry,
   SwitchToTask2Profile,
@@ -73,6 +75,8 @@ const char * stageName(const Stage stage)
       return "等待启动";
     case Stage::NavigateToQr:
       return "二维码点导航";
+    case Stage::NavigatePostQr:
+      return "二维码后置点导航";
     case Stage::WaitForQr:
       return "二维码识别/TTS";
     case Stage::NavigateToEntry:
@@ -132,6 +136,15 @@ public:
 
     sign_pub_ = create_publisher<std_msgs::msg::Int32>(sign_topic_, 10);
     guard_path_pub_ = create_publisher<nav_msgs::msg::Path>(guard_input_topic_, 1);
+    if (enable_vlm_image_relay_) {
+      vlm_image_pub_ =
+        create_publisher<sensor_msgs::msg::CompressedImage>(vlm_image_output_topic_, 1);
+      image_sub_ = create_subscription<sensor_msgs::msg::CompressedImage>(
+        vlm_image_input_topic_, 10,
+        [this](sensor_msgs::msg::CompressedImage::SharedPtr msg) {
+          onImage(std::move(msg));
+        });
+    }
 
     qr_sub_ = create_subscription<std_msgs::msg::String>(
       qr_result_topic_, 10,
@@ -190,6 +203,11 @@ private:
 
     auto_start_ = declare_parameter<bool>("auto_start", false);
     use_trajectory_guard_ = declare_parameter<bool>("use_trajectory_guard", true);
+    use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", true);
+    enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
+    vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
+    vlm_image_output_topic_ =
+      declare_parameter<std::string>("vlm_image_output_topic", "/vlm_image");
     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);
@@ -216,6 +234,7 @@ private:
 
     qr_pose_ = singlePoseFromParameter("qr_pose", {0.80, 0.20, 0.0});
     entry_pose_ = singlePoseFromParameter("entry_pose", {1.20, 0.20, 0.0});
+    post_qr_pose_ = singlePoseFromParameter("post_qr_pose", {1.20, 0.20, 0.0});
     vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 4);
 
     const auto clockwise_defaults = std::vector<double>{
@@ -340,7 +359,7 @@ private:
       (now() - qr_result_time_).seconds() >= post_qr_wait_sec_)
     {
       finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label);
-      runEntryNavigation();
+      runQrTransitNavigation();
       return;
     }
 
@@ -420,10 +439,20 @@ private:
     RCLCPP_INFO(get_logger(), "published %s=%d", sign_topic_.c_str(), value);
   }
 
+  void disableQrDetectionOnce()
+  {
+    if (qr_detection_disabled_) {
+      return;
+    }
+    qr_detection_disabled_ = true;
+    publishSign(sign_qr_disable_);
+  }
+
   void runQrNavigation()
   {
     latest_qr_result_.clear();
     selected_direction_ = RouteDirection::Unknown;
+    qr_detection_disabled_ = false;
     publishSign(sign_profile_normal_);
     publishSign(sign_qr_enable_);
     startStage(Stage::NavigateToQr, navigation_timeout_sec_);
@@ -453,9 +482,37 @@ private:
     }
   }
 
+  void runQrTransitNavigation()
+  {
+    if (qrTransitTargetAfterRecognition(use_post_qr_pose_) == QrTransitTarget::PostQr) {
+      runPostQrNavigation();
+    } else {
+      runEntryNavigation();
+    }
+  }
+
+  void runPostQrNavigation()
+  {
+    disableQrDetectionOnce();
+    startStage(Stage::NavigatePostQr, navigation_timeout_sec_);
+    sendNavigateGoal(
+      post_qr_pose_, [this](const bool ok) {
+        if (stage_ != Stage::NavigatePostQr) {
+          RCLCPP_DEBUG(get_logger(), "stale post-QR navigation result ignored");
+          return;
+        }
+        finishStage(ok ? "reached " + poseSummary(post_qr_pose_) : "navigation failed");
+        if (!ok) {
+          failRace("failed to reach post-QR pose");
+          return;
+        }
+        runEntryNavigation();
+      });
+  }
+
   void runEntryNavigation()
   {
-    publishSign(sign_qr_disable_);
+    disableQrDetectionOnce();
     startStage(Stage::NavigateToEntry, navigation_timeout_sec_);
     sendNavigateGoal(
       entry_pose_, [this](const bool ok) {
@@ -610,10 +667,37 @@ private:
       return;
     }
     vlm_capture_triggered_ = true;
+    publishSingleVlmImageFrame();
     publishSign(sign_vlm_trigger_);
     RCLCPP_INFO(get_logger(), "VLM capture triggered: %s", reason.c_str());
   }
 
+  void publishSingleVlmImageFrame()
+  {
+    sensor_msgs::msg::CompressedImage::SharedPtr image;
+    {
+      std::lock_guard<std::mutex> lock(image_mutex_);
+      if (latest_image_) {
+        image = std::make_shared<sensor_msgs::msg::CompressedImage>(*latest_image_);
+      }
+    }
+
+    if (!shouldPublishVlmImageFrame(enable_vlm_image_relay_, static_cast<bool>(image))) {
+      if (enable_vlm_image_relay_) {
+        RCLCPP_WARN(
+          get_logger(), "VLM image relay enabled but no image has been received from %s",
+          vlm_image_input_topic_.c_str());
+      }
+      return;
+    }
+
+    image->header.stamp = now();
+    vlm_image_pub_->publish(*image);
+    RCLCPP_INFO(
+      get_logger(), "published one VLM image frame %s -> %s",
+      vlm_image_input_topic_.c_str(), vlm_image_output_topic_.c_str());
+  }
+
   void maybeTriggerPassThroughVlmCapture()
   {
     if (vlm_capture_mode_ != VlmCaptureMode::PassThrough ||
@@ -779,6 +863,12 @@ private:
     latest_odom_ = std::move(msg);
   }
 
+  void onImage(sensor_msgs::msg::CompressedImage::SharedPtr msg)
+  {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    latest_image_ = std::move(msg);
+  }
+
   void onQrResult(std_msgs::msg::String::SharedPtr msg)
   {
     if (msg->data.empty()) {
@@ -805,7 +895,7 @@ private:
       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();
+      runQrTransitNavigation();
     }
   }
 
@@ -823,6 +913,9 @@ private:
   bool race_started_{false};
   bool auto_start_{false};
   bool use_trajectory_guard_{true};
+  bool use_post_qr_pose_{true};
+  bool enable_vlm_image_relay_{false};
+  bool qr_detection_disabled_{false};
   rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
   rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME};
   double stage_timeout_sec_{0.0};
@@ -832,6 +925,8 @@ private:
   std::string qr_result_topic_;
   std::string vlm_result_topic_;
   std::string odom_topic_;
+  std::string vlm_image_input_topic_;
+  std::string vlm_image_output_topic_;
   std::string navigate_action_;
   std::string compute_path_action_;
   std::string follow_path_action_;
@@ -858,6 +953,7 @@ private:
   int vlm_waypoint_number_{4};
 
   geometry_msgs::msg::PoseStamped qr_pose_;
+  geometry_msgs::msg::PoseStamped post_qr_pose_;
   geometry_msgs::msg::PoseStamped entry_pose_;
   RouteConfig clockwise_route_;
   RouteConfig counterclockwise_route_;
@@ -873,13 +969,17 @@ private:
   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_;
+  sensor_msgs::msg::CompressedImage::SharedPtr latest_image_;
   mutable std::mutex odom_mutex_;
+  mutable std::mutex image_mutex_;
 
   rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_pub_;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr guard_path_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr vlm_image_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::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr image_sub_;
   rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;
   rclcpp_action::Client<ComputePathThroughPoses>::SharedPtr compute_path_client_;
   rclcpp_action::Client<FollowPath>::SharedPtr follow_path_client_;

src/racing_control/test/test_racing_control_helpers.cpp

@@ -73,6 +73,24 @@ TEST(RacingControlHelpers, LatchesFirstKnownQrDirection)
       racing_control::RouteDirection::Counterclockwise));
 }
 
+TEST(RacingControlHelpers, SelectsPostQrTransitTargetWhenEnabled)
+{
+  EXPECT_EQ(
+    racing_control::qrTransitTargetAfterRecognition(true),
+    racing_control::QrTransitTarget::PostQr);
+  EXPECT_EQ(
+    racing_control::qrTransitTargetAfterRecognition(false),
+    racing_control::QrTransitTarget::Entry);
+}
+
+TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable)
+{
+  EXPECT_TRUE(racing_control::shouldPublishVlmImageFrame(true, true));
+  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, true));
+  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(true, false));
+  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false));
+}
+
 TEST(RacingControlHelpers, ParsesVlmCaptureMode)
 {
   EXPECT_EQ(

src/racing_control/点位格式转换.md

@@ -6,7 +6,8 @@
 
 ## 输入文件约定
 
-- `main_.json`：只保存 `qr` 和 `entry` 两个点。
+- `main_.json`：保存 `qr`、`back` 和 `entry` 点时，`back` 用作二维码识别后的过渡点。
+- `post_qr_pose` 是二维码识别后、进入 `entry_pose` 前的可调过渡点；没有单独采点时可以先填成 `entry_pose`。
 - `main_1.json`：顺时针路线点，最后一个点名为 `home`。
 - `main_2.json`：逆时针路线点，最后一个点名为 `home`。
 - `main_1.json` 和 `main_2.json` 的第 4 个路线点是 VLM 拍摄点。正式参数用 `vlm_waypoint_number: 4` 表示，不依赖原始点名。
@@ -28,7 +29,15 @@
 字段映射：
 
 - `main_.json` 中 `qr` -> `qr_pose`
+- `main_.json` 中 `back` -> `post_qr_pose`
 - `main_.json` 中 `entry` -> `entry_pose`
+- 如需二维码后过渡点，可把采到的新点写入 `post_qr_pose`；不需要该点时设置 `use_post_qr_pose: false`。
+
+## VLM 单帧图像转发
+
+- 默认 `enable_vlm_image_relay: false`，总控只发布 `/sign4return=9` 触发 VLM。
+- 设置 `enable_vlm_image_relay: true` 后，总控会缓存 `vlm_image_input_topic` 的最新 `sensor_msgs/msg/CompressedImage`，即将触发 VLM 时先发布一帧到 `vlm_image_output_topic`，再发布 `/sign4return=9`。
+- 如需让 VLM 使用这帧图片，把 VLM 节点的 `image_topic` 设置成 `/vlm_image`。
 - `main_1.json` 中除 `home` 外的所有点 -> `clockwise_waypoints`
 - `main_1.json` 中 `home` -> `clockwise_home_pose`
 - `main_2.json` 中除 `home` 外的所有点 -> `counterclockwise_waypoints`