NASA Astrobee Robot Software  0.19.1
Flight software for the Astrobee robots operating inside the International Space Station.
pcl_conversions.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Open Source Robotics Foundation, Inc.
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation, Inc. nor
19  * the names of its contributors may be used to endorse or promote
20  * products derived from this software without specific prior
21  * written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
37 #ifndef MAPPER_PCL_CONVERSIONS_H_
38 #define MAPPER_PCL_CONVERSIONS_H_
39 
40 
41 #include <ros/ros.h>
42 
43 #include <pcl/conversions.h>
44 
45 #include <pcl/PCLHeader.h>
46 #include <std_msgs/Header.h>
47 
48 #include <pcl/PCLPointField.h>
49 #include <sensor_msgs/PointField.h>
50 
51 #include <pcl/PCLPointCloud2.h>
52 #include <sensor_msgs/PointCloud2.h>
53 
54 #include <Eigen/Geometry>
55 #include <Eigen/StdVector>
56 
57 #include <vector>
58 #include <string>
59 
60 // subset of https://github.com/ros-perception/pcl_conversion/
61 // copied due to reduced overhead?
62 
63 namespace pcl_conversions {
64 
67 inline void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp) {
68  stamp.fromNSec(pcl_stamp * 1000ull); // Convert from us to ns
69 }
70 
71 inline void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp) {
72  pcl_stamp = stamp.toNSec() / 1000ull; // Convert from ns to us
73 }
74 
75 inline ros::Time fromPCL(const pcl::uint64_t &pcl_stamp) {
76  ros::Time stamp;
77  fromPCL(pcl_stamp, stamp);
78  return stamp;
79 }
80 
81 inline pcl::uint64_t toPCL(const ros::Time &stamp) {
82  pcl::uint64_t pcl_stamp;
83  toPCL(stamp, pcl_stamp);
84  return pcl_stamp;
85 }
86 
89 inline void fromPCL(const pcl::PCLHeader &pcl_header,
90  std_msgs::Header &header) {
91  fromPCL(pcl_header.stamp, header.stamp);
92  header.seq = pcl_header.seq;
93  header.frame_id = pcl_header.frame_id;
94 }
95 
96 inline void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header) {
97  toPCL(header.stamp, pcl_header.stamp);
98  pcl_header.seq = header.seq;
99  pcl_header.frame_id = header.frame_id;
100 }
101 
102 inline std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header) {
103  std_msgs::Header header;
104  fromPCL(pcl_header, header);
105  return header;
106 }
107 
108 inline pcl::PCLHeader toPCL(const std_msgs::Header &header) {
109  pcl::PCLHeader pcl_header;
110  toPCL(header, pcl_header);
111  return pcl_header;
112 }
113 
116 inline void fromPCL(const pcl::PCLPointField &pcl_pf,
117  sensor_msgs::PointField &pf) {
118  pf.name = pcl_pf.name;
119  pf.offset = pcl_pf.offset;
120  pf.datatype = pcl_pf.datatype;
121  pf.count = pcl_pf.count;
122 }
123 
124 inline void fromPCL(const std::vector<pcl::PCLPointField> &pcl_pfs,
125  std::vector<sensor_msgs::PointField> &pfs) {
126  pfs.resize(pcl_pfs.size());
127  std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();
128  int i = 0;
129  for (; it != pcl_pfs.end(); ++it, ++i) {
130  fromPCL(*(it), pfs[i]);
131  }
132 }
133 
134 inline void toPCL(const sensor_msgs::PointField &pf,
135  pcl::PCLPointField &pcl_pf) {
136  pcl_pf.name = pf.name;
137  pcl_pf.offset = pf.offset;
138  pcl_pf.datatype = pf.datatype;
139  pcl_pf.count = pf.count;
140 }
141 
142 inline void toPCL(const std::vector<sensor_msgs::PointField> &pfs,
143  std::vector<pcl::PCLPointField> &pcl_pfs) {
144  pcl_pfs.resize(pfs.size());
145  std::vector<sensor_msgs::PointField>::const_iterator it = pfs.begin();
146  int i = 0;
147  for (; it != pfs.end(); ++it, ++i) {
148  toPCL(*(it), pcl_pfs[i]);
149  }
150 }
151 
154 inline void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2,
155  sensor_msgs::PointCloud2 &pc2) {
156  fromPCL(pcl_pc2.header, pc2.header);
157  pc2.height = pcl_pc2.height;
158  pc2.width = pcl_pc2.width;
159  fromPCL(pcl_pc2.fields, pc2.fields);
160  pc2.is_bigendian = pcl_pc2.is_bigendian;
161  pc2.point_step = pcl_pc2.point_step;
162  pc2.row_step = pcl_pc2.row_step;
163  pc2.is_dense = pcl_pc2.is_dense;
164 }
165 
166 inline void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2,
167  sensor_msgs::PointCloud2 &pc2) {
168  copyPCLPointCloud2MetaData(pcl_pc2, pc2);
169  pc2.data = pcl_pc2.data;
170 }
171 
172 inline void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2,
173  sensor_msgs::PointCloud2 &pc2) {
174  copyPCLPointCloud2MetaData(pcl_pc2, pc2);
175  pc2.data.swap(pcl_pc2.data);
176 }
177 
178 inline void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2,
179  pcl::PCLPointCloud2 &pcl_pc2) {
180  toPCL(pc2.header, pcl_pc2.header);
181  pcl_pc2.height = pc2.height;
182  pcl_pc2.width = pc2.width;
183  toPCL(pc2.fields, pcl_pc2.fields);
184  pcl_pc2.is_bigendian = pc2.is_bigendian;
185  pcl_pc2.point_step = pc2.point_step;
186  pcl_pc2.row_step = pc2.row_step;
187  pcl_pc2.is_dense = pc2.is_dense;
188 }
189 
190 inline void toPCL(const sensor_msgs::PointCloud2 &pc2,
191  pcl::PCLPointCloud2 &pcl_pc2) {
192  copyPointCloud2MetaData(pc2, pcl_pc2);
193  pcl_pc2.data = pc2.data;
194 }
195 
196 inline void moveToPCL(sensor_msgs::PointCloud2 &pc2,
197  pcl::PCLPointCloud2 &pcl_pc2) {
198  copyPointCloud2MetaData(pc2, pcl_pc2);
199  pcl_pc2.data.swap(pc2.data);
200 }
201 
202 } // namespace pcl_conversions
203 
204 namespace pcl {
205 
208 inline int getFieldIndex(const sensor_msgs::PointCloud2 &cloud,
209  const std::string &field_name) {
210  // Get the index we need
211  for (size_t d = 0; d < cloud.fields.size(); ++d) {
212  if (cloud.fields[d].name == field_name) {
213  return (static_cast<int>(d));
214  }
215  }
216  return (-1);
217 }
218 
221 inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud) {
222  std::string result;
223  for (size_t i = 0; i < cloud.fields.size() - 1; ++i) {
224  result += cloud.fields[i].name + " ";
225  }
226  result += cloud.fields[cloud.fields.size() - 1].name;
227  return (result);
228 }
229 
233 template <typename T>
234 void toROSMsg(const pcl::PointCloud<T> &pcl_cloud,
235  sensor_msgs::PointCloud2 &cloud) {
236  pcl::PCLPointCloud2 pcl_pc2;
237  pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
238  pcl_conversions::moveFromPCL(pcl_pc2, cloud);
239 }
240 
241 template <typename T>
242 void fromROSMsg(const sensor_msgs::PointCloud2 &cloud,
243  pcl::PointCloud<T> &pcl_cloud) {
244  pcl::PCLPointCloud2 pcl_pc2;
245  pcl_conversions::toPCL(cloud, pcl_pc2);
246  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
247 }
248 
249 template <typename T>
250 void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud,
251  pcl::PointCloud<T> &pcl_cloud) {
252  pcl::PCLPointCloud2 pcl_pc2;
253  pcl_conversions::moveToPCL(cloud, pcl_pc2);
254  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
255 }
256 
259 template <typename PointT>
260 void createMapping(const std::vector<sensor_msgs::PointField> &msg_fields,
261  MsgFieldMap &field_map) {
262  std::vector<pcl::PCLPointField> pcl_msg_fields;
263  pcl_conversions::toPCL(msg_fields, pcl_msg_fields);
264  return createMapping<PointT>(pcl_msg_fields, field_map);
265 }
266 
267 } // namespace pcl
268 
269 namespace ros {
270 template <>
271 struct DefaultMessageCreator<pcl::PCLPointCloud2> {
272  boost::shared_ptr<pcl::PCLPointCloud2> operator()() {
273  boost::shared_ptr<pcl::PCLPointCloud2> msg(new pcl::PCLPointCloud2());
274  return msg;
275  }
276 };
277 
278 namespace message_traits {
279 template <>
280 struct MD5Sum<pcl::PCLPointCloud2> {
281  static const char *value() {
282  return MD5Sum<sensor_msgs::PointCloud2>::value();
283  }
284  static const char *value(const pcl::PCLPointCloud2 &) { return value(); }
285 
286  static const uint64_t static_value1 =
287  MD5Sum<sensor_msgs::PointCloud2>::static_value1;
288  static const uint64_t static_value2 =
289  MD5Sum<sensor_msgs::PointCloud2>::static_value2;
290 
291  // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile
292  // error here.
293  ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
294  ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
295 };
296 
297 template <>
298 struct DataType<pcl::PCLPointCloud2> {
299  static const char *value() {
300  return DataType<sensor_msgs::PointCloud2>::value();
301  }
302  static const char *value(const pcl::PCLPointCloud2 &) { return value(); }
303 };
304 
305 template <>
306 struct Definition<pcl::PCLPointCloud2> {
307  static const char *value() {
308  return Definition<sensor_msgs::PointCloud2>::value();
309  }
310  static const char *value(const pcl::PCLPointCloud2 &) { return value(); }
311 };
312 
313 template <>
314 struct HasHeader<pcl::PCLPointCloud2> : TrueType {};
315 } // namespace message_traits
316 
317 namespace serialization {
318 /*
319  * Provide a custom serialization for pcl::PCLPointCloud2
320  */
321 template <>
322 struct Serializer<pcl::PCLPointCloud2> {
323  template <typename Stream>
324  inline static void write(Stream &stream, const pcl::PCLPointCloud2 &m) {
325  std_msgs::Header header;
326  pcl_conversions::fromPCL(m.header, header);
327  stream.next(header);
328  stream.next(m.height);
329  stream.next(m.width);
330  std::vector<sensor_msgs::PointField> pfs;
331  pcl_conversions::fromPCL(m.fields, pfs);
332  stream.next(pfs);
333  stream.next(m.is_bigendian);
334  stream.next(m.point_step);
335  stream.next(m.row_step);
336  stream.next(m.data);
337  stream.next(m.is_dense);
338  }
339 
340  template <typename Stream>
341  inline static void read(Stream &stream, pcl::PCLPointCloud2 &m) {
342  std_msgs::Header header;
343  stream.next(header);
344  pcl_conversions::toPCL(header, m.header);
345  stream.next(m.height);
346  stream.next(m.width);
347  std::vector<sensor_msgs::PointField> pfs;
348  stream.next(pfs);
349  pcl_conversions::toPCL(pfs, m.fields);
350  stream.next(m.is_bigendian);
351  stream.next(m.point_step);
352  stream.next(m.row_step);
353  stream.next(m.data);
354  stream.next(m.is_dense);
355  }
356 
357  inline static uint32_t serializedLength(const pcl::PCLPointCloud2 &m) {
358  uint32_t length = 0;
359 
360  std_msgs::Header header;
361  pcl_conversions::fromPCL(m.header, header);
362  length += serializationLength(header);
363  length += 4; // height
364  length += 4; // width
365  std::vector<sensor_msgs::PointField> pfs;
366  pcl_conversions::fromPCL(m.fields, pfs);
367  length += serializationLength(pfs); // fields
368  length += 1; // is_bigendian
369  length += 4; // point_step
370  length += 4; // row_step
371  length += 4; // data's size
372  length += m.data.size() * sizeof(pcl::uint8_t);
373  length += 1; // is_dense
374 
375  return length;
376  }
377 };
378 
379 /*
380  * Provide a custom serialization for pcl::PCLPointField
381  */
382 template <>
383 struct Serializer<pcl::PCLPointField> {
384  template <typename Stream>
385  inline static void write(Stream &stream, const pcl::PCLPointField &m) {
386  stream.next(m.name);
387  stream.next(m.offset);
388  stream.next(m.datatype);
389  stream.next(m.count);
390  }
391 
392  template <typename Stream>
393  inline static void read(Stream &stream, pcl::PCLPointField &m) {
394  stream.next(m.name);
395  stream.next(m.offset);
396  stream.next(m.datatype);
397  stream.next(m.count);
398  }
399 
400  inline static uint32_t serializedLength(const pcl::PCLPointField &m) {
401  uint32_t length = 0;
402 
403  length += serializationLength(m.name);
404  length += serializationLength(m.offset);
405  length += serializationLength(m.datatype);
406  length += serializationLength(m.count);
407 
408  return length;
409  }
410 };
411 
412 /*
413  * Provide a custom serialization for pcl::PCLHeader
414  */
415 template <>
416 struct Serializer<pcl::PCLHeader> {
417  template <typename Stream>
418  inline static void write(Stream &stream, const pcl::PCLHeader &m) {
419  std_msgs::Header header;
420  pcl_conversions::fromPCL(m, header);
421  stream.next(header);
422  }
423 
424  template <typename Stream>
425  inline static void read(Stream &stream, pcl::PCLHeader &m) {
426  std_msgs::Header header;
427  stream.next(header);
428  pcl_conversions::toPCL(header, m);
429  }
430 
431  inline static uint32_t serializedLength(const pcl::PCLHeader &m) {
432  uint32_t length = 0;
433 
434  std_msgs::Header header;
435  pcl_conversions::fromPCL(m, header);
436  length += serializationLength(header);
437 
438  return length;
439  }
440 };
441 } // namespace serialization
442 
443 } // namespace ros
444 
445 #endif // MAPPER_PCL_CONVERSIONS_H_
pcl::createMapping
void createMapping(const std::vector< sensor_msgs::PointField > &msg_fields, MsgFieldMap &field_map)
Definition: pcl_conversions.h:260
pcl
Definition: correspondence_rejection_surface_normal2.h:45
pcl_conversions::moveToPCL
void moveToPCL(sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
Definition: pcl_conversions.h:196
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
Definition: pcl_conversions.h:172
pcl::moveFromROSMsg
void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
Definition: pcl_conversions.h:250
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
Definition: pcl_conversions.h:221
ros::serialization::Serializer< pcl::PCLPointCloud2 >::read
static void read(Stream &stream, pcl::PCLPointCloud2 &m)
Definition: pcl_conversions.h:341
ros::serialization::Serializer< pcl::PCLPointField >::serializedLength
static uint32_t serializedLength(const pcl::PCLPointField &m)
Definition: pcl_conversions.h:400
pcl_conversions
Definition: pcl_conversions.h:63
ros
Definition: pcl_conversions.h:269
ros::serialization::Serializer< pcl::PCLPointCloud2 >::write
static void write(Stream &stream, const pcl::PCLPointCloud2 &m)
Definition: pcl_conversions.h:324
ros::DefaultMessageCreator< pcl::PCLPointCloud2 >::operator()
boost::shared_ptr< pcl::PCLPointCloud2 > operator()()
Definition: pcl_conversions.h:272
ros::message_traits::Definition< pcl::PCLPointCloud2 >::value
static const char * value()
Definition: pcl_conversions.h:307
ros::message_traits::DataType< pcl::PCLPointCloud2 >::value
static const char * value()
Definition: pcl_conversions.h:299
ros::serialization::Serializer< pcl::PCLHeader >::serializedLength
static uint32_t serializedLength(const pcl::PCLHeader &m)
Definition: pcl_conversions.h:431
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
Definition: pcl_conversions.h:242
ros::message_traits::Definition< pcl::PCLPointCloud2 >::value
static const char * value(const pcl::PCLPointCloud2 &)
Definition: pcl_conversions.h:310
pcl_conversions::fromPCL
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
Definition: pcl_conversions.h:67
ros::serialization::Serializer< pcl::PCLPointField >::write
static void write(Stream &stream, const pcl::PCLPointField &m)
Definition: pcl_conversions.h:385
ros::message_traits::DataType< pcl::PCLPointCloud2 >::value
static const char * value(const pcl::PCLPointCloud2 &)
Definition: pcl_conversions.h:302
ros::message_traits::MD5Sum< pcl::PCLPointCloud2 >::value
static const char * value(const pcl::PCLPointCloud2 &)
Definition: pcl_conversions.h:284
pcl_conversions::toPCL
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
Definition: pcl_conversions.h:71
pcl::toROSMsg
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
Definition: pcl_conversions.h:234
ros::serialization::Serializer< pcl::PCLPointCloud2 >::serializedLength
static uint32_t serializedLength(const pcl::PCLPointCloud2 &m)
Definition: pcl_conversions.h:357
ros::serialization::Serializer< pcl::PCLPointField >::read
static void read(Stream &stream, pcl::PCLPointField &m)
Definition: pcl_conversions.h:393
pcl_conversions::copyPCLPointCloud2MetaData
void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
Definition: pcl_conversions.h:154
pcl_conversions::copyPointCloud2MetaData
void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
Definition: pcl_conversions.h:178
ros::serialization::Serializer< pcl::PCLHeader >::write
static void write(Stream &stream, const pcl::PCLHeader &m)
Definition: pcl_conversions.h:418
ros::serialization::Serializer< pcl::PCLHeader >::read
static void read(Stream &stream, pcl::PCLHeader &m)
Definition: pcl_conversions.h:425
localization_common::Time
double Time
Definition: time.h:23
ros::message_traits::MD5Sum< pcl::PCLPointCloud2 >::value
static const char * value()
Definition: pcl_conversions.h:281
pcl::getFieldIndex
int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
Definition: pcl_conversions.h:208