diff --git a/CHANGELOG.md b/CHANGELOG.md index ace39f8..2c46d59 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,11 @@ All notable changes to this project will be documented in this file. 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 +- [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()` - [upgrade] check if both `frame_id`s are the same diff --git a/include/merge_pcl/merge_pcl.h b/include/merge_pcl/merge_pcl.h index b218d54..f746e87 100644 --- a/include/merge_pcl/merge_pcl.h +++ b/include/merge_pcl/merge_pcl.h @@ -5,6 +5,9 @@ #ifndef MERGE_PCL_H #define MERGE_PCL_H +#include +#include +#include #include #include #include @@ -16,9 +19,6 @@ #include #include #include -#include -#include -#include #include using namespace std; @@ -28,23 +28,30 @@ class PointCloudMerger { PointCloudMerger(); ~PointCloudMerger() = default; - void mergeCallback(const sensor_msgs::PointCloud2ConstPtr& pcl2_0, - const sensor_msgs::PointCloud2ConstPtr& pcl2_1); + // Method to process buffers + void processBuffers(); private: - typedef message_filters::sync_policies::ApproximateTime - SyncPolicy; - typedef shared_ptr> Synchronizer; - ros::NodeHandle nh; int pcl2_source_num; + size_t queue_size; + double timeout; + std::string pcl2_topic_0, pcl2_frame_0; + ros::Subscriber pcl2_sub_0; + void pcl2Callback0(const sensor_msgs::PointCloud2ConstPtr& pcl2); + std::string pcl2_topic_1, pcl2_frame_1; - shared_ptr> pcl2_sub_0, pcl2_sub_1; - int queue_size; - Synchronizer synchronizer_; + ros::Subscriber pcl2_sub_1; + void pcl2Callback1(const sensor_msgs::PointCloud2ConstPtr& pcl2); + + // Buffers to store point clouds + std::deque::Ptr> buffer_0; + std::deque::Ptr> buffer_1; + std::mutex buffer_mutex; + // Transform listeners tf::TransformListener listener0, listener1; double tf_duration; diff --git a/package.xml b/package.xml index 88646b3..8eeb33a 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ easondrone_mapping - 2.1.4 + 2.1.5 A ROS package for mapping via octomap diff --git a/src/merge_pcl/merge_pcl.cpp b/src/merge_pcl/merge_pcl.cpp index a293543..c6c26b2 100644 --- a/src/merge_pcl/merge_pcl.cpp +++ b/src/merge_pcl/merge_pcl.cpp @@ -2,75 +2,108 @@ using namespace std; -PointCloudMerger::PointCloudMerger() : nh("~"){ +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); + 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); + nh.param("tf_duration", tf_duration, 0.1); + listener0.waitForTransform(pcl2_frame_out, pcl2_frame_0, ros::Time(0), ros::Duration(tf_duration)); + listener1.waitForTransform(pcl2_frame_out, pcl2_frame_1, ros::Time(0), ros::Duration(tf_duration)); + 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); - queue_size = boost::thread::hardware_concurrency(); - - pcl2_sub_0.reset(new message_filters::Subscriber - (nh, pcl2_topic_0, queue_size)); - pcl2_sub_1.reset(new message_filters::Subscriber - (nh, pcl2_topic_1, queue_size)); - - synchronizer_.reset(new message_filters::Synchronizer(SyncPolicy(100), *pcl2_sub_0, *pcl2_sub_1)); - synchronizer_->registerCallback(boost::bind(&PointCloudMerger::mergeCallback, this, _1, _2)); - - pcl2_pub = nh.advertise(pcl2_topic_out, queue_size); + ROS_INFO("[merge_pcl] Initialized!"); +} - listener0.waitForTransform(pcl2_frame_out, pcl2_frame_0, ros::Time(0), ros::Duration(tf_duration)); - listener1.waitForTransform(pcl2_frame_out, pcl2_frame_1, ros::Time(0), ros::Duration(tf_duration)); +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; + } + } - ROS_INFO("[easondrone_mapping] merge_pcl initialized!"); + std::lock_guard lock(buffer_mutex); + buffer_0.push_back(pclxyz_0); + if (buffer_0.size() > queue_size) { + buffer_0.pop_front(); + } } -void PointCloudMerger::mergeCallback(const sensor_msgs::PointCloud2ConstPtr& pcl2_0, const sensor_msgs::PointCloud2ConstPtr& pcl2_1) { - pcl::PointCloud::Ptr pclxyz_0, pclxyz_1, pclxyz_output; - - pclxyz_0.reset(new pcl::PointCloud()); - pcl::fromROSMsg(*pcl2_0, *pclxyz_0); - // check if both frames are the same - if(pcl2_0->header.frame_id != pcl2_frame_out){ - try{ - pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_0, *pclxyz_0, listener0); - } - catch(...){ - ROS_ERROR("pcl_ros::transformPointCloud failed!"); +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; } } - pclxyz_1.reset(new pcl::PointCloud()); - pcl::fromROSMsg(*pcl2_1, *pclxyz_1); - // check if both frames are the same - if(pcl2_1->header.frame_id != pcl2_frame_out){ - try{ - pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_1, *pclxyz_1, listener1); - } - catch(...){ - ROS_ERROR("pcl_ros::transformPointCloud failed!"); - } + std::lock_guard lock(buffer_mutex); + buffer_1.push_back(pclxyz_1); + if (buffer_1.size() > queue_size) { + buffer_1.pop_front(); } +} - pclxyz_output.reset(new pcl::PointCloud()); +void PointCloudMerger::processBuffers() { + while (ros::ok()) { + pcl::PointCloud::Ptr pclxyz_0, pclxyz_1, pclxyz_output; + + { + std::lock_guard lock(buffer_mutex); + 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(); + } + } - pclxyz_output->header.seq = pclxyz_1->header.seq; - pclxyz_output->header.stamp = pclxyz_1->header.stamp; - pclxyz_output->header.frame_id = pcl2_frame_out; + if (pclxyz_0 || pclxyz_1) { + pclxyz_output.reset(new pcl::PointCloud()); + if(pclxyz_0){ + *pclxyz_output += *pclxyz_0; + } + if(pclxyz_1){ + *pclxyz_output += *pclxyz_1; + } - *pclxyz_output += *pclxyz_1; - *pclxyz_output += *pclxyz_0; + sensor_msgs::PointCloud2 pcl2_output; + pcl2_output.header.frame_id = pcl2_frame_out; + pcl::toROSMsg(*pclxyz_output, pcl2_output); - sensor_msgs::PointCloud2 pcl2_output; - pcl::toROSMsg(*pclxyz_output, pcl2_output); + pcl2_pub.publish(pcl2_output); + } - if(pcl2_pub){ - pcl2_pub.publish(pcl2_output); + ros::Duration(tf_duration).sleep(); // Small delay to avoid excessive CPU usage } } @@ -78,6 +111,10 @@ int main(int argc, char** argv) { ros::init(argc, argv, "merge_pcl"); PointCloudMerger pc_merger; + boost::thread processing_thread(&PointCloudMerger::processBuffers, &pc_merger); + ros::spin(); + processing_thread.join(); + return 0; }