43 #pragma GCC system_header
46 #include <pcl/PCLPointField.h>
47 #include <pcl/PCLPointCloud2.h>
48 #include <pcl/PCLImage.h>
49 #include <pcl/point_cloud.h>
50 #include <pcl/type_traits.h>
51 #include <pcl/for_each_type.h>
52 #include <pcl/exceptions.h>
53 #include <pcl/console/print.h>
55 #include <boost/foreach.hpp>
63 template<
typename Po
intT>
71 f.
name = traits::name<PointT, U>::value;
72 f.
offset = traits::offset<PointT, U>::value;
73 f.
datatype = traits::datatype<PointT, U>::value;
74 f.
count = traits::datatype<PointT, U>::size;
82 template<
typename Po
intT>
86 std::vector<FieldMapping>& map)
91 template<
typename Tag>
void
94 for (
const auto& field :
fields_)
101 mapping.
size =
sizeof (
typename traits::datatype<PointT, Tag>::type);
102 map_.push_back (mapping);
107 PCL_WARN (
"Failed to find match for field '%s'.\n", traits::name<PointT, Tag>::value);
111 const std::vector<pcl::PCLPointField>&
fields_;
112 std::vector<FieldMapping>&
map_;
123 template<
typename Po
intT>
void
128 for_each_type< typename traits::fieldList<PointT>::type > (mapper);
131 if (field_map.size() > 1)
134 MsgFieldMap::iterator i = field_map.begin(), j = i + 1;
135 while (j != field_map.end())
140 if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset)
142 i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size);
143 j = field_map.erase(j);
167 template <
typename Po
intT>
void
179 cloud.
points.resize (num_points);
185 if (field_map.size() == 1 &&
186 field_map[0].serialized_offset == 0 &&
187 field_map[0].struct_offset == 0 &&
189 field_map[0].size ==
sizeof(
PointT))
196 memcpy (cloud_data, msg_data, msg.
data.size ());
201 memcpy (cloud_data, msg_data, cloud_row_step);
216 memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size);
218 cloud_data +=
sizeof (
PointT);
228 template<
typename Po
intT>
void
232 createMapping<PointT> (msg.
fields, field_map);
240 template<
typename Po
intT>
void
257 std::size_t data_size =
sizeof (
PointT) * cloud.
size ();
258 msg.
data.resize (data_size);
261 memcpy(&msg.
data[0], &cloud[0], data_size);
281 template<
typename CloudT>
void
285 if (cloud.width == 0 && cloud.height == 0)
286 throw std::runtime_error(
"Needs to be a dense like cloud!!");
289 if (cloud.size () != cloud.width * cloud.height)
290 throw std::runtime_error(
"The width and height do not match the cloud size!");
291 msg.
height = cloud.height;
292 msg.
width = cloud.width;
299 for (std::size_t y = 0; y < cloud.height; y++)
301 for (std::size_t x = 0; x < cloud.width; x++)
304 memcpy (pixel, &cloud (x, y).rgb, 3 *
sizeof(
std::uint8_t));
317 const auto predicate = [](
const auto& field) {
return field.name ==
"rgb"; };
318 const auto result = std::find_if(cloud.
fields.cbegin (), cloud.
fields.cend (), predicate);
319 if (result == cloud.
fields.end ())
320 throw std::runtime_error (
"No rgb field!!");
324 throw std::runtime_error (
"Needs to be a dense like cloud!!");
330 int rgb_offset = cloud.
fields[rgb_index].offset;
338 for (std::size_t y = 0; y < cloud.
height; y++)
340 for (std::size_t x = 0; x < cloud.
width; x++, rgb_offset += point_step)