diff --git a/CHANGELOG.md b/CHANGELOG.md
index ea5e93a..b175c0f 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.3: 2024-08-23
+- [new feature] 转发lidar_frame坐标系下的点云到world坐标系下
+
## v2.1.2:
- import `message_filters::sync_policies::ApproximateTime` for merge
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f10ced2..b6ebd07 100644
--- a/CMakeLists.txt
+++ b/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}
-)
diff --git a/README.md b/README.md
index 71206b9..d253b7e 100644
--- a/README.md
+++ b/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
diff --git a/launch/simulation.launch b/launch/simulation.launch
index fefa2d9..6f6cec9 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -9,6 +9,8 @@
+
+
diff --git a/package.xml b/package.xml
index 146360f..f3297a1 100644
--- a/package.xml
+++ b/package.xml
@@ -1,13 +1,12 @@
easondrone_mapping
- 2.1.2
+ 2.1.3
A ROS package for mapping via octomap
Eason Hua
- Eason Yi
Apache
diff --git a/scripts/pub_pcl_world.py b/scripts/pub_pcl_world.py
new file mode 100644
index 0000000..efd8fd9
--- /dev/null
+++ b/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