From a28814f5970b26f9f772db1fa2bc2dc95702514f Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Wed, 22 Apr 2026 13:41:47 +0800 Subject: [PATCH] =?UTF-8?q?=E5=89=8D=E9=9B=B7=E8=BE=BE=E4=B8=BA=E4=B8=BB?= =?UTF-8?q?=E4=BD=93=E5=90=88=E5=B9=B6=E7=82=B9=E4=BA=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/merge_two_lidars.cpp | 165 +++++++++++------- 1 file changed, 99 insertions(+), 66 deletions(-) diff --git a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp index 2692128..735175a 100644 --- a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp +++ b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp @@ -298,7 +298,7 @@ class LidarMerger : public rclcpp::Node /* 统计 */ last_stats_time_ = now(); - front_rx_ = rear_rx_ = merged_count_ = 0; + front_rx_ = rear_rx_ = merged_count_ = front_only_count_ = 0; RCLCPP_INFO(get_logger(), "LidarMerger ready | time_tolerance=%.3fs | use_gpu=%s", time_tolerance_, use_gpu_ ? "true" : "false"); @@ -433,82 +433,111 @@ class LidarMerger : public rclcpp::Node /* 匹配最新时间戳对齐帧，合并并发布 */ void tryMatchAndPublish(std::deque& cf, std::deque& cr) { - if (cf.empty() || cr.empty()) return; + if (cf.empty()) return; - /* 在前 3 帧内寻找时间差最小的配对 */ - constexpr size_t SEARCH = 3; - size_t fi_best = 0, ri_best = 0; - double min_diff = std::numeric_limits::max(); - - for (size_t fi = 0; fi < std::min(SEARCH, cf.size()); ++fi) + /* 有后雷达数据时尝试匹配 */ + if (!cr.empty()) { - for (size_t ri = 0; ri < std::min(SEARCH, cr.size()); ++ri) + /* 在前 3 帧内寻找时间差最小的配对 */ + constexpr size_t SEARCH = 3; + size_t fi_best = 0, ri_best = 0; + double min_diff = std::numeric_limits::max(); + + for (size_t fi = 0; fi < std::min(SEARCH, cf.size()); ++fi) { - double diff = std::abs((cf[fi].stamp - cr[ri].stamp).seconds()); - if (diff < min_diff) + for (size_t ri = 0; ri < std::min(SEARCH, cr.size()); ++ri) { - min_diff = diff; - fi_best = fi; - ri_best = ri; + double diff = std::abs((cf[fi].stamp - cr[ri].stamp).seconds()); + if (diff < min_diff) + { + min_diff = diff; + fi_best = fi; + ri_best = ri; + } } } - } - /* 时间差超出容差，不合并（等待另一路的下一帧） - * 注意：这里不做 "age 等待"，直接用最新可用帧 */ - if (min_diff > time_tolerance_) - { - /* 如果两路都只有一帧且时间差不大于 3 倍容差，强制合并 - * 避免两路雷达频率略有差异时永远找不到完美匹配 */ - if (cf.size() == 1 && cr.size() == 1 && min_diff <= time_tolerance_ * 3.0) + /* 时间差在容差范围内，执行合并发布 */ + if (min_diff <= time_tolerance_ || + (cf.size() == 1 && cr.size() == 1 && min_diff <= time_tolerance_ * 3.0)) { - /* 允许宽松合并，继续向下 */ + ProcessedFrame& front_frame = cf[fi_best]; + ProcessedFrame& rear_frame = cr[ri_best]; + + auto t_merge = std::chrono::steady_clock::now(); + + /* 合并 ROS 点云（内存拷贝，μs 级） */ + auto merged_ros = mergeRosClouds(front_frame, rear_frame); + if (!merged_ros) goto cleanup; + + /* 发布 */ + pub_->publish(*merged_ros); + pool_.release(merged_ros); + ++merged_count_; + + /* 栅格化：push 到 grid_queue_，由独立线程异步处理，不阻塞发布 */ + { + auto merged_pcl = std::make_shared>(); + *merged_pcl = *front_frame.pcl_cloud + *rear_frame.pcl_cloud; + grid_queue_.push_drop_oldest(std::move(merged_pcl)); + } + + if (enable_debug_) + { + auto dt = std::chrono::duration_cast(std::chrono::steady_clock::now() - t_merge) + .count(); + auto e2e = (now() - front_frame.received_time).seconds(); + RCLCPP_INFO(get_logger(), "[Merge#%d] timediff=%.3fs, merge=%.1fms, e2e_delay=%.3fs", merged_count_.load(), + min_diff, dt / 1000.0, e2e); + } + + cleanup: + /* 从缓存中移除已使用的帧 */ + cf.erase(cf.begin() + fi_best); + cr.erase(cr.begin() + ri_best); } else { - /* 丢弃最旧的帧后返回，等下一帧触发 */ - if (cf.back().stamp < cr.back().stamp) - cf.pop_back(); - else - cr.pop_back(); - return; + /* 时间差超出容差，只发布前雷达最新帧 */ + ProcessedFrame& front_frame = cf[0]; + pub_->publish(*front_frame.ros_cloud); + { + /* 创建新的 shared_ptr 用于栅格化 */ + auto front_pcl = std::make_shared>(); + *front_pcl = *front_frame.pcl_cloud; + grid_queue_.push_drop_oldest(std::move(front_pcl)); + } + ++front_only_count_; + + if (enable_debug_) + { + RCLCPP_INFO(get_logger(), "[FrontOnly#%d] No match (min_diff=%.3fs), published front radar only", + front_only_count_.load(), min_diff); + } + + cf.erase(cf.begin()); } } - - ProcessedFrame& front_frame = cf[fi_best]; - ProcessedFrame& rear_frame = cr[ri_best]; - - auto t_merge = std::chrono::steady_clock::now(); - - /* 合并 ROS 点云（内存拷贝，μs 级） */ - auto merged_ros = mergeRosClouds(front_frame, rear_frame); - if (!merged_ros) goto cleanup; - - /* 发布 */ - pub_->publish(*merged_ros); - pool_.release(merged_ros); - ++merged_count_; - - /* 栅格化：push 到 grid_queue_，由独立线程异步处理，不阻塞发布 */ + else { - auto merged_pcl = std::make_shared>(); - *merged_pcl = *front_frame.pcl_cloud + *rear_frame.pcl_cloud; - grid_queue_.push_drop_oldest(std::move(merged_pcl)); - } + /* 只有前雷达数据，直接发布 */ + ProcessedFrame& front_frame = cf[0]; + pub_->publish(*front_frame.ros_cloud); + { + /* 创建新的 shared_ptr 用于栅格化 */ + auto front_pcl = std::make_shared>(); + *front_pcl = *front_frame.pcl_cloud; + grid_queue_.push_drop_oldest(std::move(front_pcl)); + } + ++front_only_count_; - if (enable_debug_) - { - auto dt = std::chrono::duration_cast(std::chrono::steady_clock::now() - t_merge) - .count(); - auto e2e = (now() - front_frame.received_time).seconds(); - RCLCPP_INFO(get_logger(), "[Merge#%d] timediff=%.3fs, merge=%.1fms, e2e_delay=%.3fs", merged_count_.load(), - min_diff, dt / 1000.0, e2e); - } + if (enable_debug_) + { + RCLCPP_INFO(get_logger(), "[FrontOnly#%d] Only front radar available", front_only_count_.load()); + } - cleanup: - /* 从缓存中移除已使用的帧 */ - cf.erase(cf.begin() + fi_best); - cr.erase(cr.begin() + ri_best); + cf.erase(cf.begin()); + } } /* ============================================================ @@ -893,7 +922,9 @@ class LidarMerger : public rclcpp::Node line.clear(); line.reserve(grid_size_ + 4); - std::printf(" 障碍物矩阵 [%dx%d] merged#%d\n", grid_size_, grid_size_, merged_count_.load()); + int total = merged_count_.load() + front_only_count_.load(); + std::printf(" 障碍物矩阵 [%dx%d] total#%d (merged#%d front#%d)\n", + grid_size_, grid_size_, total, merged_count_.load(), front_only_count_.load()); for (int i = 0; i < grid_size_; ++i) { @@ -1145,10 +1176,12 @@ class LidarMerger : public rclcpp::Node auto t = now(); if ((t - last_stats_time_).seconds() < 5.0) return; last_stats_time_ = t; - int f = front_rx_.load(), r = rear_rx_.load(), m = merged_count_.load(); + int f = front_rx_.load(), r = rear_rx_.load(), m = merged_count_.load(), fo = front_only_count_.load(); + int total_pub = m + fo; double merge_rate = (f > 0) ? m * 100.0 / std::min(f, r) : 0.0; - LOG_INFO("Stats | rx(F=%d,R=%d) merged=%d(%.0f%%) | queue(raw_F=%zu,raw_R=%zu,proc=%zu) | sor_method=%d", f, r, - m, merge_rate, raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(), sor_method_); + LOG_INFO("Stats | rx(F=%d,R=%d) merged=%d(%.0f%%) front_only=%d total=%d | queue(raw_F=%zu,raw_R=%zu,proc=%zu) | sor_method=%d", + f, r, m, merge_rate, fo, total_pub, + raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(), sor_method_); } /* ============================================================ @@ -1206,7 +1239,7 @@ class LidarMerger : public rclcpp::Node /* -- 统计 -- */ rclcpp::Time last_stats_time_; - std::atomic front_rx_{0}, rear_rx_{0}, merged_count_{0}; + std::atomic front_rx_{0}, rear_rx_{0}, merged_count_{0}, front_only_count_{0}; }; /* ================================================================