将 Pointcloud 从 .csv 读取到 ROS PointCloud2

Question

我有一个包含 /raw_points rostopic 的 .csv 文件，我正在尝试将该文件转换为 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)

这是我的 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

将 Pointcloud 从 .csv 读取到 ROS PointCloud2

Reading Pointcloud from .csv to ROS PointCloud2

point-clouds

ros