Reading Pointcloud from .csv to ROS PointCloud2

user_10895438 0
Viewed 265

I have a .csv file which has /raw_points rostopic, and i'm trying to convert that file into PointCloud2 data(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)

This is my 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']

These information match the CloudPoint2, but I'm not sure how to convert it to this type.

point-clouds ros
1 Answers

You need to simply iterate over each row and for every row store the relative fields in a PointCloud2 message and publish it out. For example:

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
user_119362290
Related