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 PCL_CONVERSIONS_H__
38 #define PCL_CONVERSIONS_H__
39 
40 #include <vector>
41 
42 #include <ros/ros.h>
43 
44 #include <pcl/conversions.h>
45 
46 #include <pcl/PCLHeader.h>
47 #include <std_msgs/Header.h>
48 
49 #include <pcl/PCLImage.h>
50 #include <sensor_msgs/Image.h>
51 
52 #include <pcl/PCLPointField.h>
53 #include <sensor_msgs/PointField.h>
54 
55 #include <pcl/PCLPointCloud2.h>
56 #include <sensor_msgs/PointCloud2.h>
57 
58 #include <pcl/PointIndices.h>
59 #include <pcl_msgs/PointIndices.h>
60 
61 #include <pcl/ModelCoefficients.h>
62 #include <pcl_msgs/ModelCoefficients.h>
63 
64 #include <pcl/Vertices.h>
65 #include <pcl_msgs/Vertices.h>
66 
67 #include <pcl/PolygonMesh.h>
68 #include <pcl_msgs/PolygonMesh.h>
69 
70 #include <pcl/io/pcd_io.h>
71 
72 #include <Eigen/StdVector>
73 #include <Eigen/Geometry>
74 
75 namespace pcl_conversions {
76 
79  inline
80  void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
81  {
82  stamp.fromNSec(pcl_stamp * 1000ull); // Convert from us to ns
83  }
84 
85  inline
86  void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp)
87  {
88  pcl_stamp = stamp.toNSec() / 1000ull; // Convert from ns to us
89  }
90 
91  inline
92  ros::Time fromPCL(const std::uint64_t &pcl_stamp)
93  {
94  ros::Time stamp;
95  fromPCL(pcl_stamp, stamp);
96  return stamp;
97  }
98 
99  inline
100  std::uint64_t toPCL(const ros::Time &stamp)
101  {
102  std::uint64_t pcl_stamp;
103  toPCL(stamp, pcl_stamp);
104  return pcl_stamp;
105  }
106 
109  inline
110  void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header)
111  {
112  fromPCL(pcl_header.stamp, header.stamp);
113  header.seq = pcl_header.seq;
114  header.frame_id = pcl_header.frame_id;
115  }
116 
117  inline
118  void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header)
119  {
120  toPCL(header.stamp, pcl_header.stamp);
121  pcl_header.seq = header.seq;
122  pcl_header.frame_id = header.frame_id;
123  }
124 
125  inline
126  std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
127  {
128  std_msgs::Header header;
129  fromPCL(pcl_header, header);
130  return header;
131  }
132 
133  inline
134  pcl::PCLHeader toPCL(const std_msgs::Header &header)
135  {
136  pcl::PCLHeader pcl_header;
137  toPCL(header, pcl_header);
138  return pcl_header;
139  }
140 
143  inline
144  void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
145  {
146  fromPCL(pcl_image.header, image.header);
147  image.height = pcl_image.height;
148  image.width = pcl_image.width;
149  image.encoding = pcl_image.encoding;
150  image.is_bigendian = pcl_image.is_bigendian;
151  image.step = pcl_image.step;
152  }
153 
154  inline
155  void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
156  {
157  copyPCLImageMetaData(pcl_image, image);
158  image.data = pcl_image.data;
159  }
160 
161  inline
162  void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
163  {
164  copyPCLImageMetaData(pcl_image, image);
165  image.data.swap(pcl_image.data);
166  }
167 
168  inline
169  void copyImageMetaData(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
170  {
171  toPCL(image.header, pcl_image.header);
172  pcl_image.height = image.height;
173  pcl_image.width = image.width;
174  pcl_image.encoding = image.encoding;
175  pcl_image.is_bigendian = image.is_bigendian;
176  pcl_image.step = image.step;
177  }
178 
179  inline
180  void toPCL(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
181  {
182  copyImageMetaData(image, pcl_image);
183  pcl_image.data = image.data;
184  }
185 
186  inline
187  void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
188  {
189  copyImageMetaData(image, pcl_image);
190  pcl_image.data.swap(image.data);
191  }
192 
195  inline
196  void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf)
197  {
198  pf.name = pcl_pf.name;
199  pf.offset = pcl_pf.offset;
200  pf.datatype = pcl_pf.datatype;
201  pf.count = pcl_pf.count;
202  }
203 
204  inline
205  void fromPCL(const std::vector<pcl::PCLPointField> &pcl_pfs, std::vector<sensor_msgs::PointField> &pfs)
206  {
207  pfs.resize(pcl_pfs.size());
208  std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();
209  int i = 0;
210  for(; it != pcl_pfs.end(); ++it, ++i) {
211  fromPCL(*(it), pfs[i]);
212  }
213  }
214 
215  inline
216  void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf)
217  {
218  pcl_pf.name = pf.name;
219  pcl_pf.offset = pf.offset;
220  pcl_pf.datatype = pf.datatype;
221  pcl_pf.count = pf.count;
222  }
223 
224  inline
225  void toPCL(const std::vector<sensor_msgs::PointField> &pfs, std::vector<pcl::PCLPointField> &pcl_pfs)
226  {
227  pcl_pfs.resize(pfs.size());
228  std::vector<sensor_msgs::PointField>::const_iterator it = pfs.begin();
229  int i = 0;
230  for(; it != pfs.end(); ++it, ++i) {
231  toPCL(*(it), pcl_pfs[i]);
232  }
233  }
234 
237  inline
238  void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
239  {
240  fromPCL(pcl_pc2.header, pc2.header);
241  pc2.height = pcl_pc2.height;
242  pc2.width = pcl_pc2.width;
243  fromPCL(pcl_pc2.fields, pc2.fields);
244  pc2.is_bigendian = pcl_pc2.is_bigendian;
245  pc2.point_step = pcl_pc2.point_step;
246  pc2.row_step = pcl_pc2.row_step;
247  pc2.is_dense = pcl_pc2.is_dense;
248  }
249 
250  inline
251  void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
252  {
253  copyPCLPointCloud2MetaData(pcl_pc2, pc2);
254  pc2.data = pcl_pc2.data;
255  }
256 
257  inline
258  void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
259  {
260  copyPCLPointCloud2MetaData(pcl_pc2, pc2);
261  pc2.data.swap(pcl_pc2.data);
262  }
263 
264  inline
265  void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
266  {
267  toPCL(pc2.header, pcl_pc2.header);
268  pcl_pc2.height = pc2.height;
269  pcl_pc2.width = pc2.width;
270  toPCL(pc2.fields, pcl_pc2.fields);
271  pcl_pc2.is_bigendian = pc2.is_bigendian;
272  pcl_pc2.point_step = pc2.point_step;
273  pcl_pc2.row_step = pc2.row_step;
274  pcl_pc2.is_dense = pc2.is_dense;
275  }
276 
277  inline
278  void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
279  {
280  copyPointCloud2MetaData(pc2, pcl_pc2);
281  pcl_pc2.data = pc2.data;
282  }
283 
284  inline
285  void moveToPCL(sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
286  {
287  copyPointCloud2MetaData(pc2, pcl_pc2);
288  pcl_pc2.data.swap(pc2.data);
289  }
290 
293  inline
294  void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi)
295  {
296  fromPCL(pcl_pi.header, pi.header);
297  pi.indices = pcl_pi.indices;
298  }
299 
300  inline
301  void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi)
302  {
303  fromPCL(pcl_pi.header, pi.header);
304  pi.indices.swap(pcl_pi.indices);
305  }
306 
307  inline
308  void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi)
309  {
310  toPCL(pi.header, pcl_pi.header);
311  pcl_pi.indices = pi.indices;
312  }
313 
314  inline
315  void moveToPCL(pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi)
316  {
317  toPCL(pi.header, pcl_pi.header);
318  pcl_pi.indices.swap(pi.indices);
319  }
320 
323  inline
324  void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
325  {
326  fromPCL(pcl_mc.header, mc.header);
327  mc.values = pcl_mc.values;
328  }
329 
330  inline
331  void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
332  {
333  fromPCL(pcl_mc.header, mc.header);
334  mc.values.swap(pcl_mc.values);
335  }
336 
337  inline
338  void toPCL(const pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
339  {
340  toPCL(mc.header, pcl_mc.header);
341  pcl_mc.values = mc.values;
342  }
343 
344  inline
345  void moveToPCL(pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
346  {
347  toPCL(mc.header, pcl_mc.header);
348  pcl_mc.values.swap(mc.values);
349  }
350 
353  namespace internal
354  {
355  template <class T>
356  inline void move(std::vector<T> &a, std::vector<T> &b)
357  {
358  b.swap(a);
359  }
360 
361  template <class T1, class T2>
362  inline void move(std::vector<T1> &a, std::vector<T2> &b)
363  {
364  b.assign(a.cbegin(), a.cend());
365  }
366  }
367 
368  inline
369  void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert)
370  {
371  vert.vertices.assign(pcl_vert.vertices.cbegin(), pcl_vert.vertices.cend());
372  }
373 
374  inline
375  void fromPCL(const std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::Vertices> &verts)
376  {
377  verts.resize(pcl_verts.size());
378  std::vector<pcl::Vertices>::const_iterator it = pcl_verts.begin();
379  std::vector<pcl_msgs::Vertices>::iterator jt = verts.begin();
380  for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
381  fromPCL(*(it), *(jt));
382  }
383  }
384 
385  inline
386  void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert)
387  {
388  internal::move(pcl_vert.vertices, vert.vertices);
389  }
390 
391  inline
392  void fromPCL(std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::Vertices> &verts)
393  {
394  verts.resize(pcl_verts.size());
395  std::vector<pcl::Vertices>::iterator it = pcl_verts.begin();
396  std::vector<pcl_msgs::Vertices>::iterator jt = verts.begin();
397  for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
398  moveFromPCL(*(it), *(jt));
399  }
400  }
401 
402  inline
403  void toPCL(const pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert)
404  {
405  pcl_vert.vertices.assign(vert.vertices.cbegin(), vert.vertices.cend());
406  }
407 
408  inline
409  void toPCL(const std::vector<pcl_msgs::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
410  {
411  pcl_verts.resize(verts.size());
412  std::vector<pcl_msgs::Vertices>::const_iterator it = verts.begin();
413  std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
414  for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
415  toPCL(*(it), *(jt));
416  }
417  }
418 
419  inline
420  void moveToPCL(pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert)
421  {
422  internal::move(vert.vertices, pcl_vert.vertices);
423  }
424 
425  inline
426  void moveToPCL(std::vector<pcl_msgs::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
427  {
428  pcl_verts.resize(verts.size());
429  std::vector<pcl_msgs::Vertices>::iterator it = verts.begin();
430  std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
431  for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
432  moveToPCL(*(it), *(jt));
433  }
434  }
435 
438  inline
439  void fromPCL(const pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh)
440  {
441  fromPCL(pcl_mesh.header, mesh.header);
442  fromPCL(pcl_mesh.cloud, mesh.cloud);
443  fromPCL(pcl_mesh.polygons, mesh.polygons);
444  }
445 
446  inline
447  void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh)
448  {
449  fromPCL(pcl_mesh.header, mesh.header);
450  moveFromPCL(pcl_mesh.cloud, mesh.cloud);
451  }
452 
453  inline
454  void toPCL(const pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
455  {
456  toPCL(mesh.header, pcl_mesh.header);
457  toPCL(mesh.cloud, pcl_mesh.cloud);
458  toPCL(mesh.polygons, pcl_mesh.polygons);
459  }
460 
461  inline
462  void moveToPCL(pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
463  {
464  toPCL(mesh.header, pcl_mesh.header);
465  moveToPCL(mesh.cloud, pcl_mesh.cloud);
466  moveToPCL(mesh.polygons, pcl_mesh.polygons);
467  }
468 
469 } // namespace pcl_conversions
470 
471 namespace pcl {
472 
475  inline int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
476  {
477  // Get the index we need
478  for (size_t d = 0; d < cloud.fields.size(); ++d) {
479  if (cloud.fields[d].name == field_name) {
480  return (static_cast<int>(d));
481  }
482  }
483  return (-1);
484  }
485 
488  inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
489  {
490  std::string result;
491  for (size_t i = 0; i < cloud.fields.size () - 1; ++i) {
492  result += cloud.fields[i].name + " ";
493  }
494  result += cloud.fields[cloud.fields.size () - 1].name;
495  return (result);
496  }
497 
500  inline
501  void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
502  {
503  pcl::PCLPointCloud2 pcl_cloud;
504  pcl_conversions::toPCL(cloud, pcl_cloud);
505  pcl::PCLImage pcl_image;
506  pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
507  pcl_conversions::moveFromPCL(pcl_image, image);
508  }
509 
510  inline
511  void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
512  {
513  pcl::PCLPointCloud2 pcl_cloud;
514  pcl_conversions::moveToPCL(cloud, pcl_cloud);
515  pcl::PCLImage pcl_image;
516  pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
517  pcl_conversions::moveFromPCL(pcl_image, image);
518  }
519 
520  template<typename T> void
521  toROSMsg (const pcl::PointCloud<T> &cloud, sensor_msgs::Image& msg)
522  {
523  // Ease the user's burden on specifying width/height for unorganized datasets
524  if (cloud.width == 0 && cloud.height == 0)
525  {
526  throw std::runtime_error("Needs to be a dense like cloud!!");
527  }
528  else
529  {
530  if (cloud.points.size () != cloud.width * cloud.height)
531  throw std::runtime_error("The width and height do not match the cloud size!");
532  msg.height = cloud.height;
533  msg.width = cloud.width;
534  }
535 
536  // sensor_msgs::image_encodings::BGR8;
537  msg.encoding = "bgr8";
538  msg.step = msg.width * sizeof (std::uint8_t) * 3;
539  msg.data.resize (msg.step * msg.height);
540  for (size_t y = 0; y < cloud.height; y++)
541  {
542  for (size_t x = 0; x < cloud.width; x++)
543  {
544  std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
545  memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(std::uint8_t));
546  }
547  }
548  }
549 
552  template<typename T>
553  void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
554  {
555  pcl::PCLPointCloud2 pcl_pc2;
556  pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
557  pcl_conversions::moveFromPCL(pcl_pc2, cloud);
558  }
559 
560  template<typename T>
561  void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
562  {
563  pcl::PCLPointCloud2 pcl_pc2;
564 #if PCL_VERSION_COMPARE(>=, 1, 13, 1)
565  pcl_conversions::copyPointCloud2MetaData(cloud, pcl_pc2); // Like pcl_conversions::toPCL, but does not copy the binary data
566  pcl::MsgFieldMap field_map;
567  pcl::createMapping<T> (pcl_pc2.fields, field_map);
568  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud, field_map, &cloud.data[0]);
569 #else
570  pcl_conversions::toPCL(cloud, pcl_pc2);
571  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
572 #endif
573  }
574 
575  template<typename T>
576  void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
577  {
578  pcl::PCLPointCloud2 pcl_pc2;
579  pcl_conversions::moveToPCL(cloud, pcl_pc2);
580  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
581  }
582 
585  template<typename PointT>
586  void createMapping(const std::vector<sensor_msgs::PointField>& msg_fields, MsgFieldMap& field_map)
587  {
588  std::vector<pcl::PCLPointField> pcl_msg_fields;
589  pcl_conversions::toPCL(msg_fields, pcl_msg_fields);
590  return createMapping<PointT>(pcl_msg_fields, field_map);
591  }
592 
593  namespace io {
594 
597  inline int
598  savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud,
599  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
600  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
601  const bool binary_mode = false)
602  {
603  pcl::PCLPointCloud2 pcl_cloud;
604  pcl_conversions::toPCL(cloud, pcl_cloud);
605  return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
606  }
607 
608  inline int
609  destructiveSavePCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud,
610  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
611  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
612  const bool binary_mode = false)
613  {
614  pcl::PCLPointCloud2 pcl_cloud;
615  pcl_conversions::moveToPCL(cloud, pcl_cloud);
616  return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
617  }
618 
621  inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
622  {
623  pcl::PCLPointCloud2 pcl_cloud;
624  int ret = pcl::io::loadPCDFile(file_name, pcl_cloud);
625  pcl_conversions::moveFromPCL(pcl_cloud, cloud);
626  return ret;
627  }
628 
629  } // namespace io
630 
633  inline
634  bool concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1,
635  const sensor_msgs::PointCloud2 &cloud2,
636  sensor_msgs::PointCloud2 &cloud_out)
637  {
638  //if one input cloud has no points, but the other input does, just return the cloud with points
639  if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0)
640  {
641  cloud_out = cloud2;
642  return (true);
643  }
644  else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
645  {
646  cloud_out = cloud1;
647  return (true);
648  }
649 
650  bool strip = false;
651  for (size_t i = 0; i < cloud1.fields.size (); ++i)
652  if (cloud1.fields[i].name == "_")
653  strip = true;
654 
655  for (size_t i = 0; i < cloud2.fields.size (); ++i)
656  if (cloud2.fields[i].name == "_")
657  strip = true;
658 
659  if (!strip && cloud1.fields.size () != cloud2.fields.size ())
660  {
661  PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
662  return (false);
663  }
664 
665  // Copy cloud1 into cloud_out
666  cloud_out = cloud1;
667  size_t nrpts = cloud_out.data.size ();
668  // Height = 1 => no more organized
669  cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
670  cloud_out.height = 1;
671  if (!cloud1.is_dense || !cloud2.is_dense)
672  cloud_out.is_dense = false;
673  else
674  cloud_out.is_dense = true;
675 
676  // We need to strip the extra padding fields
677  if (strip)
678  {
679  // Get the field sizes for the second cloud
680  std::vector<sensor_msgs::PointField> fields2;
681  std::vector<int> fields2_sizes;
682  for (size_t j = 0; j < cloud2.fields.size (); ++j)
683  {
684  if (cloud2.fields[j].name == "_")
685  continue;
686 
687  fields2_sizes.push_back (cloud2.fields[j].count *
688  pcl::getFieldSize (cloud2.fields[j].datatype));
689  fields2.push_back (cloud2.fields[j]);
690  }
691 
692  cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);
693 
694  // Copy the second cloud
695  for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
696  {
697  int i = 0;
698  for (size_t j = 0; j < fields2.size (); ++j)
699  {
700  if (cloud1.fields[i].name == "_")
701  {
702  ++i;
703  continue;
704  }
705 
706  // We're fine with the special RGB vs RGBA use case
707  if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") ||
708  (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") ||
709  (cloud1.fields[i].name == fields2[j].name))
710  {
711  memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]),
712  reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
713  fields2_sizes[j]);
714  ++i; // increment the field size i
715  }
716  }
717  }
718  }
719  else
720  {
721  for (size_t i = 0; i < cloud1.fields.size (); ++i)
722  {
723  // We're fine with the special RGB vs RGBA use case
724  if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") ||
725  (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb"))
726  continue;
727  // Otherwise we need to make sure the names are the same
728  if (cloud1.fields[i].name != cloud2.fields[i].name)
729  {
730  PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());
731  return (false);
732  }
733  }
734  cloud_out.data.resize (nrpts + cloud2.data.size ());
735  memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
736  }
737  return (true);
738  }
739 
740 } // namespace pcl
741 
742 namespace ros
743 {
744  template<>
745  struct DefaultMessageCreator<pcl::PCLPointCloud2>
746  {
748  {
749  boost::shared_ptr<pcl::PCLPointCloud2> msg(new pcl::PCLPointCloud2());
750  return msg;
751  }
752  };
753 
754  namespace message_traits
755  {
756  template<>
757  struct MD5Sum<pcl::PCLPointCloud2>
758  {
759  static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); }
760  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
761 
762  static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
763  static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
764 
765  // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
766  ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
767  ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
768  };
769 
770  template<>
771  struct DataType<pcl::PCLPointCloud2>
772  {
773  static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
774  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
775  };
776 
777  template<>
778  struct Definition<pcl::PCLPointCloud2>
779  {
780  static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
781  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
782  };
783 
784  template<> struct HasHeader<pcl::PCLPointCloud2> : TrueType {};
785  } // namespace ros::message_traits
786 
787  namespace serialization
788  {
789  /*
790  * Provide a custom serialization for pcl::PCLPointCloud2
791  */
792  template<>
793  struct Serializer<pcl::PCLPointCloud2>
794  {
795  template<typename Stream>
796  inline static void write(Stream& stream, const pcl::PCLPointCloud2& m)
797  {
798  std_msgs::Header header;
799  pcl_conversions::fromPCL(m.header, header);
800  stream.next(header);
801  stream.next(m.height);
802  stream.next(m.width);
803  std::vector<sensor_msgs::PointField> pfs;
804  pcl_conversions::fromPCL(m.fields, pfs);
805  stream.next(pfs);
806  stream.next(m.is_bigendian);
807  stream.next(m.point_step);
808  stream.next(m.row_step);
809  stream.next(m.data);
810  stream.next(m.is_dense);
811  }
812 
813  template<typename Stream>
814  inline static void read(Stream& stream, pcl::PCLPointCloud2& m)
815  {
816  std_msgs::Header header;
817  stream.next(header);
818  pcl_conversions::toPCL(header, m.header);
819  stream.next(m.height);
820  stream.next(m.width);
821  std::vector<sensor_msgs::PointField> pfs;
822  stream.next(pfs);
823  pcl_conversions::toPCL(pfs, m.fields);
824  stream.next(m.is_bigendian);
825  stream.next(m.point_step);
826  stream.next(m.row_step);
827  stream.next(m.data);
828  stream.next(m.is_dense);
829  }
830 
831  inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m)
832  {
833  uint32_t length = 0;
834 
835  std_msgs::Header header;
836  pcl_conversions::fromPCL(m.header, header);
837  length += serializationLength(header);
838  length += 4; // height
839  length += 4; // width
840  std::vector<sensor_msgs::PointField> pfs;
841  pcl_conversions::fromPCL(m.fields, pfs);
842  length += serializationLength(pfs); // fields
843  length += 1; // is_bigendian
844  length += 4; // point_step
845  length += 4; // row_step
846  length += 4; // data's size
847  length += m.data.size() * sizeof(std::uint8_t);
848  length += 1; // is_dense
849 
850  return length;
851  }
852  };
853 
854  /*
855  * Provide a custom serialization for pcl::PCLPointField
856  */
857  template<>
858  struct Serializer<pcl::PCLPointField>
859  {
860  template<typename Stream>
861  inline static void write(Stream& stream, const pcl::PCLPointField& m)
862  {
863  stream.next(m.name);
864  stream.next(m.offset);
865  stream.next(m.datatype);
866  stream.next(m.count);
867  }
868 
869  template<typename Stream>
870  inline static void read(Stream& stream, pcl::PCLPointField& m)
871  {
872  stream.next(m.name);
873  stream.next(m.offset);
874  stream.next(m.datatype);
875  stream.next(m.count);
876  }
877 
878  inline static uint32_t serializedLength(const pcl::PCLPointField& m)
879  {
880  uint32_t length = 0;
881 
882  length += serializationLength(m.name);
883  length += serializationLength(m.offset);
884  length += serializationLength(m.datatype);
885  length += serializationLength(m.count);
886 
887  return length;
888  }
889  };
890 
891  /*
892  * Provide a custom serialization for pcl::PCLHeader
893  */
894  template<>
895  struct Serializer<pcl::PCLHeader>
896  {
897  template<typename Stream>
898  inline static void write(Stream& stream, const pcl::PCLHeader& m)
899  {
900  std_msgs::Header header;
901  pcl_conversions::fromPCL(m, header);
902  stream.next(header);
903  }
904 
905  template<typename Stream>
906  inline static void read(Stream& stream, pcl::PCLHeader& m)
907  {
908  std_msgs::Header header;
909  stream.next(header);
910  pcl_conversions::toPCL(header, m);
911  }
912 
913  inline static uint32_t serializedLength(const pcl::PCLHeader& m)
914  {
915  uint32_t length = 0;
916 
917  std_msgs::Header header;
918  pcl_conversions::fromPCL(m, header);
919  length += serializationLength(header);
920 
921  return length;
922  }
923  };
924  } // namespace ros::serialization
925 
926 } // namespace ros
927 
928 
929 #endif /* PCL_CONVERSIONS_H__ */
d
void createMapping(const std::vector< sensor_msgs::PointField > &msg_fields, MsgFieldMap &field_map)
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
static uint32_t serializedLength(const pcl::PCLPointField &m)
void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
int destructiveSavePCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), const bool binary_mode=false)
Time & fromNSec(uint64_t t)
uint64_t toNSec() const
static uint32_t serializedLength(const pcl::PCLPointCloud2 &m)
static void write(Stream &stream, const pcl::PCLPointCloud2 &m)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
static uint32_t serializedLength(const pcl::PCLHeader &m)
static const char * value(const pcl::PCLPointCloud2 &)
static void write(Stream &stream, const pcl::PCLPointField &m)
#define ROS_STATIC_ASSERT(cond)
static void read(Stream &stream, pcl::PCLPointCloud2 &m)
static void read(Stream &stream, pcl::PCLPointField &m)
int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
static void write(Stream &stream, const pcl::PCLHeader &m)
void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp)
static const char * value(const pcl::PCLPointCloud2 &)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
bool concatenatePointCloud(const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out)
void copyImageMetaData(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
uint32_t serializationLength(const T &t)
void move(std::vector< T > &a, std::vector< T > &b)
const std::string header
static void read(Stream &stream, pcl::PCLHeader &m)
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
static const char * value(const pcl::PCLPointCloud2 &)
int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
int savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), const bool binary_mode=false)
void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)


pcl_conversions
Author(s): William Woodall
autogenerated on Thu Feb 16 2023 03:08:01