Skip to content

Commit

Permalink
Merge pull request #27 from HuaYuXiao/multithread-dev
Browse files Browse the repository at this point in the history
support for random num of pointcloud input sources
  • Loading branch information
HuaYuXiao authored Dec 25, 2024
2 parents 36df6f2 + 0040b13 commit eaa3bee
Show file tree
Hide file tree
Showing 9 changed files with 140 additions and 133 deletions.
7 changes: 6 additions & 1 deletion CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -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()`
Expand Down
3 changes: 0 additions & 3 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -34,7 +32,6 @@ include_directories(
SYSTEM
include
${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${OCTOMAP_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
Expand Down
41 changes: 33 additions & 8 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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
<launch>
Expand All @@ -35,13 +58,15 @@ rosrun easondrone_mapping pub_pcl_world.py
</launch>
```

## 建立地图
## 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
Expand All @@ -59,17 +84,17 @@ 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`工具查看

```bash
octovis ~/easondrone_ws/reconstruct/easondrone_mapping/map.bt
```

### 方法2:rviz
方法2:rviz

一种方法是在`rviz`中查看

Expand All @@ -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/)
Expand Down
2 changes: 1 addition & 1 deletion config/easondrone.rviz
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
18 changes: 9 additions & 9 deletions config/merge_pcl.yaml
Original file line number Diff line number Diff line change
@@ -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
39 changes: 19 additions & 20 deletions include/merge_pcl/merge_pcl.h
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <thread>
#include <deque>
#include <mutex>
#include <vector>
#include <ros/ros.h>
#include <ros/rate.h>
#include <pcl_ros/point_cloud.h>
Expand All @@ -16,47 +17,45 @@
#include <exception>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <message_filters/subscriber.h>
#include <sensor_msgs/PointCloud2.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> 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<std::string> pc2_topics_in;
size_t queue_size;
double timeout;
std::vector<ros::Subscriber> 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<pcl::PointCloud<pcl::PointXYZ>::Ptr> buffer_0;
std::deque<pcl::PointCloud<pcl::PointXYZ>::Ptr> buffer_1;
std::deque<PointCloudT::Ptr> 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
7 changes: 1 addition & 6 deletions launch/octomap_server.launch
Original file line number Diff line number Diff line change
@@ -1,15 +1,10 @@
<launch>
<!-- filter lidar -->
<!-- <node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter">-->
<!-- <rosparam command="load" file="$(find ego_planner)/config/filter_lidar.yaml"/>-->
<!-- </node>-->

<!-- run the laser_to_pointcloud -->
<!-- <node pkg="easondrone_mapping" type="laser_to_pointcloud.py" name="laser_to_pointcloud"/>-->

<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<rosparam command="load" file="$(find easondrone_mapping)/config/octomap_server.yaml"/>

<remap from="/cloud_in" to="/sensor/pcl2_out"/>
<remap from="/cloud_in" to="/sensor/pc2_out"/>
</node>
</launch>
5 changes: 2 additions & 3 deletions package.xml
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
<?xml version="1.0"?>
<package format="2">
<name>easondrone_mapping</name>
<version>2.1.5</version>
<version>2.1.6</version>
<description>
A ROS package for mapping via octomap
A ROS package to process PointCloud and build OctoMap
</description>

<maintainer email="12010508@mail.sustech.edu.cn">Eason Hua</maintainer>
Expand All @@ -20,7 +20,6 @@
<depend>nav_msgs</depend>
<depend>pcl_ros</depend>
<depend>octomap</depend>
<depend>message_filters</depend>
<depend>tf</depend>
<depend>tf2_ros</depend>
<depend>tf2_sensor_msgs</depend>
Expand Down
Loading

0 comments on commit eaa3bee

Please sign in to comment.