[upgrade] set queue_size of Subscriber to `boost::thread::hardwar…

…e_concurrency()`

CHANGELOG.md

@@ -5,6 +5,9 @@ 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.4 - 2024-12-24
+- [upgrade] set `queue_size` of `Subscriber` to `boost::thread::hardware_concurrency()`
+
 ## v2.1.3: 2024-08-23
 - [new feature] 转发lidar_frame坐标系下的点云到world坐标系下
 

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 2.8.3)
 project(easondrone_mapping)
 
 set(CMAKE_BUILD_TYPE "Release")
@@ -9,6 +9,7 @@ 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
@@ -36,6 +37,7 @@ include_directories(
        ${Eigen3_INCLUDE_DIRS}
        ${PCL_INCLUDE_DIRS}
 		${OCTOMAP_INCLUDE_DIRS}
+		${Boost_INCLUDE_DIRS}
 )
 
 link_directories(${PCL_LIBRARY_DIRS})
@@ -45,6 +47,7 @@ add_executable(merge_pcl src/merge_pcl/merge_pcl.cpp)
 target_link_libraries(merge_pcl
 		${catkin_LIBRARIES}
 		${PCL_LIBRARIES}
+		${Boost_LIBRARIES}
 )
 
 catkin_install_python(PROGRAMS

README.md

@@ -4,15 +4,17 @@
 ![Static Badge](https://img.shields.io/badge/ROS-melodic-22314E?logo=ros)
 ![Static Badge](https://img.shields.io/badge/C%2B%2B-17-00599C?logo=cplusplus)
 ![Static Badge](https://img.shields.io/badge/Python-3.8.10-3776AB?logo=python)
-![Static Badge](https://img.shields.io/badge/Ubuntu-18.04.6-E95420?logo=ubuntu)
+![Static Badge](https://img.shields.io/badge/Ubuntu-20.04.6-E95420?logo=ubuntu)
 
 A ROS package for mapping via octomap.
 
 ## Installation
 
 ```bash
-git clone https://github.com/HuaYuXiao/easondrone_mapping.git ~/easondrone_ws/reconstruct/easondrone_mapping
-cd ~/easondrone_ws && catkin_make install --source reconstruct/easondrone_mapping --build reconstruct/easondrone_mapping/build
+cd ~/easondrone_ws/reconstruct
+git clone https://github.com/HuaYuXiao/easondrone_mapping.git
+cd ~/easondrone_ws
+catkin_make --source reconstruct/easondrone_mapping --build reconstruct/easondrone_mapping/build
 ```
 
 ## 转发点云
@@ -23,11 +25,20 @@ EGO-Planner等规划器要求点云发布在world坐标系下，因此需要将
 rosrun easondrone_mapping pub_pcl_world.py
 ```
 
+## merge pointcloud
+
+```xml
+<launch>
+    <node pkg="easondrone_mapping" type="merge_pcl" name="merge_pcl">
+        <rosparam command="load" file="$(find easondrone_mapping)/config/merge_pcl.yaml" />
+    </node>
+</launch>
+```
 
 ## 建立地图
 
 ```bash
-roslaunch easondrone_mapping simulation.launch
+roslaunch easondrone_mapping octomap_server.launch
 ```
 
 ## 保存地图

config/merge_pcl.yaml

@@ -4,3 +4,5 @@ pcl2_frame_0: laser_livox
 pcl2_topic_1: /realsense/depth_camera/depth/points
 pcl2_frame_0: realsense_camera_frame
 pcl2_topic_out: /sensor/pcl2_out
+pcl2_frame_out: base_link
+timeout: 0.1

include/merge_pcl/merge_pcl.h

@@ -40,10 +40,10 @@ class PointCloudMerger {
 
     int pcl2_source_num;
     std::string pcl2_topic_0, pcl2_topic_1, pcl2_topic_out;
-    std::string pcl2_frame_0, pcl2_frame_1;
-
+    std::string pcl2_frame_0, pcl2_frame_1, pcl2_frame_out;
 
     shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> pcl2_sub_0, pcl2_sub_1;
+    int queue_size;
     Synchronizer synchronizer_;
     tf::TransformListener listener0, listener1;
     ros::Publisher pcl2_pub;

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>easondrone_mapping</name>
-    <version>2.1.3</version>
+    <version>2.1.4</version>
     <description>
         A ROS package for mapping via octomap
     </description>

src/merge_pcl/merge_pcl.cpp

@@ -9,45 +9,51 @@ PointCloudMerger::PointCloudMerger() : nh("~"){
     nh.param<string>("pcl2_topic_1", pcl2_topic_1, "");
     nh.param<string>("pcl2_frame_1", pcl2_frame_1, "");
     nh.param<string>("pcl2_topic_out", pcl2_topic_out, "");
+    nh.param<string>("pcl2_frame_out", pcl2_frame_out, "");
 
-    pcl2_sub_0.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, pcl2_topic_0, 50));
-    pcl2_sub_1.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, pcl2_topic_1, 50));
+    queue_size = boost::thread::hardware_concurrency();
+
+    pcl2_sub_0.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>
+    (nh, pcl2_topic_0, queue_size));
+    pcl2_sub_1.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>
+    (nh, pcl2_topic_1, queue_size));
 
     synchronizer_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(100), *pcl2_sub_0, *pcl2_sub_1));
     synchronizer_->registerCallback(boost::bind(&PointCloudMerger::mergeCallback, this, _1, _2));
 
     pcl2_pub = nh.advertise<sensor_msgs::PointCloud2>(pcl2_topic_out, 10);
 
-    listener0.waitForTransform("base_link", pcl2_frame_0, ros::Time(0), ros::Duration(0.05));
-    listener1.waitForTransform("base_link", pcl2_frame_1, ros::Time(0), ros::Duration(0.05));
+    // TODO: configurable dura
+    listener0.waitForTransform(pcl2_frame_out, pcl2_frame_0, ros::Time(0), ros::Duration(0.05));
+    listener1.waitForTransform(pcl2_frame_out, pcl2_frame_1, ros::Time(0), ros::Duration(0.05));
 
-    std::cout << "[easondrone_mapping] merge_pcl initialized!" << std::endl;
+    ROS_INFO("[easondrone_mapping] merge_pcl initialized!");
 }
 
 void PointCloudMerger::mergeCallback(const sensor_msgs::PointCloud2ConstPtr& pcl2_0, const sensor_msgs::PointCloud2ConstPtr& pcl2_1) {
-    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_xyz_0, pcl_xyz_1, pcl_xyz_output;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pclxyz_0, pclxyz_1, pclxyz_output;
 
-    pcl_xyz_0.reset(new pcl::PointCloud<pcl::PointXYZ>());
-    pcl::fromROSMsg(*pcl2_0, *pcl_xyz_0);
-    // 转换D435i点云到map坐标系
-    pcl_ros::transformPointCloud("base_link", *pcl_xyz_0, *pcl_xyz_0, listener1);
+    pclxyz_0.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::fromROSMsg(*pcl2_0, *pclxyz_0);
+    // TODO: check if both frames are the same
+    pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_0, *pclxyz_0, listener1);
 
-    pcl_xyz_1.reset(new pcl::PointCloud<pcl::PointXYZ>());
-    pcl::fromROSMsg(*pcl2_1, *pcl_xyz_1);
-    // 转换LiDAR点云到map坐标系
-    pcl_ros::transformPointCloud("base_link", *pcl_xyz_1, *pcl_xyz_1, listener0);
+    pclxyz_1.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::fromROSMsg(*pcl2_1, *pclxyz_1);
+    // TODO: check if both frames are the same
+    pcl_ros::transformPointCloud(pcl2_frame_out, *pclxyz_1, *pclxyz_1, listener0);
 
-    pcl_xyz_output.reset(new pcl::PointCloud<pcl::PointXYZ>());
+    pclxyz_output.reset(new pcl::PointCloud<pcl::PointXYZ>());
 
-    pcl_xyz_output->header.seq = pcl_xyz_1->header.seq;
-    pcl_xyz_output->header.stamp = pcl_xyz_1->header.stamp;
-    pcl_xyz_output->header.frame_id = "base_link";
+    pclxyz_output->header.seq = pclxyz_1->header.seq;
+    pclxyz_output->header.stamp = pclxyz_1->header.stamp;
+    pclxyz_output->header.frame_id = pcl2_frame_out;
 
-    *pcl_xyz_output += *pcl_xyz_1;
-    *pcl_xyz_output += *pcl_xyz_0;
+    *pclxyz_output += *pclxyz_1;
+    *pclxyz_output += *pclxyz_0;
 
     sensor_msgs::PointCloud2 pcl2_output;
-    pcl::toROSMsg(*pcl_xyz_output, pcl2_output);
+    pcl::toROSMsg(*pclxyz_output, pcl2_output);
 
     if(pcl2_pub){
         pcl2_pub.publish(pcl2_output);