Merge pull request #19 from HuaYuXiao/dev-pcl

[Update] Transfer PointCloud with `pcl_ros::transformPointCloud`

Author: HuaYuXiao

CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
        visualization_msgs
 	   prometheus_msgs
 		message_filters
+		tf
 	)
 
 catkin_package(

package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>uav_octomapping</name>
-    <version>2.0.0</version>
+    <version>2.1.0</version>
     <description>
-        A ROS package to build 3D octomap with 2D lidar
+        A ROS package to build octomap with LiDAR and D435i
     </description>
     <author email="[email protected]">Eason Yi</author>
     <maintainer email="[email protected]">Eason Hua</maintainer>
@@ -22,6 +22,7 @@
     <build_depend>pcl_ros</build_depend>
     <build_depend>octomap</build_depend>
     <build_depend>message_filters</build_depend>
+    <build_depend>tf</build_depend>
 
     <build_export_depend>roscpp</build_export_depend>
     <build_export_depend>rospy</build_export_depend>
@@ -33,6 +34,7 @@
     <build_export_depend>pcl_ros</build_export_depend>
     <build_export_depend>octomap</build_export_depend>
     <build_export_depend>message_filters</build_export_depend>
+    <build_export_depend>tf</build_export_depend>
 
     <exec_depend>roscpp</exec_depend>
     <exec_depend>rospy</exec_depend>
@@ -44,6 +46,7 @@
     <exec_depend>prometheus_msgs</exec_depend>
     <exec_depend>pcl_ros</exec_depend>
     <exec_depend>message_filters</exec_depend>
+    <exec_depend>tf</exec_depend>
 
     <export>
         <!-- Other tools can request additional information be placed here -->

src/merge_pcl/pcl_merge.cpp

@@ -5,6 +5,11 @@
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <exception>
+#include <tf/transform_listener.h>
+#include <pcl_ros/transforms.h>
+#include <pcl/point_types.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <geometry_msgs/TransformStamped.h>
 
 using namespace std;
 
@@ -23,71 +28,66 @@ class PointCloudMerger {
 private:
     ros::NodeHandle nh_;
     ros::Subscriber LiDAR_sub, D435i_sub;
+    tf::TransformListener listener_LiDAR, listener_D435i;
     ros::Publisher merged_pub_;
     pcl::PointCloud<pcl::PointXYZ>::Ptr LiDAR_pcl, D435i_pcl;
+    tf::StampedTransform transform_LiDAR, transform_D435i;
 };
 
-PointCloudMerger::PointCloudMerger() : nh_("~") {
+PointCloudMerger::PointCloudMerger() : nh_("~"){
     LiDAR_sub = nh_.subscribe("/prometheus/sensors/3Dlidar_scan", 1, &PointCloudMerger::LiDAR_cb, this);
     D435i_sub = nh_.subscribe("/camera/depth/color/points", 1, &PointCloudMerger::D435i_cb, this);
     merged_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/prometheus/merged_pcl", 1);
 
     LiDAR_pcl.reset(new pcl::PointCloud<pcl::PointXYZ>());
     D435i_pcl.reset(new pcl::PointCloud<pcl::PointXYZ>());
 
+    listener_LiDAR.waitForTransform("base_link", "3Dlidar_link", ros::Time(0), ros::Duration(0.05));
+    listener_D435i.waitForTransform("base_link", "D435i::camera_depth_frame", ros::Time(0), ros::Duration(0.05));
+
     // 设置cout的精度为小数点后两位
     std::cout << std::fixed << std::setprecision(2);
 
     std::cout << "[mapping] merge_pcl initialized!" << std::endl;
 }
 
 void PointCloudMerger::LiDAR_cb(const sensor_msgs::PointCloud2ConstPtr& msg) {
-    try {
-        cout << "LiDAR received!" << endl;
+    cout << "LiDAR received!" << endl;
 
             pcl::fromROSMsg(*msg, *LiDAR_pcl);
+    // 转换LiDAR点云到base_link坐标系
+    pcl_ros::transformPointCloud("base_link", *LiDAR_pcl, *LiDAR_pcl, listener_LiDAR);
+
             mergePointClouds();
-    } catch (const std::exception& e) {
-        ROS_ERROR_STREAM("Exception in LiDAR_cb: " << e.what());
-    }
 }
 
 void PointCloudMerger::D435i_cb(const sensor_msgs::PointCloud2ConstPtr& msg) {
-    try {
         cout << "D435i received!" << endl;
 
             pcl::fromROSMsg(*msg, *D435i_pcl);
+    // 转换D435i点云到base_link坐标系
+    pcl_ros::transformPointCloud("base_link", *D435i_pcl, *D435i_pcl, listener_D435i);
+    
             mergePointClouds();
-    } catch (const std::exception& e) {
-        ROS_ERROR_STREAM("Exception in D435i_cb: " << e.what());
-    }
 }
 
 void PointCloudMerger::mergePointClouds() {
-    try {
-        cout << "Merging point clouds!" << endl;
-
         pcl::PointCloud<pcl::PointXYZ>::Ptr merged_pcl;
         merged_pcl.reset(new pcl::PointCloud<pcl::PointXYZ>());
 
-//        merged_pcl->points.reserve(LiDAR_pcl->size() + D435i_pcl->size());
-        merged_pcl->header = LiDAR_pcl->header;
-
-        cout << merged_pcl->header << endl;
+        merged_pcl->header.seq = LiDAR_pcl->header.seq;
+        merged_pcl->header.stamp = LiDAR_pcl->header.stamp;
+        merged_pcl->header.frame_id = "base_link";
 
         *merged_pcl += *LiDAR_pcl;
         *merged_pcl += *D435i_pcl;
 
         sensor_msgs::PointCloud2 merged_cloud_msg;
         pcl::toROSMsg(*merged_pcl, merged_cloud_msg);
 
-        merged_cloud_msg.header.frame_id = "map";
         merged_pub_.publish(merged_cloud_msg);
 
         cout << "Merged pcl published!" << endl;
-    } catch (const std::exception& e) {
-        ROS_ERROR_STREAM("Exception in mergePointClouds: " << e.what());
-    }
 }
 
 int main(int argc, char** argv) {
@@ -96,4 +96,4 @@ int main(int argc, char** argv) {
 
     ros::spin();
     return 0;
-}
+}