【ros】numpy 格式点云转 sensor_msgs/PointCloud2 格式发布

1. 问题描述

需要从 mat 文件中读取点云数据 (numpy格式) 转换成 ROS 中的 sensor_msgs/PointCloud2 格式发布。

2. 代码

#!/usr/bin/env python
# -*- coding: UTF-8 -*-
# license removed for brevity
import rospy
from std_msgs.msg import String
import scipy.io as scio
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField
import numpy as np
import time
from std_msgs.msg import Header
from sensor_msgs import point_cloud2

import numpy as np
 
from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2
 
def talker():

    pub = rospy.Publisher('points_raw', PointCloud2, queue_size=10)
    rospy.init_node('node', anonymous=True)
    rate = rospy.Rate(10) # 10hz


    import os
    path = '/home/~/roboware_ws/mat_result_12s'
    files= os.listdir(path) #得到文件夹下的所有文件名称

    files.sort(key = lambda x: int(x[:-4]))
    count = 0

    while count < len(files):
    # while not rospy.is_shutdown():

        # dataFile = '/home/hzh/roboware_ws/mat_result_12s/0.mat'

        # data_mat = scio.loadmat(dataFile)

        # cloud_arr = data_mat['pts_raw']

        data_mat = scio.loadmat(path + '/' + str(files[count]))
        
        cloud_arr = data_mat['pts_raw']

        cloud_arr = np.asarray(cloud_arr, np.float32)

        points = np.zeros_like(cloud_arr)
        points[:, 0] = cloud_arr[:, 2]
        points[:, 1] = -cloud_arr[:, 0]
        points[:, 2] = -cloud_arr[:, 1]

        msg = PointCloud2()

        msg.header.stamp = rospy.Time().now()

        msg.header.frame_id = "velodyne"

        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()

        pub.publish(msg)
        print('send!)
        print('count:',count)
        
        count += 1
        
        rate.sleep()
        
if __name__ == '__main__':     

    talker()

3. 效果