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;
88 point_field.datatype = datatype;
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));
177 int datatype(va_arg(vl,
int));
178 offset = addPointField(
cloud_msg_, name, count, datatype, offset);
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;
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>
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>
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>
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);
358 template<
typename T,
typename TT,
typename U,
typename C,
template <
typename>
class V>
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 V< T > & operator=(const V< T > &iter)
void reserve(size_t size)
PointCloud2Modifier(PointCloud2 &cloud_msg)
Default constructor.
void setPointCloud2Fields(int n_fields,...)
Function setting some fields in a PointCloud and adjusting the internals of the PointCloud2.
int set_field(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name)
TT & operator[](size_t i) const
Tools for manipulating sensor_msgs.
V< T > & operator+=(int i)
void clear()
remove all T's from the original sensor_msgs::PointCloud2
PointCloud2IteratorBase()
void setPointCloud2FieldsByString(int n_fields,...)
Function setting some fields in a PointCloud and adjusting the internals of the PointCloud2.
bool operator!=(const V< T > &iter) const