lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

前雷达为主体合并点云

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-22 13:41:47 +08:00
parent 0c73b91e1e
commit a28814f597
1 changed files with 99 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp
View File

@ -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,8 +433,11 @@ class LidarMerger : public rclcpp::Node
/* 匹配最新时间戳对齐帧，合并并发布 */
void tryMatchAndPublish(std::deque<ProcessedFrame>& cf, std::deque<ProcessedFrame>& cr)
{
if (cf.empty() || cr.empty()) return;
if (cf.empty()) return;
/* 有后雷达数据时尝试匹配 */
if (!cr.empty())
{
/* 在前 3 帧内寻找时间差最小的配对 */
constexpr size_t SEARCH = 3;
size_t fi_best = 0, ri_best = 0;
@ -454,27 +457,10 @@ class LidarMerger : public rclcpp::Node
}
}
/* 时间差超出容差，不合并（等待另一路的下一帧）
* 注意：这里不做 "age 等待"，直接用最新可用帧 */
if (min_diff > time_tolerance_)
/* 时间差在容差范围内，执行合并发布 */
if (min_diff <= time_tolerance_ ||
(cf.size() == 1 && cr.size() == 1 && min_diff <= time_tolerance_ * 3.0))
{
/* 如果两路都只有一帧且时间差不大于 3 倍容差，强制合并
* 避免两路雷达频率略有差异时永远找不到完美匹配 */
if (cf.size() == 1 && cr.size() == 1 && min_diff <= time_tolerance_ * 3.0)
{
/* 允许宽松合并，继续向下 */
}
else
{
/* 丢弃最旧的帧后返回，等下一帧触发 */
if (cf.back().stamp < cr.back().stamp)
cf.pop_back();
else
cr.pop_back();
return;
}
}
ProcessedFrame& front_frame = cf[fi_best];
ProcessedFrame& rear_frame = cr[ri_best];
@ -510,6 +496,49 @@ class LidarMerger : public rclcpp::Node
cf.erase(cf.begin() + fi_best);
cr.erase(cr.begin() + ri_best);
}
else
{
/* 时间差超出容差，只发布前雷达最新帧 */
ProcessedFrame& front_frame = cf[0];
pub_->publish(*front_frame.ros_cloud);
{
/* 创建新的 shared_ptr 用于栅格化 */
auto front_pcl = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
*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());
}
}
else
{
/* 只有前雷达数据，直接发布 */
ProcessedFrame& front_frame = cf[0];
pub_->publish(*front_frame.ros_cloud);
{
/* 创建新的 shared_ptr 用于栅格化 */
auto front_pcl = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
*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] Only front radar available", front_only_count_.load());
}
cf.erase(cf.begin());
}
}
/* ============================================================
* 坐标变换（线程安全：tf2::Buffer 内部加锁）
@ -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<int> front_rx_{0}, rear_rx_{0}, merged_count_{0};
std::atomic<int> front_rx_{0}, rear_rx_{0}, merged_count_{0}, front_only_count_{0};
};
/* ================================================================