ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

How o save a PointCloud2 data in Python

asked 2017-02-22 18:35:28 -0600

Joy16 gravatar image

Hi! I am subscribing to a data of Type: sensor_msgs/PointCloud2. How do I save he point cloud data in a python file. How do I convert it to ROS daa. Please share links of tutorial if any. I am not able to find one.

edit retag flag offensive close merge delete

Comments

Support for PCL datatypes in Python is rather thin. If you search around, you'll find a few people who have written some conversion methods themselves (ie: PCL -> numpy) but it's all unofficial. PCL currently works best from C++ I'm afraid.

gvdhoorn gravatar image gvdhoorn  ( 2017-02-23 00:45:20 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
5

answered 2020-04-12 20:01:24 -0600

Have you heard of open3d ?, that is a Python libabry which can help you save cloud with popular formats(e.g .ply). lets say you have pointcloud2 topic and you want to read and save the Pointcloud

from sensor_msgs.msg import PointCloud2, PointField
import numpy as np
import open3d as o3d
import sensor_msgs.point_cloud2 as pc2
import ctypes
import struct

def callback(self, ros_point_cloud):
        xyz = np.array([[0,0,0]])
        rgb = np.array([[0,0,0]])
        #self.lock.acquire()
        gen = pc2.read_points(ros_point_cloud, skip_nans=True)
        int_data = list(gen)

        for x in int_data:
            test = x[3] 
            # cast float32 to int so that bitwise operations are possible
            s = struct.pack('>f' ,test)
            i = struct.unpack('>l',s)[0]
            # you can get back the float value by the inverse operations
            pack = ctypes.c_uint32(i).value
            r = (pack & 0x00FF0000)>> 16
            g = (pack & 0x0000FF00)>> 8
            b = (pack & 0x000000FF)
            # prints r,g,b values in the 0-255 range
                        # x,y,z can be retrieved from the x[0],x[1],x[2]
            xyz = np.append(xyz,[[x[0],x[1],x[2]]], axis = 0)
            rgb = np.append(rgb,[[r,g,b]], axis = 0)

        out_pcd = o3d.geometry.PointCloud()    
        out_pcd.points = o3d.utility.Vector3dVector(xyz)
        out_pcd.colors = o3d.utility.Vector3dVector(rgb)
        o3d.io.write_point_cloud("/home/atas/cloud.ply",out_pcd)

I have used this code before and it works but you may adjust some parts of the code for your needs

edit flag offensive delete link more

Comments

This is good solution. However, when I tried running it, it took too long to save only one pcl/pcd file.

Considering that the average point number in a PointCloud message is usually too big, and we already know it from the length of int_data array, we can eliminate the np.append() operations by pre-allocating the space for numpy arrays as below:

xyz = np.empty((len(int_data), 3))
rgb = np.empty((len(int_data), 3))

Then we can assign each point in the loop with an index. E.g. xyz[idx] = x[:3]

This way the function completes way faster.

harun-loodos gravatar image harun-loodos  ( 2022-07-07 10:09:46 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2017-02-22 18:35:28 -0600

Seen: 5,799 times

Last updated: Apr 12 '20