Ask Your Question
0

Better way of converting sensor_msgs::PointCloud2 to PointCloud<PointXYZRGB>

asked 2018-02-19 09:44:04 -0500

Ravi Joshi gravatar image

Hi, I am receiving sensor_msgs::PointCloud2 inside a callback function. I am converting it to PointCloud<PointXYZRGB> using following procedure-

  1. First convert sensor_msgs::PointCloud2 to pcl::PCLPointCloud2
  2. Later convert pcl::PCLPointCloud2 to pcl::PointCloud<pcl::PointXYZRGBA>
  3. Finally convert pcl::PointCloud<pcl::PointXYZRGBA> to pcl::PointCloud<pcl::PointXYZRGB>

See below the code snippet-

inline void PointCloudXYZRGBAtoXYZRGB(pcl::PointCloud<pcl::PointXYZRGBA>& in, pcl::PointCloud<pcl::PointXYZRGB>& out)
{
  out.width   = in.width;
  out.height  = in.height;
  out.points.resize(in.points.size());
  for (size_t i = 0; i < in.points.size (); i++)
  {
    out.points[i].x = in.points[i].x;
    out.points[i].y = in.points[i].y;
    out.points[i].z = in.points[i].z;
    out.points[i].r = in.points[i].r;
    out.points[i].g = in.points[i].g;
    out.points[i].b = in.points[i].b;
  }
}

void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& pc_msg)
{
  pcl::PCLPointCloud2 pcl_pc2;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGBA> temp_cloud;
  pcl_conversions::toPCL(*msg, pcl_pc2);
  pcl::fromPCLPointCloud2(pcl_pc2, temp_cloud);
  PointCloudXYZRGBAtoXYZRGB(temp_cloud, *cloud);
}

I am wondering if there exists better way which is efficient in terms of time taken. Kindly suggest.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2018-02-19 15:13:13 -0500

lucasw gravatar image

updated 2018-02-19 15:18:09 -0500

I believe you want the example code from http://wiki.ros.org/pcl_ros#Subscribi... but make the

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

into:

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

http://wiki.ros.org/pcl_ros#ROS_C.2B-... :

pcl_ros extends the ROS C++ client library to support message passing with PCL native data types. Simply add the following include to your ROS node source code:

#include <pcl_ros/point_cloud.h>

This header allows you to publish and subscribe pcl::PointCloud<t> objects as ROS messages. These appear to ROS as sensor_msgs/PointCloud2 messages, offering seamless interoperability with non-PCL-using ROS nodes. For example, you may publish a pcl::PointCloud<t> in one of your nodes and visualize it in rviz using a Point Cloud2 display. It works by hooking into the roscpp serialization infrastructure.

The old format sensor_msgs/PointCloud is not supported in PCL.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-02-19 09:44:04 -0500

Seen: 2,389 times

Last updated: Feb 19 '18