From 8991e5208e49c9096bd0e1b82767d849353b5d63 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Thu, 6 Aug 2026 16:34:53 +0800 Subject: [PATCH] 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 | 2 +- .../obstacle_nav2/trajectory_guard.hpp | 7 + .../launch/obstacle_nav2.launch.py | 2 +- .../obstacle_nav2/src/trajectory_guard.cpp | 13 + .../src/trajectory_guard_node.cpp | 9 +- .../test/test_trajectory_guard.cpp | 28 + src/origincar_base/CMakeLists.txt | 12 +- src/origincar_base/config/ekf.yaml | 2 +- .../include/origincar_base/log.hpp | 56 + .../include/origincar_base/origincar_base.h | 8 + .../launch/base_serial.launch.py | 2 +- .../launch/base_serial.launch.py.bak | 2 +- src/origincar_base/src/log.cpp | 232 ++++ src/origincar_base/src/origincar_base.cpp | 63 +- src/origincar_base/src/origincar_base.cpp.bak | 4 +- .../test/scan_odom_timing_logger_test.cpp | 103 ++ src/planner/REAL_ROBOT_RUNBOOK.md | 4 +- .../launch/odom_hybrid_astar.launch.py | 2 +- .../launch/real_hybrid_astar.launch.py | 2 +- src/planner/操作手册.md | 6 +- src/racing_control/CMakeLists.txt | 2 + src/racing_control/config/racing_control.yaml | 9 +- .../include/racing_control/racing_control.hpp | 17 + .../launch/racing_control.launch.py | 21 + src/racing_control/package.xml | 1 + .../src/racing_control copy.cpp | 1007 +++++++++++++++++ src/racing_control/src/racing_control.cpp | 106 +- .../test/test_racing_control_helpers.cpp | 18 + src/racing_control/点位格式转换.md | 11 +- 29 files changed, 1722 insertions(+), 29 deletions(-) create mode 100644 src/origincar_base/include/origincar_base/log.hpp create mode 100644 src/origincar_base/src/log.cpp create mode 100644 src/origincar_base/test/scan_odom_timing_logger_test.cpp create mode 100644 src/racing_control/src/racing_control copy.cpp diff --git a/CLAUDE.md b/CLAUDE.md index eab6c1c..3f18beb 100644 --- a/CLAUDE.md +++ b/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` diff --git a/src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp b/src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp index a698259..abbdb88 100644 --- a/src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp +++ b/src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp @@ -59,6 +59,13 @@ std::optional findClearRejoinIndex( const nav_msgs::msg::OccupancyGrid & costmap, const GuardSettings & settings); +std::optional 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 & last_repair_nearest_index, std::size_t nearest_index, diff --git a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py index daff3c7..fd3396c 100755 --- a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py +++ b/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') diff --git a/src/navigation/obstacle_nav2/src/trajectory_guard.cpp b/src/navigation/obstacle_nav2/src/trajectory_guard.cpp index ebadf80..98dc9ec 100644 --- a/src/navigation/obstacle_nav2/src/trajectory_guard.cpp +++ b/src/navigation/obstacle_nav2/src/trajectory_guard.cpp @@ -213,6 +213,19 @@ std::optional findClearRejoinIndex( return std::nullopt; } +std::optional 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 & last_repair_nearest_index, std::size_t nearest_index, diff --git a/src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp b/src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp index bfda08b..fade0db 100644 --- a/src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp +++ b/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( get_parameter("patched_path_topic").as_string(), 1); + reject_path_pub_ = create_publisher( + get_parameter("reject_path_topic").as_string(), 1); cmd_vel_pub_ = create_publisher( get_parameter("cmd_vel_topic").as_string(), 1); @@ -236,8 +239,8 @@ private: search_start = std::max(search_start, advanced); } - const auto rejoin = findClearRejoinIndex( - active_path_, search_start, *latest_costmap_, settings_); + const auto rejoin = findClearRejoinIndexWithPlannerCostmap( + 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::SharedPtr planner_costmap_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr patched_path_pub_; + rclcpp::Publisher::SharedPtr reject_path_pub_; rclcpp::Publisher::SharedPtr cmd_vel_pub_; rclcpp_action::Client::SharedPtr planner_client_; rclcpp_action::Client::SharedPtr follow_client_; diff --git a/src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp b/src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp index 3f68b34..a91c6a3 100644 --- a/src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp +++ b/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 last_repair_index = 10u; diff --git a/src/origincar_base/CMakeLists.txt b/src/origincar_base/CMakeLists.txt index bc4474c..050823f 100644 --- a/src/origincar_base/CMakeLists.txt +++ b/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 + $ + $ +) + 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) diff --git a/src/origincar_base/config/ekf.yaml b/src/origincar_base/config/ekf.yaml index f7538ee..b24791a 100644 --- a/src/origincar_base/config/ekf.yaml +++ b/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 diff --git a/src/origincar_base/include/origincar_base/log.hpp b/src/origincar_base/include/origincar_base/log.hpp new file mode 100644 index 0000000..e12b770 --- /dev/null +++ b/src/origincar_base/include/origincar_base/log.hpp @@ -0,0 +1,56 @@ +#ifndef ORIGINCAR_BASE_LOG_HPP_ +#define ORIGINCAR_BASE_LOG_HPP_ + +#include +#include +#include +#include +#include +#include + +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::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 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_ diff --git a/src/origincar_base/include/origincar_base/origincar_base.h b/src/origincar_base/include/origincar_base/origincar_base.h index c527d53..2748ea1 100644 --- a/src/origincar_base/include/origincar_base/origincar_base.h +++ b/src/origincar_base/include/origincar_base/origincar_base.h @@ -4,10 +4,12 @@ #include #include #include +#include #include #include #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 @@ -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_ diff --git a/src/origincar_base/launch/base_serial.launch.py b/src/origincar_base/launch/base_serial.launch.py index eebfcc2..c8079ec 100644 --- a/src/origincar_base/launch/base_serial.launch.py +++ b/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', diff --git a/src/origincar_base/launch/base_serial.launch.py.bak b/src/origincar_base/launch/base_serial.launch.py.bak index 5093bed..01ae75c 100644 --- a/src/origincar_base/launch/base_serial.launch.py.bak +++ b/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', diff --git a/src/origincar_base/src/log.cpp b/src/origincar_base/src/log.cpp new file mode 100644 index 0000000..b55ffed --- /dev/null +++ b/src/origincar_base/src/log.cpp @@ -0,0 +1,232 @@ +#include "origincar_base/log.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +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(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>(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(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 diff --git a/src/origincar_base/src/origincar_base.cpp b/src/origincar_base/src/origincar_base.cpp index ea906de..985c265 100644 --- a/src/origincar_base/src/origincar_base.cpp +++ b/src/origincar_base/src/origincar_base.cpp @@ -5,6 +5,7 @@ #include "robot_localization/srv/set_pose.hpp" #include #include +#include #include 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 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 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 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("usart_port_name", "/dev/ttyCH343USB0"); - this->declare_parameter("serial_baud_rate", 115200); + this->declare_parameter("serial_baud_rate", 921600); this->declare_parameter("cmd_vel", "cmd_vel"); this->declare_parameter("akm_cmd_vel", "ackermann_cmd"); this->declare_parameter("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"); } - + diff --git a/src/origincar_base/src/origincar_base.cpp.bak b/src/origincar_base/src/origincar_base.cpp.bak index 2a972d6..ce42427 100644 --- a/src/origincar_base/src/origincar_base.cpp.bak +++ b/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("usart_port_name", "/dev/ttyCH343USB0"); this->declare_parameter("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(); diff --git a/src/origincar_base/test/scan_odom_timing_logger_test.cpp b/src/origincar_base/test/scan_odom_timing_logger_test.cpp new file mode 100644 index 0000000..f996464 --- /dev/null +++ b/src/origincar_base/test/scan_odom_timing_logger_test.cpp @@ -0,0 +1,103 @@ +#include "origincar_base/log.hpp" + +#include +#include +#include +#include +#include +#include +#include + +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(input)), std::istreambuf_iterator()); +} + +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; +} diff --git a/src/planner/REAL_ROBOT_RUNBOOK.md b/src/planner/REAL_ROBOT_RUNBOOK.md index fb0722c..c36afd4 100644 --- a/src/planner/REAL_ROBOT_RUNBOOK.md +++ b/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 ``` diff --git a/src/planner/launch/odom_hybrid_astar.launch.py b/src/planner/launch/odom_hybrid_astar.launch.py index 408ce16..dc0b7d3 100644 --- a/src/planner/launch/odom_hybrid_astar.launch.py +++ b/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", diff --git a/src/planner/launch/real_hybrid_astar.launch.py b/src/planner/launch/real_hybrid_astar.launch.py index cf7a852..ad3b1dd 100644 --- a/src/planner/launch/real_hybrid_astar.launch.py +++ b/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", diff --git a/src/planner/操作手册.md b/src/planner/操作手册.md index de70542..39825e2 100644 --- a/src/planner/操作手册.md +++ b/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 diff --git a/src/racing_control/CMakeLists.txt b/src/racing_control/CMakeLists.txt index 9d87a47..c277d47 100644 --- a/src/racing_control/CMakeLists.txt +++ b/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 ) diff --git a/src/racing_control/config/racing_control.yaml b/src/racing_control/config/racing_control.yaml index 56bfcea..747b45f 100644 --- a/src/racing_control/config/racing_control.yaml +++ b/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] - entry_pose: [2.4951887885861144, 2.2268832181534743, 1.5626000867375398] + qr_pose: [4.3860322643582883, 1.2934897914487595, 0.94658891138326606] + 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 diff --git a/src/racing_control/include/racing_control/racing_control.hpp b/src/racing_control/include/racing_control/racing_control.hpp index 9fc9af8..093b24f 100644 --- a/src/racing_control/include/racing_control/racing_control.hpp +++ b/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) { diff --git a/src/racing_control/launch/racing_control.launch.py b/src/racing_control/launch/racing_control.launch.py index 91fca79..9a8677e 100644 --- a/src/racing_control/launch/racing_control.launch.py +++ b/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, ] ) diff --git a/src/racing_control/package.xml b/src/racing_control/package.xml index 812e9da..f988f0e 100644 --- a/src/racing_control/package.xml +++ b/src/racing_control/package.xml @@ -14,6 +14,7 @@ nav2_msgs rclcpp rclcpp_action + sensor_msgs std_msgs ament_index_python diff --git a/src/racing_control/src/racing_control copy.cpp b/src/racing_control/src/racing_control copy.cpp new file mode 100644 index 0000000..b820f19 --- /dev/null +++ b/src/racing_control/src/racing_control copy.cpp @@ -0,0 +1,1007 @@ +#include "racing_control/racing_control.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#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 "sensor_msgs/msg/compressed_image.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, + NavigatePostQr, + WaitForQr, + NavigateToEntry, + SwitchToTask2Profile, + ComputeCirclePath, + ExecuteCirclePath, + SwitchToNormalProfile, + WaitForVlm, + ReturnOrigin, + Finished, + Failed +}; + +enum class RouteSegment +{ + None, + ToVlm, + AfterVlm, + FullRoute +}; + +struct RouteConfig +{ + std::string label; + std::vector 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::NavigatePostQr: + 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; + using ComputeGoalHandle = rclcpp_action::ClientGoalHandle; + using FollowGoalHandle = rclcpp_action::ClientGoalHandle; + + RacingControl() + : Node("racing_control") + { + loadParameters(); + + sign_pub_ = create_publisher(sign_topic_, 10); + guard_path_pub_ = create_publisher(guard_input_topic_, 1); + if (enable_vlm_image_relay_) { + vlm_image_pub_ = + create_publisher(vlm_image_output_topic_, 1); + image_sub_ = create_subscription( + vlm_image_input_topic_, 10, + [this](sensor_msgs::msg::CompressedImage::SharedPtr msg) { + onImage(std::move(msg)); + }); + } + + qr_sub_ = create_subscription( + qr_result_topic_, 10, + [this](std_msgs::msg::String::SharedPtr msg) {onQrResult(std::move(msg));}); + vlm_sub_ = create_subscription( + vlm_result_topic_, 10, + [this](std_msgs::msg::String::SharedPtr msg) {onVlmResult(std::move(msg));}); + odom_sub_ = create_subscription( + odom_topic_, 10, + [this](nav_msgs::msg::Odometry::SharedPtr msg) {onOdom(std::move(msg));}); + + navigate_client_ = rclcpp_action::create_client(this, navigate_action_); + compute_path_client_ = + rclcpp_action::create_client(this, compute_path_action_); + follow_path_client_ = rclcpp_action::create_client(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("frame_id", "odom"); + sign_topic_ = declare_parameter("sign_topic", "/sign4return"); + qr_result_topic_ = declare_parameter("qr_result_topic", "/qr_results"); + vlm_result_topic_ = declare_parameter("vlm_result_topic", "/vlm_result"); + odom_topic_ = declare_parameter("odom_topic", "/odom_combined"); + navigate_action_ = declare_parameter("navigate_action", "/navigate_to_pose"); + compute_path_action_ = + declare_parameter("compute_path_action", "/compute_path_through_poses"); + follow_path_action_ = declare_parameter("follow_path_action", "/follow_path"); + guard_input_topic_ = + declare_parameter( + "trajectory_guard_input_topic", + "/trajectory_guard/input_path"); + planner_id_ = declare_parameter("planner_id", "GridBased"); + controller_id_ = declare_parameter("controller_id", "FollowPath"); + goal_checker_id_ = declare_parameter("goal_checker_id", ""); + + auto_start_ = declare_parameter("auto_start", false); + use_trajectory_guard_ = declare_parameter("use_trajectory_guard", true); + use_post_qr_pose_ = declare_parameter("use_post_qr_pose", true); + enable_vlm_image_relay_ = declare_parameter("enable_vlm_image_relay", false); + vlm_image_input_topic_ = declare_parameter("vlm_image_input_topic", "/image"); + vlm_image_output_topic_ = + declare_parameter("vlm_image_output_topic", "/vlm_image"); + navigation_timeout_sec_ = declare_parameter("navigation_timeout_sec", 120.0); + path_planning_timeout_sec_ = declare_parameter("path_planning_timeout_sec", 30.0); + circle_timeout_sec_ = declare_parameter("circle_timeout_sec", 120.0); + qr_result_timeout_sec_ = declare_parameter("qr_result_timeout_sec", 8.0); + profile_switch_wait_sec_ = declare_parameter("profile_switch_wait_sec", 1.0); + post_qr_wait_sec_ = declare_parameter("post_qr_wait_sec", 1.0); + vlm_capture_wait_sec_ = declare_parameter("vlm_capture_wait_sec", 0.5); + pass_through_vlm_trigger_radius_ = + declare_parameter("pass_through_vlm_trigger_radius", 0.35); + circle_goal_tolerance_ = declare_parameter("circle_goal_tolerance", 0.30); + + const auto vlm_capture_mode = + declare_parameter("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("sign_qr_enable", 0); + sign_qr_disable_ = declare_parameter("sign_qr_disable", 5); + sign_vlm_trigger_ = declare_parameter("sign_vlm_trigger", 9); + sign_profile_normal_ = declare_parameter("sign_profile_normal", 10); + sign_profile_task2_ = declare_parameter("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}); + post_qr_pose_ = singlePoseFromParameter("post_qr_pose", {1.20, 0.20, 0.0}); + vlm_waypoint_number_ = declare_parameter("vlm_waypoint_number", 4); + + const auto clockwise_defaults = std::vector{ + 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{ + 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>("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>( + "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 & defaults) + { + const auto poses = posesFromFlatDoubles( + declare_parameter>(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(~(ICANON | ECHO)); + tcsetattr(STDIN_FILENO, TCSANOW, &raw); + + while (!stop_keyboard_.load()) { + fd_set read_set; + FD_ZERO(&read_set); + FD_SET(STDIN_FILENO, &read_set); + timeval timeout {}; + timeout.tv_sec = 0; + timeout.tv_usec = 200000; + const int ready = select(STDIN_FILENO + 1, &read_set, nullptr, nullptr, &timeout); + if (ready > 0 && FD_ISSET(STDIN_FILENO, &read_set)) { + char c = 0; + if (read(STDIN_FILENO, &c, 1) == 1 && c == ' ') { + start_requested_.store(true); + } + } + } + + tcsetattr(STDIN_FILENO, TCSANOW, &old_termios); + }); + } + + void tick() + { + if (start_requested_.exchange(false)) { + startRace(); + } + + if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) { + return; + } + + const auto elapsed = (now() - stage_start_).seconds(); + if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) { + if (stage_ == Stage::WaitForQr) { + finishStage("QR wait timeout"); + failRace("QR wait timed out before route direction was selected"); + return; + } + if (stage_ == Stage::WaitForVlm) { + RCLCPP_WARN(get_logger(), "VLM capture wait timed out; continuing route"); + finishStage("VLM capture wait timeout"); + runRemainingRouteSegment(); + return; + } + failRace("stage timed out: " + std::string(stageName(stage_))); + return; + } + + if (stage_ == Stage::SwitchToTask2Profile && elapsed >= profile_switch_wait_sec_) { + finishStage("profile switch wait complete"); + runFirstRouteSegment(); + return; + } + + if (stage_ == Stage::SwitchToNormalProfile && elapsed >= profile_switch_wait_sec_) { + finishStage("normal profile restored"); + runReturnOrigin(); + return; + } + + if (stage_ == Stage::WaitForQr && selected_direction_ != RouteDirection::Unknown && + (now() - qr_result_time_).seconds() >= post_qr_wait_sec_) + { + finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label); + runQrTransitNavigation(); + 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 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_); + 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 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() + { + disableQrDetectionOnce(); + 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::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; + 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 lock(image_mutex_); + if (latest_image_) { + image = std::make_shared(*latest_image_); + } + } + + if (!shouldPublishVlmImageFrame(enable_vlm_image_relay_, static_cast(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 || + 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 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 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::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 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::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 stampPoses( + std::vector 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(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 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 lock(odom_mutex_); + latest_odom_ = std::move(msg); + } + + void onImage(sensor_msgs::msg::CompressedImage::SharedPtr msg) + { + std::lock_guard lock(image_mutex_); + latest_image_ = 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); + runQrTransitNavigation(); + } + } + + 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}; + 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}; + + std::string frame_id_; + std::string sign_topic_; + 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_; + 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 post_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 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_; + sensor_msgs::msg::CompressedImage::SharedPtr latest_image_; + mutable std::mutex odom_mutex_; + mutable std::mutex image_mutex_; + + rclcpp::Publisher::SharedPtr sign_pub_; + rclcpp::Publisher::SharedPtr guard_path_pub_; + rclcpp::Publisher::SharedPtr vlm_image_pub_; + rclcpp::Subscription::SharedPtr qr_sub_; + rclcpp::Subscription::SharedPtr vlm_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp_action::Client::SharedPtr navigate_client_; + rclcpp_action::Client::SharedPtr compute_path_client_; + rclcpp_action::Client::SharedPtr follow_path_client_; + rclcpp::TimerBase::SharedPtr tick_timer_; + + std::atomic start_requested_{false}; + std::atomic 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()); + } 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/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp index 60d4e36..b820f19 100644 --- a/src/racing_control/src/racing_control.cpp +++ b/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(sign_topic_, 10); guard_path_pub_ = create_publisher(guard_input_topic_, 1); + if (enable_vlm_image_relay_) { + vlm_image_pub_ = + create_publisher(vlm_image_output_topic_, 1); + image_sub_ = create_subscription( + vlm_image_input_topic_, 10, + [this](sensor_msgs::msg::CompressedImage::SharedPtr msg) { + onImage(std::move(msg)); + }); + } qr_sub_ = create_subscription( qr_result_topic_, 10, @@ -190,6 +203,11 @@ private: auto_start_ = declare_parameter("auto_start", false); use_trajectory_guard_ = declare_parameter("use_trajectory_guard", true); + use_post_qr_pose_ = declare_parameter("use_post_qr_pose", true); + enable_vlm_image_relay_ = declare_parameter("enable_vlm_image_relay", false); + vlm_image_input_topic_ = declare_parameter("vlm_image_input_topic", "/image"); + vlm_image_output_topic_ = + declare_parameter("vlm_image_output_topic", "/vlm_image"); navigation_timeout_sec_ = declare_parameter("navigation_timeout_sec", 120.0); path_planning_timeout_sec_ = declare_parameter("path_planning_timeout_sec", 30.0); circle_timeout_sec_ = declare_parameter("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("vlm_waypoint_number", 4); const auto clockwise_defaults = std::vector{ @@ -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 lock(image_mutex_); + if (latest_image_) { + image = std::make_shared(*latest_image_); + } + } + + if (!shouldPublishVlmImageFrame(enable_vlm_image_relay_, static_cast(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 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::SharedPtr sign_pub_; rclcpp::Publisher::SharedPtr guard_path_pub_; + rclcpp::Publisher::SharedPtr vlm_image_pub_; rclcpp::Subscription::SharedPtr qr_sub_; rclcpp::Subscription::SharedPtr vlm_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr image_sub_; rclcpp_action::Client::SharedPtr navigate_client_; rclcpp_action::Client::SharedPtr compute_path_client_; rclcpp_action::Client::SharedPtr follow_path_client_; diff --git a/src/racing_control/test/test_racing_control_helpers.cpp b/src/racing_control/test/test_racing_control_helpers.cpp index 80615ea..b94477e 100644 --- a/src/racing_control/test/test_racing_control_helpers.cpp +++ b/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( diff --git a/src/racing_control/点位格式转换.md b/src/racing_control/点位格式转换.md index 2ee3f0c..c16678a 100644 --- a/src/racing_control/点位格式转换.md +++ b/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`