point_cloud2.cpp
Go to the documentation of this file.
00001 #include <sensor_msgs/PointField.h>
00002 #include <sensor_msgs/PointCloud2.h>
00003 #include <mrpt/slam/CSimplePointsMap.h>
00004 #include <mrpt/slam/CColouredPointsMap.h>
00005 #include "mrpt_bridge/point_cloud2.h"
00006 #include <ros/console.h>
00007 
00008 namespace mrpt_bridge
00009 {
00010 
00011 inline bool check_field(const sensor_msgs::PointField& input_field, std::string check_name,
00012                         const sensor_msgs::PointField** output)
00013 {
00014   bool coherence_error = false;
00015   if (input_field.name == check_name)
00016   {
00017     if (input_field.datatype != sensor_msgs::PointField::FLOAT32
00018         && input_field.datatype != sensor_msgs::PointField::FLOAT64)
00019     {
00020       *output = NULL;
00021       coherence_error = true;
00022     }
00023     else
00024     {
00025       *output = &input_field;
00026     }
00027   }
00028   return coherence_error;
00029 }
00030 
00031 inline void get_float_from_field(const sensor_msgs::PointField* field, const unsigned char* data, float& output)
00032 {
00033   if (field != NULL)
00034   {
00035     if (field->datatype == sensor_msgs::PointField::FLOAT32)
00036       output = *(reinterpret_cast<const float*>(&data[field->offset]));
00037     else
00038       output = (float)(*(reinterpret_cast<const double*>(&data[field->offset])));
00039 
00040   }
00041   else
00042     output = 0.0;
00043 }
00044 
00049 bool copy(const sensor_msgs::PointCloud2 &msg, mrpt::slam::CSimplePointsMap &obj)
00050 {
00051   // Copy point data
00052   unsigned int num_points = msg.width * msg.height;
00053   obj.clear();
00054   obj.reserve(num_points);
00055 
00056   bool incompatible_clouds = false;
00057   const sensor_msgs::PointField *x_field = NULL, *y_field = NULL, *z_field = NULL;
00058 
00059   for (unsigned int i = 0; i < msg.fields.size() && !incompatible_clouds; i++)
00060   {
00061     incompatible_clouds |= check_field(msg.fields[i], "x", &x_field);
00062     incompatible_clouds |= check_field(msg.fields[i], "y", &y_field);
00063     incompatible_clouds |= check_field(msg.fields[i], "z", &z_field);
00064   }
00065 
00066   if (incompatible_clouds || (x_field == NULL && y_field == NULL && z_field == NULL))
00067     return false;
00068 
00069   // If not, memcpy each group of contiguous fields separately
00070   for (unsigned int row = 0; row < msg.height; ++row)
00071   {
00072     const unsigned char* row_data = &msg.data[row * msg.row_step];
00073     for (uint32_t col = 0; col < msg.width; ++col)
00074     {
00075       const unsigned char* msg_data = row_data + col * msg.point_step;
00076 
00077       float x, y, z;
00078       get_float_from_field(x_field, msg_data, x);
00079       get_float_from_field(y_field, msg_data, y);
00080       get_float_from_field(z_field, msg_data, z);
00081       obj.insertPoint(x, y, z);
00082     }
00083   }
00084 
00085   return true;
00086 }
00087 
00094 bool copy(const mrpt::slam::CSimplePointsMap &obj, const std_msgs::Header &msg_header,
00095                            sensor_msgs::PointCloud2 &msg)
00096 {
00097   MRPT_TODO("Implement pointcloud2 mrpt2ros");
00098   throw ros::Exception("not implemented yet.");
00099   return true;
00100 }
00101 
00102 } //namespace image_proc


mrpt_bridge
Author(s):
autogenerated on Tue Aug 5 2014 10:58:06