Ask Your Question
3

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
2

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 imagemichaeljaison ( 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 imageMehdi. ( 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 imagelr101095 ( 2018-05-09 00:40:48 -0500 )edit
4

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

wallE gravatar image

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

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

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 imagetaiping.z ( 2018-10-04 02:42:31 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

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

Seen: 20,619 times

Last updated: Feb 11 '15