Subscribe pointcloud and convert it to numpy in python
How to subscribe pointcloud and convert it into numpy using python
ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
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)
Asked: 2020-02-13 23:06:22 -0600
Seen: 8,795 times
Last updated: Feb 13 '20
trying to publish an array of words in rospy using numpy
ROS and OpenCV: sending single images, image type, and receiving laptop camera stream
read bag messages as numpy in python [closed]
why do we need roslib.load_manifest?
how to publish a list of numpy arrays
Is there a release date of ros 2 or more informations about it?
Is the planned ROS1 to ROS2/DDS bridge available yet?