point_cloud2_iterator.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Open Source Robotics Foundation
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Open Source Robotics Foundation nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #ifndef SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
00036 #define SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
00037 
00038 #include <sensor_msgs/PointCloud2.h>
00039 #include <cstdarg>
00040 #include <string>
00041 #include <vector>
00042 
00048 namespace
00049 {
00053 inline int sizeOfPointField(int datatype)
00054 {
00055   if ((datatype == sensor_msgs::PointField::INT8) || (datatype == sensor_msgs::PointField::UINT8))
00056     return 1;
00057   else if ((datatype == sensor_msgs::PointField::INT16) || (datatype == sensor_msgs::PointField::UINT16))
00058     return 2;
00059   else if ((datatype == sensor_msgs::PointField::INT32) || (datatype == sensor_msgs::PointField::UINT32) ||
00060       (datatype == sensor_msgs::PointField::FLOAT32))
00061     return 4;
00062   else if (datatype == sensor_msgs::PointField::FLOAT64)
00063     return 8;
00064   else
00065   {
00066     std::stringstream err;
00067     err << "PointField of type " << datatype << " does not exist";
00068     throw std::runtime_error(err.str());
00069   }
00070   return -1;
00071 }
00072 
00081 inline int addPointField(sensor_msgs::PointCloud2 &cloud_msg, const std::string &name, int count, int datatype,
00082     int offset)
00083 {
00084   sensor_msgs::PointField point_field;
00085   point_field.name = name;
00086   point_field.count = count;
00087   point_field.datatype = datatype;
00088   point_field.offset = offset;
00089   cloud_msg.fields.push_back(point_field);
00090 
00091   // Update the offset
00092   return offset + point_field.count * sizeOfPointField(datatype);
00093 }
00094 }
00095 
00097 
00098 namespace sensor_msgs
00099 {
00100 
00101 inline PointCloud2Modifier::PointCloud2Modifier(PointCloud2& cloud_msg) : cloud_msg_(cloud_msg)
00102 {
00103 }
00104 
00105 inline size_t PointCloud2Modifier::size() const
00106 {
00107   return cloud_msg_.data.size() / cloud_msg_.point_step;
00108 }
00109 
00110 inline void PointCloud2Modifier::reserve(size_t size)
00111 {
00112   cloud_msg_.data.reserve(size * cloud_msg_.point_step);
00113 }
00114 
00115 inline void PointCloud2Modifier::resize(size_t size)
00116 {
00117   cloud_msg_.data.resize(size * cloud_msg_.point_step);
00118 
00119   // Update height/width
00120   if (cloud_msg_.height == 1) {
00121     cloud_msg_.width = size;
00122     cloud_msg_.row_step = size * cloud_msg_.point_step;
00123   } else
00124     if (cloud_msg_.width == 1)
00125       cloud_msg_.height = size;
00126     else {
00127       cloud_msg_.height = 1;
00128       cloud_msg_.width = size;
00129       cloud_msg_.row_step = size * cloud_msg_.point_step;
00130     }
00131 }
00132 
00133 inline void PointCloud2Modifier::clear()
00134 {
00135   cloud_msg_.data.clear();
00136 
00137   // Update height/width
00138   if (cloud_msg_.height == 1)
00139     cloud_msg_.row_step = cloud_msg_.width = 0;
00140   else
00141     if (cloud_msg_.width == 1)
00142       cloud_msg_.height = 0;
00143     else
00144       cloud_msg_.row_step = cloud_msg_.width = cloud_msg_.height = 0;
00145 }
00146 
00147 
00165 inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
00166 {
00167   cloud_msg_.fields.clear();
00168   cloud_msg_.fields.reserve(n_fields);
00169   va_list vl;
00170   va_start(vl, n_fields);
00171   int offset = 0;
00172   for (int i = 0; i < n_fields; ++i) {
00173     // Create the corresponding PointField
00174     std::string name(va_arg(vl, char*));
00175     int count(va_arg(vl, int));
00176     int datatype(va_arg(vl, int));
00177     offset = addPointField(cloud_msg_, name, count, datatype, offset);
00178   }
00179   va_end(vl);
00180 
00181   // Resize the point cloud accordingly
00182   cloud_msg_.point_step = offset;
00183   cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
00184   cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
00185 }
00186 
00197 inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...)
00198 {
00199   cloud_msg_.fields.clear();
00200   cloud_msg_.fields.reserve(n_fields);
00201   va_list vl;
00202   va_start(vl, n_fields);
00203   int offset = 0;
00204   for (int i = 0; i < n_fields; ++i) {
00205     // Create the corresponding PointFields
00206     std::string
00207     field_name = std::string(va_arg(vl, char*));
00208     if (field_name == "xyz") {
00209       sensor_msgs::PointField point_field;
00210       // Do x, y and z
00211       offset = addPointField(cloud_msg_, "x", 1, sensor_msgs::PointField::FLOAT32, offset);
00212       offset = addPointField(cloud_msg_, "y", 1, sensor_msgs::PointField::FLOAT32, offset);
00213       offset = addPointField(cloud_msg_, "z", 1, sensor_msgs::PointField::FLOAT32, offset);
00214       offset += sizeOfPointField(sensor_msgs::PointField::FLOAT32);
00215     } else
00216       if ((field_name == "rgb") || (field_name == "rgba")) {
00217         offset = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, offset);
00218         offset += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
00219       } else
00220         throw std::runtime_error("Field " + field_name + " does not exist");
00221   }
00222   va_end(vl);
00223 
00224   // Resize the point cloud accordingly
00225   cloud_msg_.point_step = offset;
00226   cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
00227   cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
00228 }
00229 
00231 
00232 namespace impl
00233 {
00234 
00237 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00238 PointCloud2IteratorBase<T, TT, U, C, V>::PointCloud2IteratorBase() : data_char_(0), data_(0), data_end_(0)
00239 {
00240 }
00241 
00246 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00247 PointCloud2IteratorBase<T, TT, U, C, V>::PointCloud2IteratorBase(C &cloud_msg, const std::string &field_name)
00248 {
00249   int offset = set_field(cloud_msg, field_name);
00250 
00251   data_char_ = &(cloud_msg.data.front()) + offset;
00252   data_ = reinterpret_cast<TT*>(data_char_);
00253   data_end_ = reinterpret_cast<TT*>(&(cloud_msg.data.back()) + 1 + offset);
00254 }
00255 
00260 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00261 V<T>& PointCloud2IteratorBase<T, TT, U, C, V>::operator =(const V<T> &iter)
00262 {
00263   if (this != &iter)
00264   {
00265     point_step_ = iter.point_step_;
00266     data_char_ = iter.data_char_;
00267     data_ = iter.data_;
00268     data_end_ = iter.data_end_;
00269     is_bigendian_ = iter.is_bigendian_;
00270   }
00271 
00272   return *this;
00273 }
00274 
00280 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00281 TT& PointCloud2IteratorBase<T, TT, U, C, V>::operator [](size_t i) const
00282 {
00283   return *(data_ + i);
00284 }
00285 
00289 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00290 TT& PointCloud2IteratorBase<T, TT, U, C, V>::operator *() const
00291 {
00292   return *data_;
00293 }
00294 
00298 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00299 V<T>& PointCloud2IteratorBase<T, TT, U, C, V>::operator ++()
00300 {
00301   data_char_ += point_step_;
00302   data_ = reinterpret_cast<TT*>(data_char_);
00303   return *static_cast<V<T>*>(this);
00304 }
00305 
00310 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00311 V<T> PointCloud2IteratorBase<T, TT, U, C, V>::operator +(int i)
00312 {
00313   V<T> res = *static_cast<V<T>*>(this);
00314 
00315   res.data_char_ += i*point_step_;
00316   res.data_ = reinterpret_cast<TT*>(res.data_char_);
00317 
00318   return res;
00319 }
00320 
00324 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00325 V<T>& PointCloud2IteratorBase<T, TT, U, C, V>::operator +=(int i)
00326 {
00327   data_char_ += i*point_step_;
00328   data_ = reinterpret_cast<TT*>(data_char_);
00329   return *static_cast<V<T>*>(this);
00330 }
00331 
00335 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00336 bool PointCloud2IteratorBase<T, TT, U, C, V>::operator !=(const V<T>& iter) const
00337 {
00338   return iter.data_ != data_;
00339 }
00340 
00344 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00345 V<T> PointCloud2IteratorBase<T, TT, U, C, V>::end() const
00346 {
00347   V<T> res = *static_cast<const V<T>*>(this);
00348   res.data_ = data_end_;
00349   return res;
00350 }
00351 
00357 template<typename T, typename TT, typename U, typename C, template <typename> class V>
00358 int PointCloud2IteratorBase<T, TT, U, C, V>::set_field(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name)
00359 {
00360   is_bigendian_ = cloud_msg.is_bigendian;
00361   point_step_ = cloud_msg.point_step;
00362   // make sure the channel is valid
00363   std::vector<sensor_msgs::PointField>::const_iterator field_iter = cloud_msg.fields.begin(), field_end =
00364       cloud_msg.fields.end();
00365   while ((field_iter != field_end) && (field_iter->name != field_name))
00366     ++field_iter;
00367 
00368   if (field_iter == field_end) {
00369     // Handle the special case of r,g,b,a (we assume they are understood as the channels of an rgb or rgba field)
00370     if ((field_name == "r") || (field_name == "g") || (field_name == "b") || (field_name == "a"))
00371     {
00372       // Check that rgb or rgba is present
00373       field_iter = cloud_msg.fields.begin();
00374       while ((field_iter != field_end) && (field_iter->name != "rgb") && (field_iter->name != "rgba"))
00375         ++field_iter;
00376       if (field_iter == field_end)
00377         throw std::runtime_error("Field " + field_name + " does not exist");
00378       if (field_name == "r")
00379       {
00380         if (is_bigendian_)
00381           return field_iter->offset + 1;
00382         else
00383           return field_iter->offset + 2;
00384       }
00385       if (field_name == "g")
00386       {
00387         if (is_bigendian_)
00388           return field_iter->offset + 2;
00389         else
00390           return field_iter->offset + 1;
00391       }
00392       if (field_name == "b")
00393       {
00394         if (is_bigendian_)
00395           return field_iter->offset + 3;
00396         else
00397           return field_iter->offset + 0;
00398       }
00399       if (field_name == "a")
00400       {
00401         if (is_bigendian_)
00402           return field_iter->offset + 0;
00403         else
00404           return field_iter->offset + 3;
00405       }
00406     } else
00407       throw std::runtime_error("Field " + field_name + " does not exist");
00408   }
00409 
00410   return field_iter->offset;
00411 }
00412 
00413 }
00414 }
00415 
00416 #endif// SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H


sensor_msgs
Author(s):
autogenerated on Thu Jun 6 2019 18:15:45