正在将Pointcloud从.csv读取到ROS PointCloud2

Question

我有一个.csv文件，其中包含/raw_points rostopic，我正在尝试将该文件转换为PointCloud2数据(http://docs.ros.org/en/api/sensor_msgs/html/msg/PointCloud2.html)。

import csv
import sys
csv.field_size_limit(sys.maxsize)
file = open("points_raw.csv")
csvreader = csv.reader(file)
header = next(csvreader)
print(header)

这是我的标题：

['Time', 'header.seq', 'header.stamp.secs', 'header.stamp.nsecs', 'header.frame_id', 'height', 'width', 'fields', 'is_bigendian', 'point_step', 'row_step', 'data', 'is_dense']

这些信息与CloudPoint2相匹配，但我不确定如何将其转换为这种类型。

Answer 1

您需要简单地对每一行进行迭代，并将每一行的相关字段存储在PointCloud2消息中并发布。例如：

import rospy
import csv
from sensor_msgs.msg import PointCloud2
def main():
#Setup ros param/init here
some_pub = rospy.Publisher('output_topic', PointCloud2, queue_size=10)
with open('some_file.csv', 'r') as f:
reader = csv.reader(f)
for line in reader:
split_line = line.split(',')
new_msg = PointCloud2()
new_msg.header.seq = split_line[1]
new_msg.header.stamp.secs = split_line[2]
#Iterate over the rest
new_msg.data = split_line[11]
new_msg.is_dense = split_line[12]
some_pub.publish(new_msg)
rospy.Rate(10).sleep() #Sleep at 10Hz