python rclpy读取雷达话题并保存为pcd

### 使用 Python 和 rclpy 实现 LiDAR 点云数据保存为 PCD 文件 为了实现从 ROS2 中订阅 LiDAR 点云话题并将其转换成 PCD 文件格式，可以创建一个基于 `rclpy` 的节点来处理这些操作。此过程涉及几个主要部分：初始化 ROS 节点、设置订阅者以监听特定的话题、接收 PointCloud2 消息以及将消息中的数据写入 PCD 文件。 #### 创建 ROS2 节点和订阅者 首先，在 Python 环境中安装必要的库： ```bash pip install numpy opencv-python pypcd rosbags ``` 接着编写如下所示的 Python 代码片段用于定义一个新的 ROS2 节点类 `PointCloudSubscriber`: ```python import rclpy from rclpy.node import Node from sensor_msgs.msg import PointCloud2 import numpy as np import os import struct import open3d as o3d class PointCloudSubscriber(Node): def __init__(self): super().__init__('point_cloud_subscriber') self.subscription = self.create_subscription( PointCloud2, '/rslidar_points_32', self.listener_callback, 10) self.save_path = 'lidar_points_80_lines' if not os.path.exists(self.save_path): os.makedirs(self.save_path) def listener_callback(self, msg): pc_data = read_pointcloud_from_msg(msg) timestamp_str = str(int(rclpy.clock.Clock().now().nanoseconds / 1e9)) file_name = f"{timestamp_str}.pcd" full_file_path = os.path.join(self.save_path, file_name) save_as_pcd(pc_data, full_file_path) def read_pointcloud_from_msg(cloud_msg): point_step = cloud_msg.point_step row_step = cloud_msg.row_step data = bytes(cloud_msg.data) points_list = [] offset = 0 while offset < len(data): xyzrgb = struct.unpack('fffff', data[offset:offset + point_step]) x, y, z, intensity, ring = xyzrgb[:5] points_list.append([x, y, z]) offset += point_step return np.array(points_list).reshape(-1, 3) def save_as_pcd(point_cloud_array, filename): pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(point_cloud_array) o3d.io.write_point_cloud(filename, pcd) def main(args=None): rclpy.init(args=args) subscriber_node = PointCloudSubscriber() try: rclpy.spin(subscriber_node) except KeyboardInterrupt: pass subscriber_node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main() ``` 上述脚本实现了以下功能： - 定义了一个名为 `PointCloudSubscriber` 的 ROS2 节点； - 设置了对该节点的一个订阅器实例，它会持续监听 `/rslidar_points_32` 这一主题上的新消息[^1]； - 当接收到新的 `sensor_msgs/msg/PointCloud2` 类型的消息时调用回调函数 `listener_callback()` 来处理该消息； - 将解码后的三维坐标存储在一个 NumPy 数组里，并通过 Open3D 库的方法将其导出为 `.pcd` 文件[^2]；