Gazebo Msgs

API Reference

10.3.1
PointCloudPackedUtils.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 // Inspired by
19 // https://github.com/ros/common_msgs/blob/275b09a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
20 
21 #ifndef GZ_MSGS_POINTCLOUDPACKEDUTILS_HH_
22 #define GZ_MSGS_POINTCLOUDPACKEDUTILS_HH_
23 
24 #include <gz/msgs/pointcloud_packed.pb.h>
25 
26 #include <cstdarg>
27 #include <functional>
28 #include <sstream>
29 #include <string>
30 #include <utility>
31 #include <vector>
32 
33 #include "gz/msgs/config.hh"
34 #include "gz/msgs/detail/PointCloudPackedUtils.hh"
35 #include <gz/math/Helpers.hh>
36 
37 namespace gz
38 {
39 namespace msgs
40 {
42 inline void InitPointCloudPacked(msgs::PointCloudPacked &_msg,
43  const std::string &_frameId, bool _memoryAligned,
45  msgs::PointCloudPacked::Field::DataType>> &_fields)
46 {
47  uint32_t offset = 0;
48 
49  // Helper function that will set a single field.
50  std::function<void(const std::string &,
51  msgs::PointCloudPacked::Field::DataType)> initPointCloudPackedHelper =
52  [&](const std::string &_name,
53  msgs::PointCloudPacked::Field::DataType _type) -> void
54  {
55  msgs::PointCloudPacked::Field *newField = _msg.add_field();
56  newField->set_name(_name);
57  newField->set_count(1);
58  newField->set_datatype(_type);
59  newField->set_offset(offset);
60  switch (_type)
61  {
62  case msgs::PointCloudPacked::Field::INT8:
63  case msgs::PointCloudPacked::Field::UINT8:
64  offset += 1;
65  break;
66  case msgs::PointCloudPacked::Field::INT16:
67  case msgs::PointCloudPacked::Field::UINT16:
68  offset += 2;
69  break;
70  case msgs::PointCloudPacked::Field::INT32:
71  case msgs::PointCloudPacked::Field::UINT32:
72  case msgs::PointCloudPacked::Field::FLOAT32:
73  offset += 4;
74  break;
75  case msgs::PointCloudPacked::Field::FLOAT64:
76  offset += 8;
77  break;
78  // LCOV_EXCL_START
79  default:
80  std::cerr << "PointCloudPacked field datatype of ["
81  << _type << "] is invalid.\n";
82  break;
83  // LCOV_EXCL_STOP
84  }
85  };
86 
87  // Set the frame
88  _msg.mutable_header()->clear_data();
89  msgs::Header::Map *frame = _msg.mutable_header()->add_data();
90  frame->set_key("frame_id");
91  frame->add_value(_frameId);
92 
93  _msg.clear_field();
94  // Setup the point cloud message.
95  for (const std::pair<std::string,
96  msgs::PointCloudPacked::Field::DataType> &field : _fields)
97  {
98  if (field.first == "xyz")
99  {
100  initPointCloudPackedHelper("x", field.second);
101  initPointCloudPackedHelper("y", field.second);
102  initPointCloudPackedHelper("z", field.second);
103  }
104  else
105  {
106  initPointCloudPackedHelper(field.first, field.second);
107  }
108 
109  // Memory align the field.
110  if (_memoryAligned)
111  offset = math::roundUpMultiple(offset, sizeof(size_t));
112  }
113 
114  // Set the point_step
115  if (_memoryAligned)
116  _msg.set_point_step(math::roundUpMultiple(offset, sizeof(size_t)));
117  else
118  _msg.set_point_step(offset);
119 }
120 
150 template<typename FieldType>
152  : public PointCloudPackedIteratorBase<
153  FieldType, FieldType, char, PointCloudPacked, PointCloudPackedIterator>
154 {
155  // Documentation inherited
156  public: PointCloudPackedIterator(PointCloudPacked &_cloudMsg,
157  const std::string &_fieldName)
158  : PointCloudPackedIteratorBase<FieldType, FieldType, char,
159  PointCloudPacked, PointCloudPackedIterator>
160  ::PointCloudPackedIteratorBase(_cloudMsg, _fieldName)
161  {
162  }
163 };
164 
167 template<typename FieldType>
169  : public PointCloudPackedIteratorBase<
170  FieldType, const FieldType, const char, const PointCloudPacked,
171  PointCloudPackedConstIterator>
172 {
174  const PointCloudPacked &_cloudMsg,
175  const std::string &_fieldName)
176  : PointCloudPackedIteratorBase<FieldType, const FieldType, const char,
177  const PointCloudPacked,
179  >::PointCloudPackedIteratorBase(_cloudMsg, _fieldName)
180  {
181  }
182 };
183 
189 inline int sizeOfPointField(
190  PointCloudPacked::Field::DataType _dataType)
191 {
192  if ((_dataType == PointCloudPacked::Field::INT8) ||
193  (_dataType == PointCloudPacked::Field::UINT8))
194  {
195  return 1;
196  }
197  else if ((_dataType == PointCloudPacked::Field::INT16) ||
198  (_dataType == PointCloudPacked::Field::UINT16))
199  {
200  return 2;
201  }
202  else if ((_dataType == PointCloudPacked::Field::INT32) ||
203  (_dataType == PointCloudPacked::Field::UINT32) ||
204  (_dataType == PointCloudPacked::Field::FLOAT32))
205  {
206  return 4;
207  }
208  else if (_dataType == PointCloudPacked::Field::FLOAT64)
209  {
210  return 8;
211  }
212  else
213  {
214  std::cerr << "PointCloudPacked::Field of type [" << _dataType
215  << "] does not exist" << std::endl;
216  }
217  return -1;
218 }
219 }
220 }
221 
222 #endif