Ask Your Question
0

How o save a PointCloud2 data in Python

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

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 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

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

Fetullah Atas gravatar image

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

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

Seen: 1,462 times

Last updated: Apr 12 '20