impl/point_cloud2_iterator.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Open Source Robotics Foundation
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Open Source Robotics Foundation nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
36 #define SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
37 
38 #include <sensor_msgs/PointCloud2.h>
39 #include <cstdarg>
40 #include <sstream>
41 #include <string>
42 #include <vector>
43 
49 namespace
50 {
54 inline int sizeOfPointField(int datatype)
55 {
56  if ((datatype == sensor_msgs::PointField::INT8) || (datatype == sensor_msgs::PointField::UINT8))
57  return 1;
58  else if ((datatype == sensor_msgs::PointField::INT16) || (datatype == sensor_msgs::PointField::UINT16))
59  return 2;
60  else if ((datatype == sensor_msgs::PointField::INT32) || (datatype == sensor_msgs::PointField::UINT32) ||
61  (datatype == sensor_msgs::PointField::FLOAT32))
62  return 4;
63  else if (datatype == sensor_msgs::PointField::FLOAT64)
64  return 8;
65  else
66  {
67  std::stringstream err;
68  err << "PointField of type " << datatype << " does not exist";
69  throw std::runtime_error(err.str());
70  }
71  return -1;
72 }
73 
82 inline int addPointField(sensor_msgs::PointCloud2 &cloud_msg, const std::string &name, int count, int datatype,
83  int offset)
84 {
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);
91 
92  // Update the offset
93  return offset + point_field.count * sizeOfPointField(datatype);
94 }
95 }
96 
98 
99 namespace sensor_msgs
100 {
101 
102 inline PointCloud2Modifier::PointCloud2Modifier(PointCloud2& cloud_msg) : cloud_msg_(cloud_msg)
103 {
104 }
105 
106 inline size_t PointCloud2Modifier::size() const
107 {
108  return cloud_msg_.data.size() / cloud_msg_.point_step;
109 }
110 
112 {
113  cloud_msg_.data.reserve(size * cloud_msg_.point_step);
114 }
115 
117 {
118  cloud_msg_.data.resize(size * cloud_msg_.point_step);
119 
120  // Update height/width
121  if (cloud_msg_.height == 1) {
122  cloud_msg_.width = size;
123  cloud_msg_.row_step = size * cloud_msg_.point_step;
124  } else
125  if (cloud_msg_.width == 1)
126  cloud_msg_.height = size;
127  else {
128  cloud_msg_.height = 1;
129  cloud_msg_.width = size;
130  cloud_msg_.row_step = size * cloud_msg_.point_step;
131  }
132 }
133 
135 {
136  cloud_msg_.data.clear();
137 
138  // Update height/width
139  if (cloud_msg_.height == 1)
140  cloud_msg_.row_step = cloud_msg_.width = 0;
141  else
142  if (cloud_msg_.width == 1)
143  cloud_msg_.height = 0;
144  else
145  cloud_msg_.row_step = cloud_msg_.width = cloud_msg_.height = 0;
146 }
147 
148 
166 inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
167 {
168  cloud_msg_.fields.clear();
169  cloud_msg_.fields.reserve(n_fields);
170  va_list vl;
171  va_start(vl, n_fields);
172  int offset = 0;
173  for (int i = 0; i < n_fields; ++i) {
174  // Create the corresponding PointField
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);
179  }
180  va_end(vl);
181 
182  // Resize the point cloud accordingly
183  cloud_msg_.point_step = offset;
184  cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
185  cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
186 }
187 
199 {
200  cloud_msg_.fields.clear();
201  cloud_msg_.fields.reserve(n_fields);
202  va_list vl;
203  va_start(vl, n_fields);
204  int offset = 0;
205  for (int i = 0; i < n_fields; ++i) {
206  // Create the corresponding PointFields
207  std::string
208  field_name = std::string(va_arg(vl, char*));
209  if (field_name == "xyz") {
210  sensor_msgs::PointField point_field;
211  // Do x, y and z
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);
216  } else
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);
220  } else
221  throw std::runtime_error("Field " + field_name + " does not exist");
222  }
223  va_end(vl);
224 
225  // Resize the point cloud accordingly
226  cloud_msg_.point_step = offset;
227  cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
228  cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
229 }
230 
232 
233 namespace impl
234 {
235 
238 template<typename T, typename TT, typename U, typename C, template <typename> class V>
240 {
241 }
242 
247 template<typename T, typename TT, typename U, typename C, template <typename> class V>
248 PointCloud2IteratorBase<T, TT, U, C, V>::PointCloud2IteratorBase(C &cloud_msg, const std::string &field_name)
249 {
250  int offset = set_field(cloud_msg, field_name);
251 
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);
255 }
256 
261 template<typename T, typename TT, typename U, typename C, template <typename> class V>
263 {
264  if (this != &iter)
265  {
266  point_step_ = iter.point_step_;
267  data_char_ = iter.data_char_;
268  data_ = iter.data_;
269  data_end_ = iter.data_end_;
270  is_bigendian_ = iter.is_bigendian_;
271  }
272 
273  return *this;
274 }
275 
281 template<typename T, typename TT, typename U, typename C, template <typename> class V>
283 {
284  return *(data_ + i);
285 }
286 
290 template<typename T, typename TT, typename U, typename C, template <typename> class V>
292 {
293  return *data_;
294 }
295 
299 template<typename T, typename TT, typename U, typename C, template <typename> class V>
301 {
303  data_ = reinterpret_cast<TT*>(data_char_);
304  return *static_cast<V<T>*>(this);
305 }
306 
311 template<typename T, typename TT, typename U, typename C, template <typename> class V>
313 {
314  V<T> res = *static_cast<V<T>*>(this);
315 
316  res.data_char_ += i*point_step_;
317  res.data_ = reinterpret_cast<TT*>(res.data_char_);
318 
319  return res;
320 }
321 
325 template<typename T, typename TT, typename U, typename C, template <typename> class V>
327 {
328  data_char_ += i*point_step_;
329  data_ = reinterpret_cast<TT*>(data_char_);
330  return *static_cast<V<T>*>(this);
331 }
332 
336 template<typename T, typename TT, typename U, typename C, template <typename> class V>
338 {
339  return iter.data_ != data_;
340 }
341 
345 template<typename T, typename TT, typename U, typename C, template <typename> class V>
347 {
348  V<T> res = *static_cast<const V<T>*>(this);
349  res.data_ = data_end_;
350  return res;
351 }
352 
358 template<typename T, typename TT, typename U, typename C, template <typename> class V>
359 int PointCloud2IteratorBase<T, TT, U, C, V>::set_field(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name)
360 {
361  is_bigendian_ = cloud_msg.is_bigendian;
362  point_step_ = cloud_msg.point_step;
363  // make sure the channel is valid
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))
367  ++field_iter;
368 
369  if (field_iter == field_end) {
370  // Handle the special case of r,g,b,a (we assume they are understood as the channels of an rgb or rgba field)
371  if ((field_name == "r") || (field_name == "g") || (field_name == "b") || (field_name == "a"))
372  {
373  // Check that rgb or rgba is present
374  field_iter = cloud_msg.fields.begin();
375  while ((field_iter != field_end) && (field_iter->name != "rgb") && (field_iter->name != "rgba"))
376  ++field_iter;
377  if (field_iter == field_end)
378  throw std::runtime_error("Field " + field_name + " does not exist");
379  if (field_name == "r")
380  {
381  if (is_bigendian_)
382  return field_iter->offset + 1;
383  else
384  return field_iter->offset + 2;
385  }
386  if (field_name == "g")
387  {
388  if (is_bigendian_)
389  return field_iter->offset + 2;
390  else
391  return field_iter->offset + 1;
392  }
393  if (field_name == "b")
394  {
395  if (is_bigendian_)
396  return field_iter->offset + 3;
397  else
398  return field_iter->offset + 0;
399  }
400  if (field_name == "a")
401  {
402  if (is_bigendian_)
403  return field_iter->offset + 0;
404  else
405  return field_iter->offset + 3;
406  }
407  } else
408  throw std::runtime_error("Field " + field_name + " does not exist");
409  }
410 
411  return field_iter->offset;
412 }
413 
414 }
415 }
416 
417 #endif// SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
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)
Tools for manipulating sensor_msgs.
void clear()
remove all T&#39;s from the original sensor_msgs::PointCloud2
void setPointCloud2FieldsByString(int n_fields,...)
Function setting some fields in a PointCloud and adjusting the internals of the PointCloud2.


sensor_msgs
Author(s): Tully Foote
autogenerated on Thu Jan 14 2021 03:25:16