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

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

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

Ajay Jain gravatar image

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

2 Answers

Sort by ยป oldest newest most voted
5

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

Ajay Jain gravatar image

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 more

Comments

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

michaeljaison gravatar image 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. gravatar image 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 gravatar image lr101095  ( 2018-05-09 00:40:48 -0500 )edit
5

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

wallE gravatar image

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

lucasw gravatar image

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 more

Comments

2

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 gravatar image 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 gravatar image 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 gravatar image 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 gravatar image 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 gravatar image 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 gravatar image abbas  ( 2022-12-21 16:22:19 -0500 )edit

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

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

Question Tools

3 followers

Stats

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

Seen: 41,955 times

Last updated: May 30 '22