Revision history [back]

Here are a few things that hopefully will help you get this working properly.

The point cloud message you posted the rostopic echo of has a custom point type with values (x,y,z,t,reflectivity,intensity,ring) to be able to use this type of point cloud with the C++ PCL library there will need to be a definition of this non-standard point type. This should look something like this code snippet from some of my work with ROS LiDAR hardware drivers:

#include <pcl/point_types.h>

/// Custom PCL point type with additional point data 
struct OcularPointType
{
  PCL_ADD_POINT4D;
  float amplitude;
  float reflectance;
  int pulseShapeDeviation;
  unsigned int timestamp;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (CustomPointType,
                                   (float, x, x)
                                   (float, y, y)
                                   (float, z, z)
                                   (float, amplitude, amplitude)
                                   (float, reflectance, reflectance)
                                   (float, pulseShapeDeviation, pulseShapeDeviation)
                                   (unsigned int, timestamp, timestamp)
)

This then allows you to use it with all the standard PCL functions. The ROS driver for your LiDAR should include a definition of this in a in include file somewhere, in which case you will be able to work with these point clouds natively.

If this doesn't get it working here is a last resort solution. The following template function demonstrates how to parse the ROS PointCloud2 message into a pcl::PointCloud yourself, this is doing the job of pcl::fromROSMsg but you can see exactly what's going on and fix it if you need to.

void yourPointCloud2Callback(const sensor_msgs::PointCloud2 & cloud_msg)
{
  pcl::PointCloud<pcl::PointXYZI> cloud;

  // Get the field structure of this point cloud
  int pointBytes = cloud_msg.point_step;
  int offset_x;
  int offset_y;
  int offset_z;
  int offset_int;
  for (int f=0; f<cloud_msg.fields.size(); ++f)
  {
    if (cloud_msg.fields[f].name == "x")
      offset_x = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "y")
      offset_y = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "z")
      offset_z = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "intensity")
      offset_int = cloud_msg.fields[f].offset;
  }

  // populate point cloud object
  for (int p=0; p<cloud_msg.width; ++p)
  {
      pcl::PointXYZI newPoint;

      newPoint.x = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_x);
      newPoint.y = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_y);
      newPoint.z = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_z);
      newPoint.intensity = *(unsigned char*)(&cloud_msg.data[0] + (pointBytes*p) + offset_int);

      cloud.points.push_back(newPoint);
  }

// The rest of your function here....

}

Hope this helps you get this working.