Using PointCloud2 data (getting x,y points) in Python

asked 2015-02-09 19:56:12 -0500

Ajay Jain
165 ●6 ●8 ●12

In a Python ROS node, I've subscribed to a sensor_msgs/LaserScan topic, converted them to sensor_msgs/PointCloud2 messages, and am trying to extract X, Y (and Z) point coordinates. The transformation from LaserScan to PointCloud2 uses the LaserProjection class of laser_geometry. What is the data format of sensor_msgs/LaserScan.data? From the documentation and other questions, it's a binary format usually converted in C++ with the pcl_ros or pcl_conversions packages (eg http://answers.ros.org/question/10947... ), but I'm working in Python.

This is my code:

import rospy, math, random
import numpy as np
from sensor_msgs.msg import LaserScan
from laser_geometry import LaserProjection

...

class Lidar():
def __init__(self, scan_topic="/robot_0/base_scan"):
    self.scan_sub = rospy.Subscriber(scan_topic, LaserScan, self.on_scan)
    self.laser_projector = LaserProjection()

def on_scan(self, scan):
    print scan
    rospy.loginfo("Got scan, projecting")
    cloud = self.laser_projector.projectLaser(scan)
    print cloud
    rospy.loginfo("Printed cloud")

Thank you in advance!

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2015-02-11 17:50:16 -0500

Ajay Jain
165 ●6 ●8 ●12

Here is my solution:

import sensor_msgs.point_cloud2 as pc2

...

def on_scan(self, scan):
    rospy.loginfo("Got scan, projecting")
    cloud = self.laser_projector.projectLaser(scan)
    gen = pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z"))
    self.xyz_generator = gen

The source/documentation for point_cloud2.py is http://docs.ros.org/indigo/api/sensor... , which describes the read_points method to parse a PointCloud2. From that method source, the format of PointCloud2.data seems to be a series of fields (x, y, z, intensity, index) packed with the struct library. The deserialization in that file is done by:

fmt = _get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names)
width, height, point_step, row_step, data, isnan = cloud.width, cloud.height, cloud.point_step, cloud.row_step, cloud.data, math.isnan
unpack_from = struct.Struct(fmt).unpack_from

edit flag offensive delete link

Comments

How can you build an organized pointcloud in python-pcl?

michaeljaison ( 2016-06-20 15:20:37 -0500 )edit

sincerely the deserialization you wrote doesn't make any sense to me. Can you elaborate? And where does _get_struct_fmt come from?

Mehdi. ( 2016-07-27 09:06:35 -0500 )edit

@Mehdi. Ajay is referring to this (see line 76): http://docs.ros.org/api/sensor_msgs/h...

lr101095 ( 2018-05-09 00:40:48 -0500 )edit

add a comment

answered 2016-04-13 11:13:44 -0500

wallE
51 ●1 ●2 ●2

updated 2022-05-30 10:40:12 -0500

lucasw

8729 ●137 ●239 ●256 https://github.com/lucasw

The above mentioned method works fine, but I implemented following as it was easy to access points that way

import sensor_msgs.point_cloud2 as pc2

...

for p in pc2.read_points(point_cloud, field_names = ("x", "y", "z"), skip_nans=True):
     print " x : %f  y: %f  z: %f" %(p[0],p[1],p[2])

edit flag offensive delete link

Comments

Hi! Very practical implementation! While occasionally, an error is reported. Any advices?

    *assert isinstance(cloud, roslib.message.Message) and cloud._type == 'sensor_msgs/PointCloud2', 'cloud is not a sensor_msgs.msg.PointCloud2'
AssertionError: cloud is not a sensor_msgs.msg.PointCloud2*

taiping.z ( 2018-10-04 02:42:31 -0500 )edit

Hey, thanks your soln worked. Now I wanna know how to get corresponding pixel values?

Atharva Mete ( 2020-08-08 15:07:09 -0500 )edit

@lucasw hi, the x,y,z are indicative of what? distance? if i want to extract the distance from an object to the lidar, what should i do? is there any relation between the code that you provided and my question?

Delbina ( 2022-08-04 10:44:36 -0500 )edit

If you know what points are on the object you care about, then the xyz values are very likely distance in meters to the origin of the lidar in it's frame- if the lidar we're tilted you would want to account for that

lucasw ( 2022-08-04 21:08:55 -0500 )edit

thanks @lucasw, now the problem is that i donot know how to filter out the environment points, and just keep points related to the object! do you have any idea about this part? how can i access to the points of the object?

and one more question about the direction of x,y,z. Is x forward, y on the right side, and z upward?

Delbina ( 2022-08-05 03:03:59 -0500 )edit

Hi @Delbina , were you able to extract only a segment of points from lidar pointcloud2 data. I am also trying to do same but cant find any suitable python package to do it.

abbas ( 2022-12-21 16:22:19 -0500 )edit

@abbas I used voxel grid filtering (http://wiki.ros.org/pcl_ros/Tutorials...)

Delbina ( 2022-12-22 08:19:28 -0500 )edit

add a comment

Using PointCloud2 data (getting x,y points) in Python

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Using PointCloud2 data (getting x,y points) in Python edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Using PointCloud2 data (getting x,y points) in Python