前雷达为主体合并点云
This commit is contained in:
parent
0c73b91e1e
commit
a28814f597
@ -298,7 +298,7 @@ class LidarMerger : public rclcpp::Node
|
|||||||
|
|
||||||
/* 统计 */
|
/* 统计 */
|
||||||
last_stats_time_ = now();
|
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_,
|
RCLCPP_INFO(get_logger(), "LidarMerger ready | time_tolerance=%.3fs | use_gpu=%s", time_tolerance_,
|
||||||
use_gpu_ ? "true" : "false");
|
use_gpu_ ? "true" : "false");
|
||||||
@ -433,82 +433,111 @@ class LidarMerger : public rclcpp::Node
|
|||||||
/* 匹配最新时间戳对齐帧，合并并发布 */
|
/* 匹配最新时间戳对齐帧，合并并发布 */
|
||||||
void tryMatchAndPublish(std::deque<ProcessedFrame>& cf, std::deque<ProcessedFrame>& cr)
|
void tryMatchAndPublish(std::deque<ProcessedFrame>& cf, std::deque<ProcessedFrame>& cr)
|
||||||
{
|
{
|
||||||
if (cf.empty() || cr.empty()) return;
|
if (cf.empty()) return;
|
||||||
|
|
||||||
/* 在前 3 帧内寻找时间差最小的配对 */
|
/* 有后雷达数据时尝试匹配 */
|
||||||
constexpr size_t SEARCH = 3;
|
if (!cr.empty())
|
||||||
size_t fi_best = 0, ri_best = 0;
|
|
||||||
double min_diff = std::numeric_limits<double>::max();
|
|
||||||
|
|
||||||
for (size_t fi = 0; fi < std::min(SEARCH, cf.size()); ++fi)
|
|
||||||
{
|
{
|
||||||
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<double>::max();
|
||||||
|
|
||||||
|
for (size_t fi = 0; fi < std::min(SEARCH, cf.size()); ++fi)
|
||||||
{
|
{
|
||||||
double diff = std::abs((cf[fi].stamp - cr[ri].stamp).seconds());
|
for (size_t ri = 0; ri < std::min(SEARCH, cr.size()); ++ri)
|
||||||
if (diff < min_diff)
|
|
||||||
{
|
{
|
||||||
min_diff = diff;
|
double diff = std::abs((cf[fi].stamp - cr[ri].stamp).seconds());
|
||||||
fi_best = fi;
|
if (diff < min_diff)
|
||||||
ri_best = ri;
|
{
|
||||||
|
min_diff = diff;
|
||||||
|
fi_best = fi;
|
||||||
|
ri_best = ri;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
/* 时间差超出容差，不合并（等待另一路的下一帧）
|
/* 时间差在容差范围内，执行合并发布 */
|
||||||
* 注意：这里不做 "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)
|
|
||||||
{
|
{
|
||||||
/* 允许宽松合并，继续向下 */
|
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<pcl::PointCloud<pcl::PointXYZ>>();
|
||||||
|
*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::microseconds>(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
|
else
|
||||||
{
|
{
|
||||||
/* 丢弃最旧的帧后返回，等下一帧触发 */
|
/* 时间差超出容差，只发布前雷达最新帧 */
|
||||||
if (cf.back().stamp < cr.back().stamp)
|
ProcessedFrame& front_frame = cf[0];
|
||||||
cf.pop_back();
|
pub_->publish(*front_frame.ros_cloud);
|
||||||
else
|
{
|
||||||
cr.pop_back();
|
/* 创建新的 shared_ptr 用于栅格化 */
|
||||||
return;
|
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[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<pcl::PointCloud<pcl::PointXYZ>>();
|
/* 只有前雷达数据，直接发布 */
|
||||||
*merged_pcl = *front_frame.pcl_cloud + *rear_frame.pcl_cloud;
|
ProcessedFrame& front_frame = cf[0];
|
||||||
grid_queue_.push_drop_oldest(std::move(merged_pcl));
|
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_)
|
if (enable_debug_)
|
||||||
{
|
{
|
||||||
auto dt = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - t_merge)
|
RCLCPP_INFO(get_logger(), "[FrontOnly#%d] Only front radar available", front_only_count_.load());
|
||||||
.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());
|
||||||
/* 从缓存中移除已使用的帧 */
|
}
|
||||||
cf.erase(cf.begin() + fi_best);
|
|
||||||
cr.erase(cr.begin() + ri_best);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ============================================================
|
/* ============================================================
|
||||||
@ -893,7 +922,9 @@ class LidarMerger : public rclcpp::Node
|
|||||||
line.clear();
|
line.clear();
|
||||||
line.reserve(grid_size_ + 4);
|
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)
|
for (int i = 0; i < grid_size_; ++i)
|
||||||
{
|
{
|
||||||
@ -1145,10 +1176,12 @@ class LidarMerger : public rclcpp::Node
|
|||||||
auto t = now();
|
auto t = now();
|
||||||
if ((t - last_stats_time_).seconds() < 5.0) return;
|
if ((t - last_stats_time_).seconds() < 5.0) return;
|
||||||
last_stats_time_ = t;
|
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;
|
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,
|
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",
|
||||||
m, merge_rate, raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(), sor_method_);
|
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_;
|
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};
|
||||||
};
|
};
|
||||||
|
|
||||||
/* ================================================================
|
/* ================================================================
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user