Extracting the (x,y,z) coordinates from pointcloud2 data in Python

Hello,

I'm in the process of using a stereo camera that generates a pointcloud2 sensor message. I can open it up in rviz, and view the pointcloud. now I want to try and use the uint8[] data for a system I'm working on. I have a Python subscription node that can subscribe to the proper topic as well as print the data inside the script. I just need to know how to get from this huge data string to a useable (x, y, z) format or numpy array in camera space for me to do something useful for it.

I did stumble across python-pcl by strawlab, but I'm not sure if that's what I need to use, or what?

Looking forward to some help!

Originally posted by atomoclast on ROS Answers with karma: 151 on 2016-03-31

Post score: 5

import sensor_msgs.point_cloud2
...
for point in sensor_msgs.point_cloud2.read_points(msg, skip_nans=True):
pt_x = point[0]
pt_y = point[1]
pt_z = point[2]


Originally posted by lucasw with karma: 8729 on 2016-04-02

This answer was NOT ACCEPTED on the original site

Post score: 5

Comment by Meric on 2019-03-22:
I belive x y and z cordinates are not same with the frame of the Camera, I am using this lines of code as below:

    number_of_sampling = 30
x_total = 0
y_total = 0
z_total = 0
counter = 0

for point in sensor_msgs.point_cloud2.read_points(data, skip_nans=True):

y_total += point[0]
z_total += point[1]
x_total += point[2]
##        print x_total, y_total, z_total
counter += 1
if counter == number_of_sampling: break

x_average = x_total / number_of_sampling
y_average = -1*y_total / number_of_sampling
z_average = -1*z_total / number_of_sampling


In other words 0 index is Y 1st index is Z and 2nd index is X

I know I'm a little late here, but a function is included in ros_numpy package for this exact purpose. http://docs.ros.org/kinetic/api/ros_numpy/html/point__cloud2_8py_source.html

An example implementation:

import ros_numpy
...
xyz_array = ros_numpy.point_cloud2.get_xyz_points(pointcloud2_array)


Originally posted by saltus with karma: 131 on 2018-08-28

This answer was NOT ACCEPTED on the original site

Post score: 6