Subscribe pointcloud and convert it to numpy in python
How to subscribe pointcloud and convert it into numpy using python
How to subscribe pointcloud and convert it into numpy using python
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)
Please start posting anonymously - your entry will be published after you log in or create a new account.
Asked: 2020-02-13 23:06:22 -0500
Seen: 3,719 times
Last updated: Feb 13 '20