35 #ifndef SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
36 #define SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
38 #include <sensor_msgs/PointCloud2.h>
54 inline int sizeOfPointField(
int datatype)
56 if ((datatype == sensor_msgs::PointField::INT8) || (datatype == sensor_msgs::PointField::UINT8))
58 else if ((datatype == sensor_msgs::PointField::INT16) || (datatype == sensor_msgs::PointField::UINT16))
60 else if ((datatype == sensor_msgs::PointField::INT32) || (datatype == sensor_msgs::PointField::UINT32) ||
61 (datatype == sensor_msgs::PointField::FLOAT32))
63 else if (datatype == sensor_msgs::PointField::FLOAT64)
67 std::stringstream err;
68 err <<
"PointField of type " <<
datatype <<
" does not exist";
69 throw std::runtime_error(err.str());
82 inline int addPointField(sensor_msgs::PointCloud2 &cloud_msg,
const std::string &name,
int count,
int datatype,
85 sensor_msgs::PointField point_field;
86 point_field.name = name;
87 point_field.count = count;
89 point_field.offset = offset;
90 cloud_msg.fields.push_back(point_field);
93 return offset + point_field.count * sizeOfPointField(datatype);
171 va_start(vl, n_fields);
173 for (
int i = 0; i < n_fields; ++i) {
175 std::string name(va_arg(vl,
char*));
176 int count(va_arg(vl,
int));
203 va_start(vl, n_fields);
205 for (
int i = 0; i < n_fields; ++i) {
208 field_name = std::string(va_arg(vl,
char*));
209 if (field_name ==
"xyz") {
210 sensor_msgs::PointField point_field;
212 offset = addPointField(
cloud_msg_,
"x", 1, sensor_msgs::PointField::FLOAT32, offset);
213 offset = addPointField(
cloud_msg_,
"y", 1, sensor_msgs::PointField::FLOAT32, offset);
214 offset = addPointField(
cloud_msg_,
"z", 1, sensor_msgs::PointField::FLOAT32, offset);
215 offset += sizeOfPointField(sensor_msgs::PointField::FLOAT32);
217 if ((field_name ==
"rgb") || (field_name ==
"rgba")) {
218 offset = addPointField(
cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, offset);
219 offset += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
221 throw std::runtime_error(
"Field " + field_name +
" does not exist");
238 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
247 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
250 int offset = set_field(cloud_msg, field_name);
252 data_char_ = &(cloud_msg.data.front()) + offset;
253 data_ =
reinterpret_cast<TT*
>(data_char_);
254 data_end_ =
reinterpret_cast<TT*
>(&(cloud_msg.data.back()) + 1 + offset);
261 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
267 data_char_ = iter.data_char_;
269 data_end_ = iter.data_end_;
270 is_bigendian_ = iter.is_bigendian_;
281 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
290 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
299 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
302 data_char_ += point_step_;
303 data_ =
reinterpret_cast<TT*
>(data_char_);
304 return *
static_cast<V<T>*
>(
this);
311 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
314 V<T> res = *
static_cast<V<T>*
>(
this);
317 res.data_ =
reinterpret_cast<TT*
>(res.data_char_);
325 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
328 data_char_ += i*point_step_;
329 data_ =
reinterpret_cast<TT*
>(data_char_);
330 return *
static_cast<V<T>*
>(
this);
336 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
339 return iter.
data_ != data_;
345 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
348 V<T> res = *
static_cast<const V<T>*
>(
this);
349 res.
data_ = data_end_;
358 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
361 is_bigendian_ = cloud_msg.is_bigendian;
362 point_step_ = cloud_msg.point_step;
364 std::vector<sensor_msgs::PointField>::const_iterator field_iter = cloud_msg.fields.begin(), field_end =
365 cloud_msg.fields.
end();
366 while ((field_iter != field_end) && (field_iter->name != field_name))
369 if (field_iter == field_end) {
371 if ((field_name ==
"r") || (field_name ==
"g") || (field_name ==
"b") || (field_name ==
"a"))
374 field_iter = cloud_msg.fields.begin();
375 while ((field_iter != field_end) && (field_iter->name !=
"rgb") && (field_iter->name !=
"rgba"))
377 if (field_iter == field_end)
378 throw std::runtime_error(
"Field " + field_name +
" does not exist");
379 if (field_name ==
"r")
382 return field_iter->offset + 1;
384 return field_iter->offset + 2;
386 if (field_name ==
"g")
389 return field_iter->offset + 2;
391 return field_iter->offset + 1;
393 if (field_name ==
"b")
396 return field_iter->offset + 3;
398 return field_iter->offset + 0;
400 if (field_name ==
"a")
403 return field_iter->offset + 0;
405 return field_iter->offset + 3;
408 throw std::runtime_error(
"Field " + field_name +
" does not exist");
411 return field_iter->offset;
417 #endif// SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H