ROS_发布点云pcd到rviz

发布标注好的点云

发布标注好的点云

读取点云文件-发布

 std_msgs/Header header：数据头，包含该帧点云的时间戳、坐标系等属性信息
 uint32 			height：data的高度，一帧点云通常height=1，即表示无序点云；
 uint32 			width：data的宽度，即每行对应点的数量；
 sensor_msgs/PointField[] fields：包含每个点的字段属性信息，详见下文。
 bool 			is_bigendian：点云是否按正序排列
 uint32 			point_step：每个点占用的比特数，1字节=8比特，与PointField里所有字节数之和相等。
 uint32 			row_step：每行占用的比特数，=点的数量*Point_step;
 uint8[] 		data：序列化后的点云二进制数据，所有点信息都在一串字符中，无法通过data[i]取值。
 bool 			is_dense：是否存在无效点。

示例代码

#! /usr/bin/env python3
# -*- coding=utf-8 -*-

import rospy
from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2
import open3d as o3d
import numpy as np
import os
 
def talker():
    pub = rospy.Publisher('points_b', PointCloud2, queue_size=10)
    rospy.init_node('node', anonymous=True)
    rate = rospy.Rate(2) # 2hz
    ##处理PCD点云
    pcd_path = '~/pcd'
	#得到文件夹下的所有文件名称
    files= os.listdir(pcd_path) 
    #files.sort(key = lambda x: int(x[:-4]))
    files.sort()
    for i,pcd_file in enumerate(files):
        print(i,pcd_file)
        pcd = o3d.t.io.read_point_cloud(os.path.join(pcd_path,pcd_file))
        exp_points = pcd.point
        point_xyz = exp_points['positions'].numpy()
        # 构造消息
        msg = PointCloud2()
        msg.header.stamp = rospy.Time().now()
        msg.header.frame_id = "map"
        msg.fields = [
            PointField('x', 0, PointField.FLOAT32, 1),
            PointField('y', 4, PointField.FLOAT32, 1),
            PointField('z', 8, PointField.FLOAT32, 1)]
        msg.is_bigendian = False
        msg.point_step = 12
        msg.row_step = msg.point_step * point_xyz.shape[0]
        msg.is_dense = False
        if len(point_xyz.shape) == 3:
            msg.height = point_xyz.shape[1]
            msg.width = point_xyz.shape[0]
        else:
            msg.height = 1
            msg.width = len(point_xyz)
        msg.data = np.asarray(point_xyz, np.float32).tostring()
        pub.publish(msg)       
        rate.sleep()
        
if __name__ == '__main__':     
    for i in range(20):
        talker()

参考

 【ros】numpy 格式点云转 sensor_msgs/PointCloud2 格式发布 https://blog.csdn.net/qq_35632833/article/details/108050233
 动手学ROS（13）：点云(PointCloud2)的发送与接收--python和c++示例 https://zhuanlan.zhihu.com/p/421691350
 ROS(机器人操作系统) http://www.autolabor.com.cn/book/ROSTutorials/
  【Open3D】点云可视化 https://zhuanlan.zhihu.com/p/483414760
  https://github.com/isl-org/Open3D/issues/1110