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/console/print.h>
54#include <boost/foreach.hpp>
61 template<
typename Po
intT>
69 f.
name = pcl::traits::name<PointT, U>::value;
70 f.
offset = pcl::traits::offset<PointT, U>::value;
71 f.
datatype = pcl::traits::datatype<PointT, U>::value;
72 f.
count = pcl::traits::datatype<PointT, U>::size;
80 template<
typename Po
intT>
84 std::vector<FieldMapping>& map)
89 template<
typename Tag>
void
98 mapping.struct_offset = pcl::traits::offset<PointT, Tag>::value;
99 mapping.
size =
sizeof (
typename pcl::traits::datatype<PointT, Tag>::type);
105 PCL_WARN (
"Failed to find match for field '%s'.\n", pcl::traits::name<PointT, Tag>::value);
109 const std::vector<pcl::PCLPointField>&
fields_;
110 std::vector<FieldMapping>&
map_;
121 template<
typename Po
intT>
void
132 MsgFieldMap::iterator i =
field_map.begin(), j = i + 1;
138 if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset)
140 i->
size += (j->struct_offset + j->size) - (i->struct_offset + i->size);
165 template <
typename Po
intT>
void
170 cloud.header =
msg.header;
171 cloud.width =
msg.width;
172 cloud.height =
msg.height;
173 cloud.is_dense =
msg.is_dense == 1;
178 std::uint8_t*
cloud_data =
reinterpret_cast<std::uint8_t*
>(&cloud[0]);
226 template<
typename Po
intT>
void
238 template<
typename Po
intT>
void
242 if (cloud.width == 0 && cloud.height == 0)
249 assert (cloud.
size () == cloud.width * cloud.height);
250 msg.height = cloud.height;
251 msg.width = cloud.width;
266 msg.header = cloud.header;
268 msg.row_step =
static_cast<std::uint32_t
> (
sizeof (
PointT) *
msg.width);
269 msg.is_dense = cloud.is_dense;
279 template<
typename CloudT>
void
283 if (cloud.width == 0 && cloud.height == 0)
284 throw std::runtime_error(
"Needs to be a dense like cloud!!");
287 if (cloud.
size () != cloud.width * cloud.height)
288 throw std::runtime_error(
"The width and height do not match the cloud size!");
289 msg.height = cloud.height;
290 msg.width = cloud.width;
294 msg.header = cloud.header;
295 msg.encoding =
"bgr8";
296 msg.step =
msg.width *
sizeof (std::uint8_t) * 3;
298 for (std::size_t y = 0; y < cloud.height; y++)
300 for (std::size_t x = 0; x < cloud.width; x++)
302 std::uint8_t *
pixel = &(
msg.data[y *
msg.step + x * 3]);
319 throw std::runtime_error (
"No rgb field!!");
323 throw std::runtime_error (
"Needs to be a dense like cloud!!");
334 msg.encoding =
"bgr8";
335 msg.step =
static_cast<std::uint32_t
>(
msg.width *
sizeof (std::uint8_t) * 3);
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)
342 std::uint8_t *
pixel = &(
msg.data[y *
msg.step + x * 3]);
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
bool fieldOrdering(const FieldMapping &a, const FieldMapping &b)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
void createMapping(const std::vector< pcl::PCLPointField > &msg_fields, MsgFieldMap &field_map)
std::vector< detail::FieldMapping > MsgFieldMap
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
std::vector<::pcl::PCLPointField > fields
std::vector< std::uint8_t > data
A point structure representing Euclidean xyz coordinates, and the RGB color.
FieldAdder(std::vector< pcl::PCLPointField > &fields)
std::vector< pcl::PCLPointField > & fields_
FieldMapper(const std::vector< pcl::PCLPointField > &fields, std::vector< FieldMapping > &map)
const std::vector< pcl::PCLPointField > & fields_
std::vector< FieldMapping > & map_
std::size_t serialized_offset