From e4f73a72f89892569c07ab0c9bf09952d7fbfcb6 Mon Sep 17 00:00:00 2001 From: HuaYuXiao Date: Tue, 24 Dec 2024 11:20:36 +0800 Subject: [PATCH] [upgrade] set `queue_size` of `Subscriber` to `boost::thread::hardware_concurrency()` --- CHANGELOG.md | 3 +++ CMakeLists.txt | 5 +++- README.md | 19 +++++++++++--- config/merge_pcl.yaml | 2 ++ include/merge_pcl/merge_pcl.h | 4 +-- package.xml | 2 +- src/merge_pcl/merge_pcl.cpp | 48 ++++++++++++++++++++--------------- 7 files changed, 54 insertions(+), 29 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b175c0f..a33a0d6 100644 --- a/CHANGELOG.md +++ b/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坐标系下 diff --git a/CMakeLists.txt b/CMakeLists.txt index 58f4fa2..7d3c201 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/README.md b/README.md index f012dee..e371a8f 100644 --- a/README.md +++ b/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 + + + + + +``` ## 建立地图 ```bash -roslaunch easondrone_mapping simulation.launch +roslaunch easondrone_mapping octomap_server.launch ``` ## 保存地图 diff --git a/config/merge_pcl.yaml b/config/merge_pcl.yaml index 40cc9bf..d5972b0 100644 --- a/config/merge_pcl.yaml +++ b/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 diff --git a/include/merge_pcl/merge_pcl.h b/include/merge_pcl/merge_pcl.h index 860619a..e61cef0 100644 --- a/include/merge_pcl/merge_pcl.h +++ b/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> pcl2_sub_0, pcl2_sub_1; + int queue_size; Synchronizer synchronizer_; tf::TransformListener listener0, listener1; ros::Publisher pcl2_pub; diff --git a/package.xml b/package.xml index 552dfd8..88646b3 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ easondrone_mapping - 2.1.3 + 2.1.4 A ROS package for mapping via octomap diff --git a/src/merge_pcl/merge_pcl.cpp b/src/merge_pcl/merge_pcl.cpp index 36c7d87..31d80da 100644 --- a/src/merge_pcl/merge_pcl.cpp +++ b/src/merge_pcl/merge_pcl.cpp @@ -9,45 +9,51 @@ PointCloudMerger::PointCloudMerger() : nh("~"){ nh.param("pcl2_topic_1", pcl2_topic_1, ""); nh.param("pcl2_frame_1", pcl2_frame_1, ""); nh.param("pcl2_topic_out", pcl2_topic_out, ""); + nh.param("pcl2_frame_out", pcl2_frame_out, ""); - pcl2_sub_0.reset(new message_filters::Subscriber(nh, pcl2_topic_0, 50)); - pcl2_sub_1.reset(new message_filters::Subscriber(nh, pcl2_topic_1, 50)); + 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, 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::Ptr pcl_xyz_0, pcl_xyz_1, pcl_xyz_output; + pcl::PointCloud::Ptr pclxyz_0, pclxyz_1, pclxyz_output; - pcl_xyz_0.reset(new pcl::PointCloud()); - 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::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::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::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()); + pclxyz_output.reset(new pcl::PointCloud()); - 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);