Python读取ply文件，并转为sensor_msgs.msg::PointCloud/PointCloud2并发布

前言：
ply文件是通过Carla仿真保存的激光雷达数据,为了通过ros发布消息，需要读取ply点云信息，并转为msg
其中我的ply文件的头文件信息有：

format ascii 1.0
element vertex 7348
property float32 x
property float32 y
property float32 z
property float32 I
end_header

主要是有xyz坐标和I（intensity）需要使用
转为PointCloud2时，不包含intensity，为了使用intensity信息，又转为了PointCloud。详细见下面程序：

#!/usr/bin/env python
#↑上面这一行必须有
"""
日期：2021.8.9
作者：kuangxk
功能:data process&Publish()
须知：读取ply文件并处理转为msg，PC指PointCloud
"""
import rospy
import open3d as o3d
from sensor_msgs.msg import PointCloud
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointCloud
from sensor_msgs.msg import PointField
from rospy_tutorials.msg import Floats
from plyfile import PlyData
import sensor_msgs
import geometry_msgs
import os
import sys
import signal
import numpy as np

def quit(signal, frame):#程序终端退出，按键指令为ctrl+c
	sys.exit()

def data_process_intensity(filedata):
    """
    处理intensity维度数据
    """
    plydata = PlyData.read(filedata)
    with_intensity = plydata['vertex'].data
    intensity_array = np.array(with_intensity['I'], dtype=np.float32)
    return intensity_array

def data_process(ply):
    """
    处理xyz维度数据，msg是PointCloud2类型，需要可以直接publish
    """
    pcd_arr = np.asarray(ply.points)
    points = np.zeros_like(pcd_arr)
    points[:, 0] = pcd_arr[:, 0]
    points[:, 1] = pcd_arr[:, 1]
    points[:, 2] = pcd_arr[:, 2]

    msg = PointCloud2()
    msg.header.stamp = rospy.Time().now()
    msg.header.frame_id = "plymsg"

    if len(points.shape) == 3:
        msg.height = points.shape[1]
        msg.width = points.shape[0]
    else:
        msg.height = 1
        msg.width = len(points)

    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 * points.shape[0]
    # msg.is_dense = int(np.isfinite(points).all())
    msg.is_dense = False
    msg.data = np.asarray(points, np.float32).tostring()
    # return msg
    return msg #msg为PointCloud2，需要可以发布

def talker(pointcloud1):
    tocpp_pub = rospy.Publisher("sendPC", PointCloud, queue_size=10)
    rate = rospy.Rate(100)#根据需要自己调节速率
    rospy.loginfo("PC has been sent")
    tocpp_pub.publish(pointcloud1)
    rate.sleep()

if __name__ == '__main__':
    """
    处理PointCloud数据初始化并发布
    """
    # 读取路径
    filepath = '/home/k/bag/data_linshi/'#根据自己的文件夹路径调整
    files = os.listdir(filepath)
    files.sort()
    rospy.init_node("carla_ply", anonymous=True)
    for file_ply in files:
        pointcloud1 = PointCloud()
        pointcloud1.header.stamp = rospy.Time().now()
        pointcloud1.header.frame_id = "pointcloud1"
        signal.signal(signal.SIGINT, quit)  # ctrl+c可以退出
        ply = o3d.io.read_point_cloud(filepath + file_ply)
        ply_msg = data_process(ply)#ply_msg为PointCloud2
        for p in pc2.read_points(ply_msg, field_names=("x", "y", "z"), skip_nans=True):
            val_point = geometry_msgs.msg.Point32()#相当于sensor_msg.msg::PointCloud.points
            (val_point.x, val_point.y, val_point.z) = (p[0], p[1], p[2])#p[0~3]分别为x,y,z坐标
            pointcloud1.points.append(val_point)
        intensity_array = data_process_intensity(filepath + file_ply)
        val1 = sensor_msgs.msg.ChannelFloat32()#相当于sensor_msg.msg::PointCloud.channels
        val1.name = "intensity"
        val1.values = intensity_array
        pointcloud1.channels.append(val1)
        talker(pointcloud1)

其中，PointCloud包括：

std_msgs/Header header
geometry_msgs/Point32[] points
sensor_msgs/ChannelFloat32[] channels

需要全部初始化才允许publish。
PointCloud/PointCloud2的官方文本介绍详见：
http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointCloud.html
http://docs.ros.org/en/jade/api/sensor_msgs/html/msg/PointCloud2.html

rosrun启动后，打开rviz，其中Fixed Frame改为：pointcloud1，效果如下：