cloud.hpp
Go to the documentation of this file.
1 #pragma once
2 
11 #include <string>
12 
13 #include <sensor_msgs/PointCloud2.h>
14 #include <sensor_msgs/PointField.h>
16 
17 #include "cloud/impl/cloud.hpp"
18 
19 namespace cras
20 {
21 
23 typedef ::sensor_msgs::PointCloud2 Cloud;
24 
26 typedef ::sensor_msgs::PointCloud2Iterator<float> CloudIter;
27 
29 typedef ::sensor_msgs::PointCloud2Iterator<int> CloudIndexIter;
30 
32 typedef ::sensor_msgs::PointCloud2ConstIterator<float> CloudConstIter;
33 
35 typedef ::sensor_msgs::PointCloud2ConstIterator<int> CloudIndexConstIter;
36 
38 typedef ::sensor_msgs::PointCloud2Modifier CloudModifier;
39 
58 typedef ::cras::impl::GenericCloudIterator<> GenericCloudIter;
59 
61 typedef ::cras::impl::GenericCloudConstIterator<> GenericCloudConstIter;
62 
68 inline size_t numPoints(const ::cras::Cloud& cloud)
69 {
70  return static_cast<size_t>(cloud.height) * static_cast<size_t>(cloud.width);
71 }
72 
79 bool hasField(const ::cras::Cloud& cloud, const ::std::string& fieldName);
80 
88 ::sensor_msgs::PointField& getField(::cras::Cloud& cloud, const ::std::string& fieldName);
89 
97 const ::sensor_msgs::PointField& getField(const ::cras::Cloud& cloud, const ::std::string& fieldName);
98 
105 size_t sizeOfPointField(int datatype);
106 
113 size_t sizeOfPointField(const ::sensor_msgs::PointField& field);
114 
124 void copyChannelData(const ::cras::Cloud& in, ::cras::Cloud& out, const ::std::string& fieldName);
125 
132 #define CREATE_FILTERED_CLOUD(IN, OUT, KEEP_ORGANIZED, FILTER) {\
133  const auto inputIsOrganized = (IN).height > 1; \
134  const auto outIsOrganized = (KEEP_ORGANIZED) && inputIsOrganized; \
135  \
136  (OUT).header = (IN).header; \
137  (OUT).fields = (IN).fields; \
138  (OUT).point_step = (IN).point_step; \
139  (OUT).height = outIsOrganized ? (IN).height : 1; \
140  (OUT).width = outIsOrganized ? (IN).width : 0; \
141  \
142  (OUT).data.resize(0); \
143  (OUT).data.reserve((IN).data.size()); \
144  \
145  ::cras::CloudConstIter x_it((IN), "x"); \
146  ::cras::CloudConstIter y_it((IN), "y"); \
147  ::cras::CloudConstIter z_it((IN), "z"); \
148  \
149  const auto numPoints = ::cras::numPoints(IN); \
150  \
151  if (!outIsOrganized) { \
152  for (size_t i = 0; i < numPoints; ++i, ++x_it, ++y_it, ++z_it) { \
153  if (FILTER) { \
154  size_t from = (i / (IN).width) * (IN).row_step + (i % (IN).width) * (IN).point_step; \
155  size_t to = from + (IN).point_step; \
156  (OUT).data.insert((OUT).data.end(), (IN).data.begin() + from, \
157  (IN).data.begin() + to); \
158  (OUT).width++; \
159  } \
160  } \
161  (OUT).is_dense = true; \
162  } else { \
163  (OUT).data.insert((OUT).data.end(), (IN).data.begin(), (IN).data.end()); \
164  \
165  ::cras::CloudIter x_out_it((OUT), "x"); \
166  ::cras::CloudIter y_out_it((OUT), "y"); \
167  ::cras::CloudIter z_out_it((OUT), "z"); \
168  const auto invalidValue = std::numeric_limits<float>::quiet_NaN(); \
169  \
170  for (size_t i = 0; i < numPoints; ++i, ++x_it, ++y_it, ++z_it, ++x_out_it, ++y_out_it, ++z_out_it) { \
171  if (!(FILTER)) { \
172  *x_out_it = *y_out_it = *z_out_it = invalidValue; \
173  (OUT).is_dense = false; \
174  } \
175  } \
176  } \
177  \
178  (OUT).row_step = (OUT).width * (OUT).point_step;\
179 }
180 }
void copyChannelData(const ::cras::Cloud &in, ::cras::Cloud &out, const ::std::string &fieldName)
Copy data belonging to the given field from in cloud to out cloud.
::sensor_msgs::PointField & getField(::cras::Cloud &cloud, const ::std::string &fieldName)
Return the sensor_msgs::PointField with the given name.
::sensor_msgs::PointCloud2Iterator< float > CloudIter
Cloud float field iterator.
Definition: cloud.hpp:26
::sensor_msgs::PointCloud2Modifier CloudModifier
Shorthand for sensor_msgs::PointCloud2Modifier.
Definition: cloud.hpp:38
size_t sizeOfPointField(int datatype)
Return the size (in bytes) of a sensor_msgs::PointField datatype.
::cras::impl::GenericCloudIterator GenericCloudIter
GenericCloudIter and GenericCloudConstIter are iterators of fields of types unknown at compile time...
Definition: cloud.hpp:58
const char * datatype()
::sensor_msgs::PointCloud2 Cloud
Shorthand for sensor_msgs::PointCloud2.
Definition: cloud.hpp:23
::sensor_msgs::PointCloud2ConstIterator< int > CloudIndexConstIter
Const cloud int field iterator.
Definition: cloud.hpp:35
size_t numPoints(const ::cras::Cloud &cloud)
Return the number of points the given pointcloud contains.
Definition: cloud.hpp:68
Definition: any.hpp:15
::sensor_msgs::PointCloud2ConstIterator< float > CloudConstIter
Const cloud float field iterator.
Definition: cloud.hpp:32
bool hasField(const ::cras::Cloud &cloud, const ::std::string &fieldName)
Return true if the cloud contains a field with the given name.
::cras::impl::GenericCloudConstIterator GenericCloudConstIter
Definition: cloud.hpp:61
::sensor_msgs::PointCloud2Iterator< int > CloudIndexIter
Cloud int field iterator.
Definition: cloud.hpp:29


cras_cpp_common
Author(s): Martin Pecka
autogenerated on Sat Jun 17 2023 02:32:53