Ask Your Question
5

Datatype to access pointcloud2

asked 2011-10-14 11:31:56 -0600

youga gravatar image

Hello all, i am working on pobot object detection project and m totally new at Ros. so far in project we have published pointclouds but can anyone tell me after subscribing to a pointcloud2 message through ros, how to access its data field ( WHERE THE ACTUAL DATA IS STORED) ROS_INFO("I heard Height[%d] Width[%d] point_step[%d] row_step[%d] data[%d] ", msg->height, msg->width, msg->point_step, msg->row_step);

through this i can access height,width but what is the datatype for data field??

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
7

answered 2011-10-14 19:24:37 -0600

ahendrix gravatar image

From the documentation on the sensor_msgs::PointCloud2 type, the 'data' member contains the actual data of the point cloud message, and the type is determined dynamically by reading the 'fields' member.

I suggest you determine what sort of data you're hoping to extract from your input point clouds, and use pcl::fromROSMsg() to convert your ros message into a pcl point cloud, at which point you can use the supplied iterators to iterate over your point cloud.

edit flag offensive delete link more
12

answered 2015-08-13 18:05:06 -0600

peci1 gravatar image

updated 2017-02-08 02:37:18 -0600

Now it seems you can do very well without the PCL library in both Python and C++.

For Python, there is the sensor_msgs.point_cloud2 package with the read_points method.

For C++, you can use sensor_msgs::PointCloud2Iterator to iterate the fields (like "x" for the x-coordinate, or "r" for the red color). And for creating/basic manipulation with PointCloud2 clouds, there is the sensor_msgs::PointCloud2Modifier class, which allows you to conveniently set the fields structure, resize or clear a point cloud.

Example usage of the C++ interface (taken and simplified from ros-perception/image_pipeline):

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

using namespace sensor_msgs;

PointCloud2Ptr points_msg = boost::make_shared<PointCloud2>();
points_msg->header = ...; // TODO fill in header
points_msg->height = mat.rows; // if this is a "full 3D" pointcloud, height is 1; if this is Kinect-like pointcloud, height is the vertical resolution
points_msg->width  = mat.cols;
points_msg->is_bigendian = false;
points_msg->is_dense = false; // there may be invalid points

sensor_msgs::PointCloud2Modifier pcd_modifier(*points_msg);
// this call also resizes the data structure according to the given width, height and fields
pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");

sensor_msgs::PointCloud2Iterator<float> iter_x(*points_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*points_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*points_msg, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*points_msg, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*points_msg, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*points_msg, "b");

for (...) // TODO get/generate point coordinates and colors
{
  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b)
  {
    // TODO fill in x, y, z, r, g, b local variables
    *iter_x = x;
    *iter_y = y;
    *iter_z = z;
    *iter_r = r;
    *iter_g = g;
    *iter_b = b;
  }
}

You can also apply a transform directly to PointCloud2 type using the tf2_sensor_msgs (both C++ and Python interface, since at least Indigo):

#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>

const sensor_msgs::PointCloud2 cloud_in, cloud_out;
const geometry_msgs::TransformStamped transform;
// TODO load everything
tf2::doTransform (cloud_in, cloud_out, transform);

Or in Python:

from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
cloud_out = do_transform_cloud(cloud_in, transform)

None of the codes was really tested, though I intend to use them tomorrow. If you test them and find a bug, please edit my answer. I really wonder why there aren't more tutorials for this kind of essential conversions.

edit flag offensive delete link more

Comments

1

There is a short tutorial here for filling in a PointCloud2 message: http://docs.ros.org/jade/api/sensor_m...

atp gravatar imageatp ( 2015-10-06 15:13:10 -0600 )edit

Hi peci1 thanks for your help, but what do you mean with "load everything" in the tf code?

brennocal gravatar imagebrennocal ( 2016-08-01 14:23:04 -0600 )edit

@brennocal: You need to somehow initialize the cloud_in and transform variables. It's application-specific code you have to write.

peci1 gravatar imagepeci1 ( 2016-08-01 17:18:47 -0600 )edit

I'm having this error:

/home/tarik/ros_ws/src/object_seg/src/cloud_transformer.cpp:6:29: fatal error: tf2_sensor_msgs.h: No such file or directory

#include <tf2_sensor_msgs.h>

I've tried adding tf2_msgs to my package.xml and CMake file.

skywalker gravatar imageskywalker ( 2017-02-06 21:09:13 -0600 )edit

Add tf2_sensor_msgs, and not tf2_msgs. Also don't forget the include_directories(${catkin_INCLUDE_DIRS}) in your CMakeLists.txt.

peci1 gravatar imagepeci1 ( 2017-02-07 02:53:47 -0600 )edit

Thanks for your reply @peci1, it only worked when I add #include <tf2_sensor_msgs/tf2_sensor_msgs.h> to my code. I have another question tough. Is it normal that doTransform() takes so long? It takes around 0.44 seconds and I have pretty decent computer. Is pcl_ros faster?

skywalker gravatar imageskywalker ( 2017-02-07 14:17:48 -0600 )edit

@skywalker: You're right, I mistyped the tf2_sensor_msgs include. I've edited the answer. About the time it takes - I don't know, you have to measure it yourself. The time it takes also depends on the size of the pcl. Our 100k points get transformed pretty fast.

peci1 gravatar imagepeci1 ( 2017-02-08 02:39:09 -0600 )edit

@peci1 Can you be more specific on time it takes? My pcl is definitely under 100k points.

skywalker gravatar imageskywalker ( 2017-02-08 09:51:24 -0600 )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: 2011-10-14 11:31:56 -0600

Seen: 9,942 times

Last updated: Feb 08 '17