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


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Apr 27 2018 05:10:54