point_cloud2.cpp

Go to the documentation of this file.

#include <sensor_msgs/PointField.h>
#include <sensor_msgs/PointCloud2.h>
#include <mrpt/slam/CSimplePointsMap.h>
#include <mrpt/slam/CColouredPointsMap.h>
#include "mrpt_bridge/point_cloud2.h"
#include <ros/console.h>

namespace mrpt_bridge
{

inline bool check_field(const sensor_msgs::PointField& input_field, std::string check_name,
                        const sensor_msgs::PointField** output)
{
  bool coherence_error = false;
  if (input_field.name == check_name)
  {
    if (input_field.datatype != sensor_msgs::PointField::FLOAT32
        && input_field.datatype != sensor_msgs::PointField::FLOAT64)
    {
      *output = NULL;
      coherence_error = true;
    }
    else
    {
      *output = &input_field;
    }
  }
  return coherence_error;
}

inline void get_float_from_field(const sensor_msgs::PointField* field, const unsigned char* data, float& output)
{
  if (field != NULL)
  {
    if (field->datatype == sensor_msgs::PointField::FLOAT32)
      output = *(reinterpret_cast<const float*>(&data[field->offset]));
    else
      output = (float)(*(reinterpret_cast<const double*>(&data[field->offset])));

  }
  else
    output = 0.0;
}

bool copy(const sensor_msgs::PointCloud2 &msg, mrpt::slam::CSimplePointsMap &obj)
{
  // Copy point data
  unsigned int num_points = msg.width * msg.height;
  obj.clear();
  obj.reserve(num_points);

  bool incompatible_clouds = false;
  const sensor_msgs::PointField *x_field = NULL, *y_field = NULL, *z_field = NULL;

  for (unsigned int i = 0; i < msg.fields.size() && !incompatible_clouds; i++)
  {
    incompatible_clouds |= check_field(msg.fields[i], "x", &x_field);
    incompatible_clouds |= check_field(msg.fields[i], "y", &y_field);
    incompatible_clouds |= check_field(msg.fields[i], "z", &z_field);
  }

  if (incompatible_clouds || (x_field == NULL && y_field == NULL && z_field == NULL))
    return false;

  // If not, memcpy each group of contiguous fields separately
  for (unsigned int row = 0; row < msg.height; ++row)
  {
    const unsigned char* row_data = &msg.data[row * msg.row_step];
    for (uint32_t col = 0; col < msg.width; ++col)
    {
      const unsigned char* msg_data = row_data + col * msg.point_step;

      float x, y, z;
      get_float_from_field(x_field, msg_data, x);
      get_float_from_field(y_field, msg_data, y);
      get_float_from_field(z_field, msg_data, z);
      obj.insertPoint(x, y, z);
    }
  }

  return true;
}

bool copy(const mrpt::slam::CSimplePointsMap &obj, const std_msgs::Header &msg_header,
                           sensor_msgs::PointCloud2 &msg)
{
  MRPT_TODO("Implement pointcloud2 mrpt2ros");
  throw ros::Exception("not implemented yet.");
  return true;
}

} //namespace image_proc