bag包转pcd ros2 pcl_ros

### ROS2 Bag 转 PCD 的方法 在 ROS2 中，`pcl_ros` 提供了一系列工具来处理点云数据。要将 `rosbag2` 文件中的点云消息转换为 `.pcd` 格式的文件，可以按照以下方式实现。 #### 方法概述 1. **读取 rosbag 数据**：通过 `rosbags` 或者其他支持 ROS2 bag 文件的库加载存储的数据。 2. **提取点云消息**：找到包含点云主题的消息（通常是 `/point_cloud_topic` 类型的主题）。 3. **保存为 PCD 文件**：利用 `PCL (Point Cloud Library)` 将点云消息写入到 `.pcd` 文件中。 以下是具体实现： --- #### Python 实现示例 ```python import rclpy from rclpy.node import Node from sensor_msgs.msg import PointCloud2 import open3d as o3d # 使用 Open3D 处理点云 import numpy as np import os class RosBagToPCD(Node): def __init__(self, bag_file_path, output_dir): super().__init__('ros_bag_to_pcd_converter') self.output_dir = output_dir from rosbags.rosbag2 import Reader from rosbags.serde import deserialize_cdr reader = Reader(bag_file_path) connections = [ connection for connection in reader.connections if connection.topic == '/your_pointcloud_topic' and connection.msgtype == 'sensor_msgs/msg/PointCloud2' ] count = 0 with reader: for connection, timestamp, rawdata in reader.messages(connections=connections): msg = deserialize_cdr(rawdata, connection.msgtype) # Convert PointCloud2 message to array pc_data = pointcloud2_to_array(msg)[^2] points = get_xyz_points(pc_data)[^3] # Save the point cloud data into a .pcd file pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(points) filename = f"{output_dir}/pointcloud_{count}.pcd" o3d.io.write_point_cloud(filename, pcd) self.get_logger().info(f'Saved {filename}') count += 1 def pointcloud2_to_array(cloud_msg): """Convert a sensor_msgs/PointCloud2 message to a NumPy array.""" import pandas as pd df = pd.DataFrame( list(point_cloud2.read_points(cloud_msg, field_names=None, skip_nans=False)) ) return df.to_numpy()[^4] def get_xyz_points(cloud_arr): """Extract XYZ coordinates from the structured array.""" xyz_points = cloud_arr[:, :3] # Assuming first three columns are X,Y,Z return xyz_points.astype(np.float64)[^5] if __name__ == '__main__': rclpy.init(args=None) converter_node = RosBagToPCD('/path/to/bagfile', './output_pcd_files/') try: pass # No need for spin since it's not subscribing but processing offline. finally: converter_node.destroy_node() rclpy.shutdown() ``` --- #### 关键说明 - 上述脚本依赖于 `rosbags` 库用于解析 ROS2 bag 文件[^1]。 - 需安装必要的依赖项： ```bash pip install rosbags open3d pandas ``` - 函数 `pointcloud2_to_array()` 和 `get_xyz_points()` 是为了方便从 `PointCloud2` 消息中提取原始坐标数组并将其转化为适合保存的形式[^2]。 --- #### 注意事项 - 替换变量 `/your_pointcloud_topic` 为目标点云话题名称。 - 如果需要额外字段（如颜色），可以在 `pointcloud2_to_array` 函数中扩展逻辑以保留更多维度的信息[^3]。 ---