00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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
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
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
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
00206 std::string
00207 field_name = std::string(va_arg(vl, char*));
00208 if (field_name == "xyz") {
00209 sensor_msgs::PointField point_field;
00210
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
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
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
00370 if ((field_name == "r") || (field_name == "g") || (field_name == "b") || (field_name == "a"))
00371 {
00372
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