From 86f467528192d03304468ddd98db5d5b79e0e816 Mon Sep 17 00:00:00 2001 From: HuaYuXiao Date: Tue, 24 Dec 2024 20:16:23 +0800 Subject: [PATCH 1/6] support for random num of pointcloud input sources --- CHANGELOG.md | 4 +- CMakeLists.txt | 3 - config/merge_pcl.yaml | 14 ++--- include/merge_pcl/merge_pcl.h | 29 +++++---- package.xml | 5 +- src/merge_pcl/merge_pcl.cpp | 112 +++++++++++++--------------------- 6 files changed, 67 insertions(+), 100 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2c46d59..a28cb03 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,10 +5,12 @@ 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.6 - 2024-12-24 +- [new feature] support for random num of pointcloud input sources + ## 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()` diff --git a/CMakeLists.txt b/CMakeLists.txt index 7d3c201..d52d5da 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,14 +6,12 @@ set(CMAKE_CXX_FLAGS "-std=c++17") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") set(PYTHON_EXECUTABLE "/usr/bin/python3.8") -find_package(Eigen3 REQUIRED) find_package(PCL 1.7 REQUIRED) find_package(octomap REQUIRED) find_package(Boost REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp rospy - message_filters tf pcl_ros std_msgs @@ -34,7 +32,6 @@ include_directories( SYSTEM include ${catkin_INCLUDE_DIRS} - ${Eigen3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} diff --git a/config/merge_pcl.yaml b/config/merge_pcl.yaml index ef62cd2..2e3f7d2 100644 --- a/config/merge_pcl.yaml +++ b/config/merge_pcl.yaml @@ -1,9 +1,9 @@ -pcl2_source_num: 2 -pcl2_topic_0: /livox/lidar -pcl2_frame_0: laser_livox -pcl2_topic_1: /realsense/depth_camera/depth/points -pcl2_frame_0: realsense_camera_frame -tf_duration: 0.1 +pcl2_topics_in: + - /livox/lidar + - /realsense/depth_camera/depth/points +timeout: 0.5 + +tf_duration: 0.05 + pcl2_topic_out: /sensor/pcl2_out pcl2_frame_out: base_link -timeout: 0.1 diff --git a/include/merge_pcl/merge_pcl.h b/include/merge_pcl/merge_pcl.h index f746e87..25e0c34 100644 --- a/include/merge_pcl/merge_pcl.h +++ b/include/merge_pcl/merge_pcl.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -34,29 +35,27 @@ class PointCloudMerger { private: ros::NodeHandle nh; - int pcl2_source_num; + // Parameters + std::vector pcl2_topics_in; size_t queue_size; + std::vector pcl2_subs; 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; - ros::Subscriber pcl2_sub_1; - void pcl2Callback1(const sensor_msgs::PointCloud2ConstPtr& pcl2); + // Transform listeners + std::vector> tf_listeners; + double tf_duration; // Buffers to store point clouds - std::deque::Ptr> buffer_0; - std::deque::Ptr> buffer_1; + std::deque::Ptr> pclxyz_buffer; std::mutex buffer_mutex; - // Transform listeners - tf::TransformListener listener0, listener1; - double tf_duration; - - std::string pcl2_topic_out, pcl2_frame_out; + // Output topic and frame + std::string pcl2_topic_out; + std::string pcl2_frame_out; ros::Publisher pcl2_pub; + + // Unified callback for point cloud subscribers + void pcl2Callback(const sensor_msgs::PointCloud2ConstPtr& pcl2, size_t index); }; #endif //MERGE_PCL_H diff --git a/package.xml b/package.xml index 8eeb33a..5dfa265 100644 --- a/package.xml +++ b/package.xml @@ -1,9 +1,9 @@ easondrone_mapping - 2.1.5 + 2.1.6 - A ROS package for mapping via octomap + A ROS package to process PointCloud and build OctoMap Eason Hua @@ -20,7 +20,6 @@ nav_msgs pcl_ros octomap - message_filters tf tf2_ros tf2_sensor_msgs diff --git a/src/merge_pcl/merge_pcl.cpp b/src/merge_pcl/merge_pcl.cpp index c6c26b2..073a939 100644 --- a/src/merge_pcl/merge_pcl.cpp +++ b/src/merge_pcl/merge_pcl.cpp @@ -3,104 +3,74 @@ using namespace std; PointCloudMerger::PointCloudMerger() : nh("~") { - nh.param("pcl2_source_num", pcl2_source_num, 2); - queue_size = boost::thread::hardware_concurrency(); + nh.getParam("pcl2_topics_in", pcl2_topics_in); 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); + queue_size = boost::thread::hardware_concurrency(); - 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); + for (size_t i = 0; i < pcl2_topics_in.size(); ++i) { + string pcl2_topic_in = pcl2_topics_in[i]; + pcl::PointCloud::Ptr pclxyz(new pcl::PointCloud()); + pclxyz_buffer.push_back(pclxyz); + tf_listeners.emplace_back(new tf::TransformListener()); + + pcl2_subs.emplace_back( + nh.subscribe( + pcl2_topic_in, queue_size, + boost::bind(&PointCloudMerger::pcl2Callback, this, _1, i) + ) + ); + } - 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("tf_duration", tf_duration, 0.05); 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!"); + ROS_INFO("[merge_pcl] Initialized success!"); } -void PointCloudMerger::pcl2Callback0(const sensor_msgs::PointCloud2ConstPtr& pcl2) { - pcl::PointCloud::Ptr pclxyz_0(new pcl::PointCloud()); - pcl::fromROSMsg(*pcl2, *pclxyz_0); +void PointCloudMerger::pcl2Callback(const sensor_msgs::PointCloud2ConstPtr& pcl2_msg, size_t index) { + pcl::PointCloud::Ptr pclxyz(new pcl::PointCloud()); + pcl::fromROSMsg(*pcl2_msg, *pclxyz); - if (pcl2->header.frame_id != pcl2_frame_out) { + if (pcl2_msg->header.frame_id != pcl2_frame_out) { try { - pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_0, *pclxyz_0, listener0); + pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz, *pclxyz, *tf_listeners[index]); } catch (...) { - ROS_ERROR("Transform failed for pcl2_0!"); + ROS_ERROR("Transform failed for pcl2_topic_in index %zu!", index); return; } } std::lock_guard lock(buffer_mutex); - 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(pclxyz_1); - if (buffer_1.size() > queue_size) { - buffer_1.pop_front(); - } + *pclxyz_buffer[index] = *pclxyz; } void PointCloudMerger::processBuffers() { while (ros::ok()) { - pcl::PointCloud::Ptr pclxyz_0, pclxyz_1, pclxyz_output; + pcl::PointCloud::Ptr pclxyz_out(new pcl::PointCloud()); { 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(); + for (const auto& pclxyz : pclxyz_buffer) { + if (!pclxyz->empty()) { + *pclxyz_out += *pclxyz; + } } } - 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; - } + if (!pclxyz_out->empty()) { + sensor_msgs::PointCloud2 pcl2_out; + pcl::toROSMsg(*pclxyz_out, pcl2_out); + pcl2_out.header.frame_id = pcl2_frame_out; - sensor_msgs::PointCloud2 pcl2_output; - pcl2_output.header.frame_id = pcl2_frame_out; - pcl::toROSMsg(*pclxyz_output, pcl2_output); - - pcl2_pub.publish(pcl2_output); + pcl2_pub.publish(pcl2_out); + } + else{ + ROS_WARN("empty pointcloud!"); } ros::Duration(tf_duration).sleep(); // Small delay to avoid excessive CPU usage @@ -109,12 +79,12 @@ void PointCloudMerger::processBuffers() { int main(int argc, char** argv) { ros::init(argc, argv, "merge_pcl"); - PointCloudMerger pc_merger; + PointCloudMerger merge_pcl; - boost::thread processing_thread(&PointCloudMerger::processBuffers, &pc_merger); + boost::thread process_thread(&PointCloudMerger::processBuffers, &merge_pcl); ros::spin(); - processing_thread.join(); + process_thread.join(); return 0; } From a1e3fc2b44b3b8dd322c1b764447244f79a43008 Mon Sep 17 00:00:00 2001 From: HuaYuXiao Date: Tue, 24 Dec 2024 20:32:09 +0800 Subject: [PATCH 2/6] apply typedef to simplify --- config/easondrone.rviz | 2 +- config/merge_pcl.yaml | 6 ++-- include/merge_pcl/merge_pcl.h | 19 ++++++------ launch/octomap_server.launch | 7 +---- src/merge_pcl/merge_pcl.cpp | 54 +++++++++++++++++------------------ 5 files changed, 42 insertions(+), 46 deletions(-) diff --git a/config/easondrone.rviz b/config/easondrone.rviz index 14c3124..0d06b85 100644 --- a/config/easondrone.rviz +++ b/config/easondrone.rviz @@ -347,7 +347,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points - Topic: /sensor/pcl2_out + Topic: /sensor/pc2_out Unreliable: false Use Fixed Frame: true Use rainbow: true diff --git a/config/merge_pcl.yaml b/config/merge_pcl.yaml index 2e3f7d2..afef2dc 100644 --- a/config/merge_pcl.yaml +++ b/config/merge_pcl.yaml @@ -1,9 +1,9 @@ -pcl2_topics_in: +pc2_topics_in: - /livox/lidar - /realsense/depth_camera/depth/points timeout: 0.5 tf_duration: 0.05 -pcl2_topic_out: /sensor/pcl2_out -pcl2_frame_out: base_link +pc2_topic_out: /sensor/pc2_out +pc2_frame_out: base_link diff --git a/include/merge_pcl/merge_pcl.h b/include/merge_pcl/merge_pcl.h index 25e0c34..5c0cd1a 100644 --- a/include/merge_pcl/merge_pcl.h +++ b/include/merge_pcl/merge_pcl.h @@ -17,11 +17,12 @@ #include #include #include -#include #include -#include #include +typedef pcl::PointXYZ PointT; +typedef pcl::PointCloud PointCloudT; + using namespace std; class PointCloudMerger { @@ -36,9 +37,9 @@ class PointCloudMerger { ros::NodeHandle nh; // Parameters - std::vector pcl2_topics_in; + std::vector pc2_topics_in; size_t queue_size; - std::vector pcl2_subs; + std::vector pc2_subs; double timeout; // Transform listeners @@ -46,16 +47,16 @@ class PointCloudMerger { double tf_duration; // Buffers to store point clouds - std::deque::Ptr> pclxyz_buffer; + std::deque pcT_buffer; std::mutex buffer_mutex; // Output topic and frame - std::string pcl2_topic_out; - std::string pcl2_frame_out; - ros::Publisher pcl2_pub; + std::string pc2_topic_out; + std::string pc2_frame_out; + ros::Publisher pc2_pub; // Unified callback for point cloud subscribers - void pcl2Callback(const sensor_msgs::PointCloud2ConstPtr& pcl2, size_t index); + void pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2, size_t index); }; #endif //MERGE_PCL_H diff --git a/launch/octomap_server.launch b/launch/octomap_server.launch index 5032ab4..ee11399 100644 --- a/launch/octomap_server.launch +++ b/launch/octomap_server.launch @@ -1,15 +1,10 @@ - - - - - - + diff --git a/src/merge_pcl/merge_pcl.cpp b/src/merge_pcl/merge_pcl.cpp index 073a939..9aee975 100644 --- a/src/merge_pcl/merge_pcl.cpp +++ b/src/merge_pcl/merge_pcl.cpp @@ -3,71 +3,71 @@ using namespace std; PointCloudMerger::PointCloudMerger() : nh("~") { - nh.getParam("pcl2_topics_in", pcl2_topics_in); + nh.getParam("pc2_topics_in", pc2_topics_in); nh.param("timeout", timeout, 0.5); queue_size = boost::thread::hardware_concurrency(); - for (size_t i = 0; i < pcl2_topics_in.size(); ++i) { - string pcl2_topic_in = pcl2_topics_in[i]; - pcl::PointCloud::Ptr pclxyz(new pcl::PointCloud()); - pclxyz_buffer.push_back(pclxyz); + for (size_t i = 0; i < pc2_topics_in.size(); ++i) { + string pc2_topic_in = pc2_topics_in[i]; + PointCloudT::Ptr pcT(new PointCloudT()); + pcT_buffer.push_back(pcT); tf_listeners.emplace_back(new tf::TransformListener()); - pcl2_subs.emplace_back( + pc2_subs.emplace_back( nh.subscribe( - pcl2_topic_in, queue_size, - boost::bind(&PointCloudMerger::pcl2Callback, this, _1, i) + pc2_topic_in, queue_size, + boost::bind(&PointCloudMerger::pc2Callback, this, _1, i) ) ); } nh.param("tf_duration", tf_duration, 0.05); - 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); + nh.param("pc2_topic_out", pc2_topic_out, ""); + nh.param("pc2_frame_out", pc2_frame_out, ""); + pc2_pub = nh.advertise(pc2_topic_out, queue_size); ROS_INFO("[merge_pcl] Initialized success!"); } -void PointCloudMerger::pcl2Callback(const sensor_msgs::PointCloud2ConstPtr& pcl2_msg, size_t index) { - pcl::PointCloud::Ptr pclxyz(new pcl::PointCloud()); - pcl::fromROSMsg(*pcl2_msg, *pclxyz); +void PointCloudMerger::pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2_msg, size_t index) { + PointCloudT::Ptr pcT(new PointCloudT()); + pcl::fromROSMsg(*pc2_msg, *pcT); - if (pcl2_msg->header.frame_id != pcl2_frame_out) { + if (pc2_msg->header.frame_id != pc2_frame_out) { try { - pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz, *pclxyz, *tf_listeners[index]); + pcl_ros::transformPointCloud(pc2_frame_out, *pcT, *pcT, *tf_listeners[index]); } catch (...) { - ROS_ERROR("Transform failed for pcl2_topic_in index %zu!", index); + ROS_ERROR("Transform failed for pc2_topic_in index %zu!", index); return; } } std::lock_guard lock(buffer_mutex); - *pclxyz_buffer[index] = *pclxyz; + *pcT_buffer[index] = *pcT; } void PointCloudMerger::processBuffers() { while (ros::ok()) { - pcl::PointCloud::Ptr pclxyz_out(new pcl::PointCloud()); + PointCloudT::Ptr pcT_out(new PointCloudT()); { std::lock_guard lock(buffer_mutex); - for (const auto& pclxyz : pclxyz_buffer) { - if (!pclxyz->empty()) { - *pclxyz_out += *pclxyz; + for (const auto& pcT : pcT_buffer) { + if (!pcT->empty()) { + *pcT_out += *pcT; } } } - if (!pclxyz_out->empty()) { - sensor_msgs::PointCloud2 pcl2_out; - pcl::toROSMsg(*pclxyz_out, pcl2_out); - pcl2_out.header.frame_id = pcl2_frame_out; + if (!pcT_out->empty()) { + sensor_msgs::PointCloud2 pc2_out; + pcl::toROSMsg(*pcT_out, pc2_out); + pc2_out.header.frame_id = pc2_frame_out; - pcl2_pub.publish(pcl2_out); + pc2_pub.publish(pc2_out); } else{ ROS_WARN("empty pointcloud!"); From 973b91ea385a096258c68507899bab4df8a80c57 Mon Sep 17 00:00:00 2001 From: HuaYuXiao Date: Wed, 25 Dec 2024 22:07:11 +0800 Subject: [PATCH 3/6] replace `boost::thread` with `std::thread` --- CHANGELOG.md | 4 +++- src/merge_pcl/merge_pcl.cpp | 27 ++++++++++++++++++--------- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a28cb03..a61c520 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,8 +5,10 @@ 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.6 - 2024-12-24 +## v2.1.6 - 2024-12-25 - [new feature] support for random num of pointcloud input sources +- [bug fix] a single `tf::TransformListener` is sufficient +- replace `boost::thread` with `std::thread` ## v2.1.5 - 2024-12-24 - [upgrade] apply multithread to process several pointclouds parallely diff --git a/src/merge_pcl/merge_pcl.cpp b/src/merge_pcl/merge_pcl.cpp index 9aee975..1b58ec0 100644 --- a/src/merge_pcl/merge_pcl.cpp +++ b/src/merge_pcl/merge_pcl.cpp @@ -6,7 +6,7 @@ PointCloudMerger::PointCloudMerger() : nh("~") { nh.getParam("pc2_topics_in", pc2_topics_in); nh.param("timeout", timeout, 0.5); - queue_size = boost::thread::hardware_concurrency(); + queue_size = std::thread::hardware_concurrency(); for (size_t i = 0; i < pc2_topics_in.size(); ++i) { string pc2_topic_in = pc2_topics_in[i]; @@ -28,7 +28,7 @@ PointCloudMerger::PointCloudMerger() : nh("~") { nh.param("pc2_frame_out", pc2_frame_out, ""); pc2_pub = nh.advertise(pc2_topic_out, queue_size); - ROS_INFO("[merge_pcl] Initialized success!"); + ROS_INFO("merge_pcl node initialized success!"); } void PointCloudMerger::pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2_msg, size_t index) { @@ -38,9 +38,9 @@ void PointCloudMerger::pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2_m if (pc2_msg->header.frame_id != pc2_frame_out) { try { pcl_ros::transformPointCloud(pc2_frame_out, *pcT, *pcT, *tf_listeners[index]); - } + } catch (...) { - ROS_ERROR("Transform failed for pc2_topic_in index %zu!", index); + ROS_ERROR("Transform failed for pc2 %zu!", index); return; } } @@ -56,7 +56,15 @@ void PointCloudMerger::processBuffers() { { std::lock_guard lock(buffer_mutex); for (const auto& pcT : pcT_buffer) { - if (!pcT->empty()) { + if (pcT->empty()) { + PCL_ERROR("Empty pointcloud received! If this continues, be careful!\n"); + continue; + } + else if(pcT_out->empty()){ + *pcT_out += *pcT; + } + else{ + // TODO: Optional ICP *pcT_out += *pcT; } } @@ -70,10 +78,11 @@ void PointCloudMerger::processBuffers() { pc2_pub.publish(pc2_out); } else{ - ROS_WARN("empty pointcloud!"); + PCL_ERROR("All pointclouds are empty! If this continues, be careful!\n"); } - ros::Duration(tf_duration).sleep(); // Small delay to avoid excessive CPU usage + // Small delay to avoid excessive CPU usage + ros::Duration(tf_duration).sleep(); } } @@ -81,10 +90,10 @@ int main(int argc, char** argv) { ros::init(argc, argv, "merge_pcl"); PointCloudMerger merge_pcl; - boost::thread process_thread(&PointCloudMerger::processBuffers, &merge_pcl); + std::thread process_thread(&PointCloudMerger::processBuffers, &merge_pcl); ros::spin(); - process_thread.join(); + // process_thread.join(); return 0; } From 79b48a86f55a7a49165cc929b8b319964d3d71e2 Mon Sep 17 00:00:00 2001 From: HuaYuXiao Date: Wed, 25 Dec 2024 22:48:53 +0800 Subject: [PATCH 4/6] [bug fix] a single `tf::TransformListener` is sufficient --- include/merge_pcl/merge_pcl.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/include/merge_pcl/merge_pcl.h b/include/merge_pcl/merge_pcl.h index 5c0cd1a..627ceb1 100644 --- a/include/merge_pcl/merge_pcl.h +++ b/include/merge_pcl/merge_pcl.h @@ -27,24 +27,22 @@ using namespace std; class PointCloudMerger { public: - PointCloudMerger(); + PointCloudMerger(ros::NodeHandle nh); ~PointCloudMerger() = default; // Method to process buffers void processBuffers(); private: - ros::NodeHandle nh; - // Parameters std::vector pc2_topics_in; size_t queue_size; std::vector pc2_subs; double timeout; - // Transform listeners - std::vector> tf_listeners; + // Transform listener double tf_duration; + tf::TransformListener tf_listener; // Buffers to store point clouds std::deque pcT_buffer; From 1ce4151de5136e1e3c9a2302ada9d547800a843f Mon Sep 17 00:00:00 2001 From: HuaYuXiao Date: Wed, 25 Dec 2024 23:02:19 +0800 Subject: [PATCH 5/6] improve safety and efficiency for buffer process --- CHANGELOG.md | 1 + include/merge_pcl/merge_pcl.h | 7 +-- src/merge_pcl/merge_pcl.cpp | 86 +++++++++++++++++++---------------- 3 files changed, 52 insertions(+), 42 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a61c520..203d597 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## v2.1.6 - 2024-12-25 - [new feature] support for random num of pointcloud input sources - [bug fix] a single `tf::TransformListener` is sufficient +- improve safety and efficiency for buffer process - replace `boost::thread` with `std::thread` ## v2.1.5 - 2024-12-24 diff --git a/include/merge_pcl/merge_pcl.h b/include/merge_pcl/merge_pcl.h index 627ceb1..bfac6be 100644 --- a/include/merge_pcl/merge_pcl.h +++ b/include/merge_pcl/merge_pcl.h @@ -34,10 +34,11 @@ class PointCloudMerger { void processBuffers(); private: - // Parameters + // Subscribers std::vector pc2_topics_in; size_t queue_size; std::vector pc2_subs; + double timeout; // Transform listener @@ -48,13 +49,13 @@ class PointCloudMerger { std::deque pcT_buffer; std::mutex buffer_mutex; - // Output topic and frame + // Publisher std::string pc2_topic_out; std::string pc2_frame_out; ros::Publisher pc2_pub; // Unified callback for point cloud subscribers - void pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2, size_t index); + void pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2, const size_t index); }; #endif //MERGE_PCL_H diff --git a/src/merge_pcl/merge_pcl.cpp b/src/merge_pcl/merge_pcl.cpp index 1b58ec0..5238e39 100644 --- a/src/merge_pcl/merge_pcl.cpp +++ b/src/merge_pcl/merge_pcl.cpp @@ -2,7 +2,7 @@ using namespace std; -PointCloudMerger::PointCloudMerger() : nh("~") { +PointCloudMerger::PointCloudMerger(ros::NodeHandle nh){ nh.getParam("pc2_topics_in", pc2_topics_in); nh.param("timeout", timeout, 0.5); @@ -12,7 +12,6 @@ PointCloudMerger::PointCloudMerger() : nh("~") { string pc2_topic_in = pc2_topics_in[i]; PointCloudT::Ptr pcT(new PointCloudT()); pcT_buffer.push_back(pcT); - tf_listeners.emplace_back(new tf::TransformListener()); pc2_subs.emplace_back( nh.subscribe( @@ -31,69 +30,78 @@ PointCloudMerger::PointCloudMerger() : nh("~") { ROS_INFO("merge_pcl node initialized success!"); } -void PointCloudMerger::pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2_msg, size_t index) { +void PointCloudMerger::pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2_msg, const size_t index) { PointCloudT::Ptr pcT(new PointCloudT()); pcl::fromROSMsg(*pc2_msg, *pcT); - if (pc2_msg->header.frame_id != pc2_frame_out) { - try { - pcl_ros::transformPointCloud(pc2_frame_out, *pcT, *pcT, *tf_listeners[index]); - } - catch (...) { - ROS_ERROR("Transform failed for pc2 %zu!", index); - return; - } + if (pcT->empty()) { + PCL_ERROR("Empty pointcloud received! (frame_id %s)\n", pc2_msg->header.frame_id); + return; } + else{ + if (pc2_msg->header.frame_id != pc2_frame_out) { + try { + pcl_ros::transformPointCloud(pc2_frame_out, *pcT, *pcT, tf_listener); + } + catch (...) { + ROS_ERROR("Transform failure for pointcloud! (frame_id %s)", pc2_msg->header.frame_id); + return; + } + } - std::lock_guard lock(buffer_mutex); - *pcT_buffer[index] = *pcT; + { + std::lock_guard lock(buffer_mutex); + *pcT_buffer[index] = *pcT; + } + } } void PointCloudMerger::processBuffers() { while (ros::ok()) { - PointCloudT::Ptr pcT_out(new PointCloudT()); - { std::lock_guard lock(buffer_mutex); - for (const auto& pcT : pcT_buffer) { - if (pcT->empty()) { - PCL_ERROR("Empty pointcloud received! If this continues, be careful!\n"); - continue; - } - else if(pcT_out->empty()){ - *pcT_out += *pcT; - } - else{ - // TODO: Optional ICP - *pcT_out += *pcT; - } + + if(pcT_buffer.empty()){ + PCL_ERROR("All pointclouds are empty! If this continues, be careful!\n"); } - } + else{ + PointCloudT::Ptr pcT_out(new PointCloudT()); + + for (const auto& pcT : pcT_buffer) { + if (pcT->empty()) { + continue; + } + else if(pcT_out->empty()){ + *pcT_out += *pcT; + } + else{ + // TODO: Optional ICP + *pcT_out += *pcT; + } + } - if (!pcT_out->empty()) { - sensor_msgs::PointCloud2 pc2_out; - pcl::toROSMsg(*pcT_out, pc2_out); - pc2_out.header.frame_id = pc2_frame_out; + sensor_msgs::PointCloud2 pc2_out; + pcl::toROSMsg(*pcT_out, pc2_out); + pc2_out.header.frame_id = pc2_frame_out; - pc2_pub.publish(pc2_out); - } - else{ - PCL_ERROR("All pointclouds are empty! If this continues, be careful!\n"); + pc2_pub.publish(pc2_out); + } } // Small delay to avoid excessive CPU usage - ros::Duration(tf_duration).sleep(); + ros::Duration(tf_duration).sleep(); } } int main(int argc, char** argv) { ros::init(argc, argv, "merge_pcl"); - PointCloudMerger merge_pcl; + ros::NodeHandle nh("~"); + + PointCloudMerger merge_pcl(nh); std::thread process_thread(&PointCloudMerger::processBuffers, &merge_pcl); ros::spin(); - // process_thread.join(); return 0; } From 0040b13e39b63c889ec6b2c58ed2b4f19ba03210 Mon Sep 17 00:00:00 2001 From: easonhua <12010508@mail.sustech.edu.cn> Date: Wed, 25 Dec 2024 23:05:36 +0800 Subject: [PATCH 6/6] Update README.md --- README.md | 41 +++++++++++++++++++++++++++++++++-------- 1 file changed, 33 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index e371a8f..6f9c23a 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ ![Static Badge](https://img.shields.io/badge/Python-3.8.10-3776AB?logo=python) ![Static Badge](https://img.shields.io/badge/Ubuntu-20.04.6-E95420?logo=ubuntu) -A ROS package for mapping via octomap. +A ROS package to process PointCloud and build OctoMap ## Installation @@ -25,7 +25,30 @@ EGO-Planner等规划器要求点云发布在world坐标系下，因此需要将 rosrun easondrone_mapping pub_pcl_world.py ``` -## merge pointcloud +## Merge Multiple sensor_msgs::PointCloud2 + +### Config Parameters + +```yaml +pc2_topics_in: + - /livox/lidar + - /realsense/depth_camera/depth/points +timeout: 0.5 + +tf_duration: 0.05 + +# The Iterative Closest Point algorithm +icp_enable: true +icp_max_iter: 1 +icp_tf_epsilon: 1e-8 +icp_euclidean_fitness_epsilon: 1e-5 +icp_max_corr_d: 0.05 + +pc2_topic_out: /sensor/pc2_out +pc2_frame_out: base_link +``` + +### Launch ROS Node ```xml @@ -35,13 +58,15 @@ rosrun easondrone_mapping pub_pcl_world.py ``` -## 建立地图 +## Build OctoMap + +### Launch ROS Node ```bash roslaunch easondrone_mapping octomap_server.launch ``` -## 保存地图 +### Save Map > You are trying to invoke `octomap_saver` as an argument to the `octomap_server` node. However, `octomap_saver` is a node of its own, so you only have to start it from a separate terminal while `octomap_server` is running. Check the documentation at http://wiki.ros.org/octomap_server#octomap_saver @@ -59,9 +84,9 @@ rosrun octomap_server octomap_saver -f ~/easondrone_ws/reconstruct/easondrone_ma - ⭐️ https://github.com/OctoMap/octomap_mapping/blob/kinetic-devel/octomap_server/src/OctomapServer.cpp - ⭐️ https://groups.google.com/g/octomap/c/ZyfNzcuGlY0?pli=1 -## 查看地图 +### 查看地图 -### 方法1：octovis +方法1：octovis 也可以借助`octovis`工具查看 @@ -69,7 +94,7 @@ rosrun octomap_server octomap_saver -f ~/easondrone_ws/reconstruct/easondrone_ma octovis ~/easondrone_ws/reconstruct/easondrone_mapping/map.bt ``` -### 方法2：rviz +方法2：rviz 一种方法是在`rviz`中查看 @@ -86,7 +111,7 @@ rosrun octomap_server octomap_server_node ~/easondrone_ws/reconstruct/easondrone - [在ROS中将点云（PointCloud2）生成Octomap，rviz可视化显示](https://blog.csdn.net/qq_41816368/article/details/133929136) - ⭐️ [octomap in rviz and occupancy grids in 3D maps](https://robotics.stackexchange.com/questions/41362/octomap-in-rviz-and-occupancy-grids-in-3d-maps) -## 加载地图 +### 加载地图 参考： - ⭐ [how to use octomap_server?](https://answers.ros.org/question/361841/how-to-use-octomap_server/)