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