From 137418b8ac3208af24cfbaca0fe8aecb86c3fe7c Mon Sep 17 00:00:00 2001
From: EasonHua <1628280289@qq.com>
Date: Fri, 23 Aug 2024 11:28:10 +0800
Subject: [PATCH] =?UTF-8?q?[v2.1.3=20new=20feature]=20=E8=BD=AC=E5=8F=91li?=
=?UTF-8?q?dar=5Fframe=E5=9D=90=E6=A0=87=E7=B3=BB=E4=B8=8B=E7=9A=84?=
=?UTF-8?q?=E7=82=B9=E4=BA=91=E5=88=B0world=E5=9D=90=E6=A0=87=E7=B3=BB?=
=?UTF-8?q?=E4=B8=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
CHANGELOG.md | 3 +++
CMakeLists.txt | 13 +++--------
README.md | 11 ++++++++-
launch/simulation.launch | 2 ++
package.xml | 3 +--
scripts/pub_pcl_world.py | 50 ++++++++++++++++++++++++++++++++++++++++
6 files changed, 69 insertions(+), 13 deletions(-)
create mode 100644 scripts/pub_pcl_world.py
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