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

Subscribe pointcloud and convert it to numpy in python

asked 2020-02-13 23:06:22 -0600

chowkamlee81 gravatar image

How to subscribe pointcloud and convert it into numpy using python

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2020-02-13 23:32:49 -0600

updated 2020-02-13 23:34:38 -0600

Itis quite straightforward as below;

from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2
import ctypes
import struct
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header

      def __init__(self):
            '''initiliaze  ros stuff '''

            self.cloud_sub = rospy.Subscriber("/your_cloud_topic", PointCloud2,self.callback,queue_size=1, buff_size=52428800)      

        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)
edit flag offensive delete link more

Question Tools

3 followers

Stats

Asked: 2020-02-13 23:06:22 -0600

Seen: 8,795 times

Last updated: Feb 13 '20