Python读取.pcd文件生成sensor_msgs::PointCloud2写入rosbag问题咨询

问题原因 #

你当前的代码仅为PointCloud2消息赋值了data与header字段，缺失了点云解析必需的核心元数据字段（包括height/width尺寸、fields字段定义、point_step/row_step步长等），导致ROS无法识别数据格式，因此RViz和rostopic echo都无法正常读取点云。

等效pcl::toROSMsg的Python实现 #

ROS官方sensor_msgs模块自带了点云消息构造接口，无需依赖第三方库的封装，等效于C++的pcl::toROSMsg功能：

• 纯XYZ点云：使用sensor_msgs.point_cloud2.create_cloud_xyz
• 带RGB的点云：使用sensor_msgs.point_cloud2.create_cloud_xyzrgb
• 自定义字段点云：使用通用接口sensor_msgs.point_cloud2.create_cloud自定义字段规则

代码修改方案 #

第一步：添加依赖导入 #

在你的导入部分添加PointField引入：

from sensor_msgs.msg import PointField

第二步：替换点云消息构造逻辑 #

将你原来构造pcl_msg的代码段：

# get data
pcl_msg = sensor_msgs.msg.PointCloud2()
pcl_msg.data = np.ndarray.tobytes(pcl_data.to_array())
pcl_msg.header.stamp = rospy.Time(t_us/1000000.0)# t_s, t_ns)
pcl_msg.header.frame_id = "reu_1/navcam_sensor"

替换为如下代码（以纯XYZ点云为例）：

# 将PCL点云转换为Nx3的numpy数组（仅取XYZ三个维度，如有强度、RGB可对应扩展）
cloud_arr = pcl_data.to_array()[:, :3].astype(np.float32)
# 构造消息头
header = rospy.Header()
header.stamp = rospy.Time(t_us/1000000.0)
header.frame_id = "reu_1/navcam_sensor"
# 直接生成合规的PointCloud2消息
pcl_msg = pc2.create_cloud_xyz(header, cloud_arr)

如果你的PCD文件包含强度等额外字段，可使用通用构造接口自定义字段：

# 构造点云字段定义，偏移量对应每个字段的字节起始位置
fields = [
    PointField('x', 0, PointField.FLOAT32, 1),
    PointField('y', 4, PointField.FLOAT32, 1),
    PointField('z', 8, PointField.FLOAT32, 1),
    PointField('intensity', 12, PointField.FLOAT32, 1),
]
# 数组维度对应字段顺序即可
cloud_arr = pcl_data.to_array()[:, :4].astype(np.float32)
pcl_msg = pc2.create_cloud(header, fields, cloud_arr)

修改后生成的rosbag即可直接用RViz和rostopic echo正常读取点云数据。

备选方案：使用pypcd直接转消息 #

如果你的项目已经依赖pypcd，可以直接调用自带的to_msg()接口，无需手动构造：

from pypcd import pypcd
# 读取PCD文件
pc = pypcd.PointCloud.from_path(os.path.join(metadata_dir, pcd_path))
# 直接转PointCloud2消息
pcl_msg = pc.to_msg()
# 手动覆盖header信息
pcl_msg.header.stamp = rospy.Time(t_us/1000000.0)
pcl_msg.header.frame_id = "reu_1/navcam_sensor"

内容的提问来源于stack exchange，提问作者rebubi