From 60d4f132a5b7d7397bb3ebb24ee7ccbad8f5699c Mon Sep 17 00:00:00 2001 From: HuaYuXiao Date: Tue, 24 Dec 2024 17:32:38 +0800 Subject: [PATCH] support for one single pointcloud input --- CHANGELOG.md | 4 +- src/merge_pcl/merge_pcl.cpp | 87 +++++++++++++++++++++---------------- 2 files changed, 52 insertions(+), 39 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c2a08d7..2c46d59 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). ## v2.1.5 - 2024-12-24 -- (beta)[upgrade] apply multithread to process several pointclouds parallely +- [upgrade] apply multithread to process several pointclouds parallely +- [upgrade] store pointclouds with `std::deque` +- support for one single pointcloud input ## v2.1.4 - 2024-12-24 - [upgrade] set `queue_size` of `Subscriber` to `boost::thread::hardware_concurrency()` diff --git a/src/merge_pcl/merge_pcl.cpp b/src/merge_pcl/merge_pcl.cpp index 21f9127..c6c26b2 100644 --- a/src/merge_pcl/merge_pcl.cpp +++ b/src/merge_pcl/merge_pcl.cpp @@ -5,14 +5,17 @@ using namespace std; PointCloudMerger::PointCloudMerger() : nh("~") { nh.param("pcl2_source_num", pcl2_source_num, 2); queue_size = boost::thread::hardware_concurrency(); + nh.param("timeout", timeout, 0.5); nh.param("pcl2_topic_0", pcl2_topic_0, ""); nh.param("pcl2_frame_0", pcl2_frame_0, ""); - pcl2_sub_0 = nh.subscribe(pcl2_topic_0, queue_size, &PointCloudMerger::pcl2Callback0, this); + pcl2_sub_0 = nh.subscribe + (pcl2_topic_0, queue_size, &PointCloudMerger::pcl2Callback0, this); nh.param("pcl2_topic_1", pcl2_topic_1, ""); nh.param("pcl2_frame_1", pcl2_frame_1, ""); - pcl2_sub_1 = nh.subscribe(pcl2_topic_1, queue_size, &PointCloudMerger::pcl2Callback1, this); + pcl2_sub_1 = nh.subscribe + (pcl2_topic_1, queue_size, &PointCloudMerger::pcl2Callback1, this); nh.param("tf_duration", tf_duration, 0.1); listener0.waitForTransform(pcl2_frame_out, pcl2_frame_0, ros::Time(0), ros::Duration(tf_duration)); @@ -20,22 +23,49 @@ PointCloudMerger::PointCloudMerger() : nh("~") { nh.param("pcl2_topic_out", pcl2_topic_out, ""); nh.param("pcl2_frame_out", pcl2_frame_out, ""); - pcl2_pub = nh.advertise(pcl2_topic_out, queue_size); + pcl2_pub = nh.advertise + (pcl2_topic_out, queue_size); ROS_INFO("[merge_pcl] Initialized!"); } void PointCloudMerger::pcl2Callback0(const sensor_msgs::PointCloud2ConstPtr& pcl2) { + pcl::PointCloud::Ptr pclxyz_0(new pcl::PointCloud()); + pcl::fromROSMsg(*pcl2, *pclxyz_0); + + if (pcl2->header.frame_id != pcl2_frame_out) { + try { + pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_0, *pclxyz_0, listener0); + } + catch (...) { + ROS_ERROR("Transform failed for pcl2_0!"); + return; + } + } + std::lock_guard lock(buffer_mutex); - buffer_0.push_back(pcl2); + buffer_0.push_back(pclxyz_0); if (buffer_0.size() > queue_size) { buffer_0.pop_front(); } } void PointCloudMerger::pcl2Callback1(const sensor_msgs::PointCloud2ConstPtr& pcl2) { + pcl::PointCloud::Ptr pclxyz_1(new pcl::PointCloud()); + pcl::fromROSMsg(*pcl2, *pclxyz_1); + + if (pcl2->header.frame_id != pcl2_frame_out) { + try { + pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_1, *pclxyz_1, listener1); + } + catch (...) { + ROS_ERROR("Transform failed for pcl2_1!"); + return; + } + } + std::lock_guard lock(buffer_mutex); - buffer_1.push_back(pcl2); + buffer_1.push_back(pclxyz_1); if (buffer_1.size() > queue_size) { buffer_1.pop_front(); } @@ -43,56 +73,37 @@ void PointCloudMerger::pcl2Callback1(const sensor_msgs::PointCloud2ConstPtr& pcl void PointCloudMerger::processBuffers() { while (ros::ok()) { - sensor_msgs::PointCloud2ConstPtr pcl2_0 = nullptr; - sensor_msgs::PointCloud2ConstPtr pcl2_1 = nullptr; + pcl::PointCloud::Ptr pclxyz_0, pclxyz_1, pclxyz_output; { std::lock_guard lock(buffer_mutex); - if (!buffer_0.empty() && !buffer_1.empty()) { - pcl2_0 = buffer_0.front(); - pcl2_1 = buffer_1.front(); + if (!buffer_0.empty()) { + pclxyz_0 = buffer_0.front(); buffer_0.pop_front(); + } + if (!buffer_1.empty()) { + pclxyz_1 = buffer_1.front(); buffer_1.pop_front(); } } - if (pcl2_0 && pcl2_1) { - pcl::PointCloud::Ptr pclxyz_0, pclxyz_1, pclxyz_output; - - pclxyz_0.reset(new pcl::PointCloud()); - pcl::fromROSMsg(*pcl2_0, *pclxyz_0); - if (pcl2_0->header.frame_id != pcl2_frame_out) { - try { - pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_0, *pclxyz_0, listener0); - } catch (...) { - ROS_ERROR("Transform failed for pcl2_0!"); - continue; - } + if (pclxyz_0 || pclxyz_1) { + pclxyz_output.reset(new pcl::PointCloud()); + if(pclxyz_0){ + *pclxyz_output += *pclxyz_0; } - - pclxyz_1.reset(new pcl::PointCloud()); - pcl::fromROSMsg(*pcl2_1, *pclxyz_1); - if (pcl2_1->header.frame_id != pcl2_frame_out) { - try { - pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_1, *pclxyz_1, listener1); - } catch (...) { - ROS_ERROR("Transform failed for pcl2_1!"); - continue; - } + if(pclxyz_1){ + *pclxyz_output += *pclxyz_1; } - pclxyz_output.reset(new pcl::PointCloud()); - *pclxyz_output += *pclxyz_0; - *pclxyz_output += *pclxyz_1; - sensor_msgs::PointCloud2 pcl2_output; + pcl2_output.header.frame_id = pcl2_frame_out; pcl::toROSMsg(*pclxyz_output, pcl2_output); - pcl2_output.header.frame_id = pcl2_frame_out; pcl2_pub.publish(pcl2_output); } - ros::Duration(0.01).sleep(); // Small delay to avoid excessive CPU usage + ros::Duration(tf_duration).sleep(); // Small delay to avoid excessive CPU usage } }