conversions.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: conversions.h 6126 2012-07-03 20:19:58Z aichim $
00037  *
00038  */
00039 
00040 #ifndef PCL_ROS_CONVERSIONS_H_ 
00041 #define PCL_ROS_CONVERSIONS_H_
00042 
00043 #include <sensor_msgs/PointField.h>
00044 #include <sensor_msgs/PointCloud2.h>
00045 #include <sensor_msgs/Image.h>
00046 #include <pcl/point_cloud.h>
00047 #include <pcl/point_traits.h>
00048 #include <pcl/for_each_type.h>
00049 #include <pcl/exceptions.h>
00050 #include <pcl/console/print.h>
00051 #include <boost/foreach.hpp>
00052 
00053 namespace pcl
00054 {
00055   namespace detail
00056   {
00057     // For converting template point cloud to message.
00058     template<typename PointT>
00059     struct FieldAdder
00060     {
00061       FieldAdder (std::vector<sensor_msgs::PointField>& fields) : fields_ (fields) {};
00062       
00063       template<typename U> void operator() ()
00064       {
00065         sensor_msgs::PointField f;
00066         f.name = traits::name<PointT, U>::value;
00067         f.offset = traits::offset<PointT, U>::value;
00068         f.datatype = traits::datatype<PointT, U>::value;
00069         f.count = traits::datatype<PointT, U>::size;
00070         fields_.push_back (f);
00071       }
00072 
00073       std::vector<sensor_msgs::PointField>& fields_;
00074     };
00075 
00076     // For converting message to template point cloud.
00077     template<typename PointT>
00078     struct FieldMapper
00079     {
00080       FieldMapper (const std::vector<sensor_msgs::PointField>& fields,
00081                    std::vector<FieldMapping>& map)
00082         : fields_ (fields), map_ (map)
00083       {
00084       }
00085       
00086       template<typename Tag> void 
00087       operator () ()
00088       {
00089         BOOST_FOREACH (const sensor_msgs::PointField& field, fields_)
00090         {
00091           if (FieldMatches<PointT, Tag>()(field))
00092           {
00093             FieldMapping mapping;
00094             mapping.serialized_offset = field.offset;
00095             mapping.struct_offset = traits::offset<PointT, Tag>::value;
00096             mapping.size = sizeof (typename traits::datatype<PointT, Tag>::type);
00097             map_.push_back (mapping);
00098             return;
00099           }
00100         }
00101         // Disable thrown exception per #595: http://dev.pointclouds.org/issues/595
00102         PCL_WARN ("Failed to find match for field '%s'.\n", traits::name<PointT, Tag>::value);
00103         //throw pcl::InvalidConversionException (ss.str ());
00104       }
00105 
00106       const std::vector<sensor_msgs::PointField>& fields_;
00107       std::vector<FieldMapping>& map_;
00108     };
00109 
00110     inline bool 
00111     fieldOrdering (const FieldMapping& a, const FieldMapping& b)
00112     {
00113       return (a.serialized_offset < b.serialized_offset);
00114     }
00115 
00116   } //namespace detail
00117 
00118   template<typename PointT> void 
00119   createMapping (const std::vector<sensor_msgs::PointField>& msg_fields, MsgFieldMap& field_map)
00120   {
00121     // Create initial 1-1 mapping between serialized data segments and struct fields
00122     detail::FieldMapper<PointT> mapper (msg_fields, field_map);
00123     for_each_type< typename traits::fieldList<PointT>::type > (mapper);
00124 
00125     // Coalesce adjacent fields into single memcpy's where possible
00126     if (field_map.size() > 1)
00127     {
00128       std::sort(field_map.begin(), field_map.end(), detail::fieldOrdering);
00129       MsgFieldMap::iterator i = field_map.begin(), j = i + 1;
00130       while (j != field_map.end())
00131       {
00132         // This check is designed to permit padding between adjacent fields.
00135         if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset)
00136         {
00137           i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size);
00138           j = field_map.erase(j);
00139         }
00140         else
00141         {
00142           ++i;
00143           ++j;
00144         }
00145       }
00146     }
00147   }
00148 
00162   template <typename PointT> void 
00163   fromROSMsg (const sensor_msgs::PointCloud2& msg, pcl::PointCloud<PointT>& cloud,
00164               const MsgFieldMap& field_map)
00165   {
00166     // Copy info fields
00167     cloud.header   = msg.header;
00168     cloud.width    = msg.width;
00169     cloud.height   = msg.height;
00170     cloud.is_dense = msg.is_dense == 1;
00171 
00172     // Copy point data
00173     uint32_t num_points = msg.width * msg.height;
00174     cloud.points.resize (num_points);
00175     uint8_t* cloud_data = reinterpret_cast<uint8_t*>(&cloud.points[0]);
00176 
00177     // Check if we can copy adjacent points in a single memcpy
00178     if (field_map.size() == 1 &&
00179         field_map[0].serialized_offset == 0 &&
00180         field_map[0].struct_offset == 0 &&
00181         msg.point_step == sizeof(PointT))
00182     {
00183       uint32_t cloud_row_step = static_cast<uint32_t> (sizeof (PointT) * cloud.width);
00184       const uint8_t* msg_data = &msg.data[0];
00185       // Should usually be able to copy all rows at once
00186       if (msg.row_step == cloud_row_step)
00187       {
00188         memcpy (cloud_data, msg_data, msg.data.size ());
00189       }
00190       else
00191       {
00192         for (uint32_t i = 0; i < msg.height; ++i, cloud_data += cloud_row_step, msg_data += msg.row_step)
00193           memcpy (cloud_data, msg_data, cloud_row_step);
00194       }
00195 
00196     }
00197     else
00198     {
00199       // If not, memcpy each group of contiguous fields separately
00200       for (uint32_t row = 0; row < msg.height; ++row)
00201       {
00202         const uint8_t* row_data = &msg.data[row * msg.row_step];
00203         for (uint32_t col = 0; col < msg.width; ++col)
00204         {
00205           const uint8_t* msg_data = row_data + col * msg.point_step;
00206           BOOST_FOREACH (const detail::FieldMapping& mapping, field_map)
00207           {
00208             memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size);
00209           }
00210           cloud_data += sizeof (PointT);
00211         }
00212       }
00213     }
00214   }
00215 
00220   template<typename PointT> void 
00221   fromROSMsg (const sensor_msgs::PointCloud2& msg, pcl::PointCloud<PointT>& cloud)
00222   {
00223     MsgFieldMap field_map;
00224     createMapping<PointT> (msg.fields, field_map);
00225     fromROSMsg (msg, cloud, field_map);
00226   }
00227 
00232   template<typename PointT> void 
00233   toROSMsg (const pcl::PointCloud<PointT>& cloud, sensor_msgs::PointCloud2& msg)
00234   {
00235     // Ease the user's burden on specifying width/height for unorganized datasets
00236     if (cloud.width == 0 && cloud.height == 0)
00237     {
00238       msg.width  = static_cast<uint32_t>(cloud.points.size ());
00239       msg.height = 1;
00240     }
00241     else
00242     {
00243       assert (cloud.points.size () == cloud.width * cloud.height);
00244       msg.height = cloud.height;
00245       msg.width  = cloud.width;
00246     }
00247 
00248     // Fill point cloud binary data (padding and all)
00249     size_t data_size = sizeof (PointT) * cloud.points.size ();
00250     msg.data.resize (data_size);
00251     memcpy (&msg.data[0], &cloud.points[0], data_size);
00252 
00253     // Fill fields metadata
00254     msg.fields.clear ();
00255     for_each_type<typename traits::fieldList<PointT>::type> (detail::FieldAdder<PointT>(msg.fields));
00256 
00257     msg.header     = cloud.header;
00258     msg.point_step = sizeof (PointT);
00259     msg.row_step   = static_cast<uint32_t> (sizeof (PointT) * msg.width);
00260     msg.is_dense   = cloud.is_dense;
00262   }
00263 
00270   template<typename CloudT> void
00271   toROSMsg (const CloudT& cloud, sensor_msgs::Image& msg)
00272   {
00273     // Ease the user's burden on specifying width/height for unorganized datasets
00274     if (cloud.width == 0 && cloud.height == 0)
00275       throw std::runtime_error("Needs to be a dense like cloud!!");
00276     else
00277     {
00278       if (cloud.points.size () != cloud.width * cloud.height)
00279         throw std::runtime_error("The width and height do not match the cloud size!");
00280       msg.height = cloud.height;
00281       msg.width = cloud.width;
00282     }
00283 
00284     // ensor_msgs::image_encodings::BGR8;
00285     msg.encoding = "bgr8";
00286     msg.step = msg.width * sizeof (uint8_t) * 3;
00287     msg.data.resize (msg.step * msg.height);
00288     for (size_t y = 0; y < cloud.height; y++)
00289     {
00290       for (size_t x = 0; x < cloud.width; x++)
00291       {
00292         uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
00293         memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t));
00294       }
00295     }
00296   }
00297 
00303   inline void
00304   toROSMsg (const sensor_msgs::PointCloud2& cloud, sensor_msgs::Image& msg)
00305   {
00306     int rgb_index = -1;
00307     // Get the index we need
00308     for (size_t d = 0; d < cloud.fields.size (); ++d)
00309       if (cloud.fields[d].name == "rgb")
00310       {
00311         rgb_index = static_cast<int>(d);
00312         break;
00313       }
00314 
00315     if(rgb_index == -1)
00316       throw std::runtime_error ("No rgb field!!");
00317     if (cloud.width == 0 && cloud.height == 0)
00318       throw std::runtime_error ("Needs to be a dense like cloud!!");
00319     else
00320     {
00321       msg.height = cloud.height;
00322       msg.width = cloud.width;
00323     }
00324     int rgb_offset = cloud.fields[rgb_index].offset;
00325     int point_step = cloud.point_step;
00326 
00327     // sensor_msgs::image_encodings::BGR8;
00328     msg.encoding = "bgr8";
00329     msg.step = static_cast<uint32_t>(msg.width * sizeof (uint8_t) * 3);
00330     msg.data.resize (msg.step * msg.height);
00331 
00332     for (size_t y = 0; y < cloud.height; y++)
00333     {
00334       for (size_t x = 0; x < cloud.width; x++, rgb_offset += point_step)
00335       {
00336         uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
00337         memcpy (pixel, &(cloud.data[rgb_offset]), 3 * sizeof (uint8_t));
00338       }
00339     }
00340   }
00341 }
00342 
00343 #endif  //#ifndef PCL_ROS_CONVERSIONS_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:43