diff --git a/CHANGELOG.md b/CHANGELOG.md index 2c46d59..203d597 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,10 +5,15 @@ 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-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 - [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/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/) 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 ef62cd2..afef2dc 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_topic_out: /sensor/pcl2_out -pcl2_frame_out: base_link -timeout: 0.1 +pc2_topics_in: + - /livox/lidar + - /realsense/depth_camera/depth/points +timeout: 0.5 + +tf_duration: 0.05 + +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 f746e87..bfac6be 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 @@ -16,47 +17,45 @@ #include #include #include -#include #include -#include #include +typedef pcl::PointXYZ PointT; +typedef pcl::PointCloud PointCloudT; + using namespace std; class PointCloudMerger { public: - PointCloudMerger(); + PointCloudMerger(ros::NodeHandle nh); ~PointCloudMerger() = default; // Method to process buffers void processBuffers(); private: - ros::NodeHandle nh; - - int pcl2_source_num; + // Subscribers + std::vector pc2_topics_in; size_t queue_size; - double timeout; + std::vector pc2_subs; - std::string pcl2_topic_0, pcl2_frame_0; - ros::Subscriber pcl2_sub_0; - void pcl2Callback0(const sensor_msgs::PointCloud2ConstPtr& pcl2); + double timeout; - std::string pcl2_topic_1, pcl2_frame_1; - ros::Subscriber pcl2_sub_1; - void pcl2Callback1(const sensor_msgs::PointCloud2ConstPtr& pcl2); + // Transform listener + double tf_duration; + tf::TransformListener tf_listener; // Buffers to store point clouds - std::deque::Ptr> buffer_0; - std::deque::Ptr> buffer_1; + std::deque pcT_buffer; std::mutex buffer_mutex; - // Transform listeners - tf::TransformListener listener0, listener1; - double tf_duration; + // Publisher + std::string pc2_topic_out; + std::string pc2_frame_out; + ros::Publisher pc2_pub; - std::string pcl2_topic_out, pcl2_frame_out; - ros::Publisher pcl2_pub; + // Unified callback for point cloud subscribers + void pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2, const 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/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..5238e39 100644 --- a/src/merge_pcl/merge_pcl.cpp +++ b/src/merge_pcl/merge_pcl.cpp @@ -2,119 +2,106 @@ using namespace std; -PointCloudMerger::PointCloudMerger() : nh("~") { - nh.param("pcl2_source_num", pcl2_source_num, 2); - queue_size = boost::thread::hardware_concurrency(); +PointCloudMerger::PointCloudMerger(ros::NodeHandle nh){ + nh.getParam("pc2_topics_in", pc2_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 = std::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 < pc2_topics_in.size(); ++i) { + string pc2_topic_in = pc2_topics_in[i]; + PointCloudT::Ptr pcT(new PointCloudT()); + pcT_buffer.push_back(pcT); - 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)); + pc2_subs.emplace_back( + nh.subscribe( + 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!"); + ROS_INFO("merge_pcl node initialized success!"); } -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; - } - } +void PointCloudMerger::pc2Callback(const sensor_msgs::PointCloud2ConstPtr& pc2_msg, const size_t index) { + PointCloudT::Ptr pcT(new PointCloudT()); + pcl::fromROSMsg(*pc2_msg, *pcT); - std::lock_guard lock(buffer_mutex); - buffer_0.push_back(pclxyz_0); - if (buffer_0.size() > queue_size) { - buffer_0.pop_front(); + if (pcT->empty()) { + PCL_ERROR("Empty pointcloud received! (frame_id %s)\n", pc2_msg->header.frame_id); + return; } -} - -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; + 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); - buffer_1.push_back(pclxyz_1); - if (buffer_1.size() > queue_size) { - buffer_1.pop_front(); - } + { + std::lock_guard lock(buffer_mutex); + *pcT_buffer[index] = *pcT; + } + } } 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(); - } - } - if (pclxyz_0 || pclxyz_1) { - pclxyz_output.reset(new pcl::PointCloud()); - if(pclxyz_0){ - *pclxyz_output += *pclxyz_0; + if(pcT_buffer.empty()){ + PCL_ERROR("All pointclouds are empty! If this continues, be careful!\n"); } - if(pclxyz_1){ - *pclxyz_output += *pclxyz_1; + 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; + } + } + + sensor_msgs::PointCloud2 pc2_out; + pcl::toROSMsg(*pcT_out, pc2_out); + pc2_out.header.frame_id = pc2_frame_out; + + pc2_pub.publish(pc2_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); } - ros::Duration(tf_duration).sleep(); // Small delay to avoid excessive CPU usage + // Small delay to avoid excessive CPU usage + ros::Duration(tf_duration).sleep(); } } int main(int argc, char** argv) { ros::init(argc, argv, "merge_pcl"); - PointCloudMerger pc_merger; + ros::NodeHandle nh("~"); + + PointCloudMerger merge_pcl(nh); - boost::thread processing_thread(&PointCloudMerger::processBuffers, &pc_merger); + std::thread process_thread(&PointCloudMerger::processBuffers, &merge_pcl); ros::spin(); - processing_thread.join(); return 0; }