[v2.1.3 new feature] 转发lidar_frame坐标系下的点云到world坐标系下

Author: HuaYuXiao

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.3: 2024-08-23
+- [new feature] 转发lidar_frame坐标系下的点云到world坐标系下
+
 ## v2.1.2: 
 - import `message_filters::sync_policies::ApproximateTime` for merge
 

CMakeLists.txt

@@ -58,15 +58,8 @@ target_link_libraries(pcl_merge
 		${PCL_LIBRARIES}
 )
 
-catkin_install_python(PROGRAMS scripts/laser_to_pointcloud.py
+catkin_install_python(PROGRAMS
+		scripts/laser_to_pointcloud.py
+		scripts/pub_pcl_world.py
 		DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-install(DIRECTORY launch
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY config
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)

README.md

@@ -12,9 +12,18 @@ A ROS package for mapping via octomap.
 
 ```bash
 cd ~/EasonDrone
-catkin_make install --source Reconstruction/EasonDrone_Mapping --build Reconstruction/EasonDrone_Mapping/build
+catkin_make --source Reconstruction/EasonDrone_Mapping --build Reconstruction/EasonDrone_Mapping/build
 ```
 
+## 转发点云
+
+EGO-Planner等规划器要求点云发布在world坐标系下，因此需要将原本发布在lidar_frame的点云转发到world下
+
+```bash
+rosrun easondrone_mapping pub_pcl_world.py
+```
+
+
 ## 建立地图
 
 ```bash

launch/simulation.launch

@@ -9,6 +9,8 @@
 
     <include file="$(find px4ctrl)/launch/px4ctrl.launch"/>
 
+    <node name="pub_pcl_world" pkg="easondrone_mapping" type="pub_pcl_world"/>
+
     <!-- 启动octomap建图 -->
     <include file="$(find easondrone_mapping)/launch/octomap_server.launch"/>
 

package.xml

@@ -1,13 +1,12 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>easondrone_mapping</name>
-    <version>2.1.2</version>
+    <version>2.1.3</version>
     <description>
         A ROS package for mapping via octomap
     </description>
 
     <maintainer email="[email protected]">Eason Hua</maintainer>
-    <author email="[email protected]">Eason Yi</author>
 
     <license>Apache</license>
 

scripts/pub_pcl_world.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import rospy
+import tf2_ros
+import tf2_sensor_msgs
+from sensor_msgs.msg import PointCloud2
+
+class LidarDataTransformer:
+    def __init__(self):
+        # Initialize the ROS node
+        rospy.init_node('pub_pcl_world', anonymous=True)
+
+        # Set up the transform listener
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
+
+        # Subscribe to the PointCloud2 topic
+        self.pc_subscriber = rospy.Subscriber('/velodyne_points', PointCloud2, self.point_cloud_callback)
+
+        # Publisher for transformed point cloud data
+        self.pc_publisher = rospy.Publisher('/velodyne_points_world', PointCloud2, queue_size=10)
+
+    def point_cloud_callback(self, msg):
+        try:
+            # Look up the transform from 'world' to 'lidar_link'
+            transform = self.tf_buffer.lookup_transform('world', msg.header.frame_id, rospy.Time(0), rospy.Duration(1.0))
+
+            # Transform the point cloud to the 'world' frame
+            transformed_cloud = tf2_sensor_msgs.do_transform_cloud(msg, transform)
+
+            # Publish the transformed point cloud
+            self.pc_publisher.publish(transformed_cloud)
+
+        except tf2_ros.LookupException as e:
+            rospy.logerr(f"Transform lookup failed: {e}")
+        except tf2_ros.ExtrapolationException as e:
+            rospy.logerr(f"Transform extrapolation failed: {e}")
+        except Exception as e:
+            rospy.logerr(f"Error transforming point cloud: {e}")
+
+    def run(self):
+        rospy.spin()
+
+if __name__ == '__main__':
+    try:
+        transformer = LidarDataTransformer()
+        transformer.run()
+    except rospy.ROSInterruptException:
+        pass