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  pcl_conversions::toPCL(cloud, pcl_pc2);
565  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
566  }
567 
568  template<typename T>
569  void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
570  {
571  pcl::PCLPointCloud2 pcl_pc2;
572  pcl_conversions::moveToPCL(cloud, pcl_pc2);
573  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
574  }
575 
578  template<typename PointT>
579  void createMapping(const std::vector<sensor_msgs::PointField>& msg_fields, MsgFieldMap& field_map)
580  {
581  std::vector<pcl::PCLPointField> pcl_msg_fields;
582  pcl_conversions::toPCL(msg_fields, pcl_msg_fields);
583  return createMapping<PointT>(pcl_msg_fields, field_map);
584  }
585 
586  namespace io {
587 
590  inline int
591  savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud,
592  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
593  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
594  const bool binary_mode = false)
595  {
596  pcl::PCLPointCloud2 pcl_cloud;
597  pcl_conversions::toPCL(cloud, pcl_cloud);
598  return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
599  }
600 
601  inline int
602  destructiveSavePCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud,
603  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
604  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
605  const bool binary_mode = false)
606  {
607  pcl::PCLPointCloud2 pcl_cloud;
608  pcl_conversions::moveToPCL(cloud, pcl_cloud);
609  return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
610  }
611 
614  inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
615  {
616  pcl::PCLPointCloud2 pcl_cloud;
617  int ret = pcl::io::loadPCDFile(file_name, pcl_cloud);
618  pcl_conversions::moveFromPCL(pcl_cloud, cloud);
619  return ret;
620  }
621 
622  } // namespace io
623 
626  inline
627  bool concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1,
628  const sensor_msgs::PointCloud2 &cloud2,
629  sensor_msgs::PointCloud2 &cloud_out)
630  {
631  //if one input cloud has no points, but the other input does, just return the cloud with points
632  if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0)
633  {
634  cloud_out = cloud2;
635  return (true);
636  }
637  else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
638  {
639  cloud_out = cloud1;
640  return (true);
641  }
642 
643  bool strip = false;
644  for (size_t i = 0; i < cloud1.fields.size (); ++i)
645  if (cloud1.fields[i].name == "_")
646  strip = true;
647 
648  for (size_t i = 0; i < cloud2.fields.size (); ++i)
649  if (cloud2.fields[i].name == "_")
650  strip = true;
651 
652  if (!strip && cloud1.fields.size () != cloud2.fields.size ())
653  {
654  PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
655  return (false);
656  }
657 
658  // Copy cloud1 into cloud_out
659  cloud_out = cloud1;
660  size_t nrpts = cloud_out.data.size ();
661  // Height = 1 => no more organized
662  cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
663  cloud_out.height = 1;
664  if (!cloud1.is_dense || !cloud2.is_dense)
665  cloud_out.is_dense = false;
666  else
667  cloud_out.is_dense = true;
668 
669  // We need to strip the extra padding fields
670  if (strip)
671  {
672  // Get the field sizes for the second cloud
673  std::vector<sensor_msgs::PointField> fields2;
674  std::vector<int> fields2_sizes;
675  for (size_t j = 0; j < cloud2.fields.size (); ++j)
676  {
677  if (cloud2.fields[j].name == "_")
678  continue;
679 
680  fields2_sizes.push_back (cloud2.fields[j].count *
681  pcl::getFieldSize (cloud2.fields[j].datatype));
682  fields2.push_back (cloud2.fields[j]);
683  }
684 
685  cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);
686 
687  // Copy the second cloud
688  for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
689  {
690  int i = 0;
691  for (size_t j = 0; j < fields2.size (); ++j)
692  {
693  if (cloud1.fields[i].name == "_")
694  {
695  ++i;
696  continue;
697  }
698 
699  // We're fine with the special RGB vs RGBA use case
700  if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") ||
701  (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") ||
702  (cloud1.fields[i].name == fields2[j].name))
703  {
704  memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]),
705  reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
706  fields2_sizes[j]);
707  ++i; // increment the field size i
708  }
709  }
710  }
711  }
712  else
713  {
714  for (size_t i = 0; i < cloud1.fields.size (); ++i)
715  {
716  // We're fine with the special RGB vs RGBA use case
717  if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") ||
718  (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb"))
719  continue;
720  // Otherwise we need to make sure the names are the same
721  if (cloud1.fields[i].name != cloud2.fields[i].name)
722  {
723  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 ());
724  return (false);
725  }
726  }
727  cloud_out.data.resize (nrpts + cloud2.data.size ());
728  memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
729  }
730  return (true);
731  }
732 
733 } // namespace pcl
734 
735 namespace ros
736 {
737  template<>
738  struct DefaultMessageCreator<pcl::PCLPointCloud2>
739  {
741  {
742  boost::shared_ptr<pcl::PCLPointCloud2> msg(new pcl::PCLPointCloud2());
743  return msg;
744  }
745  };
746 
747  namespace message_traits
748  {
749  template<>
750  struct MD5Sum<pcl::PCLPointCloud2>
751  {
752  static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); }
753  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
754 
755  static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
756  static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
757 
758  // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
759  ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
760  ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
761  };
762 
763  template<>
764  struct DataType<pcl::PCLPointCloud2>
765  {
766  static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
767  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
768  };
769 
770  template<>
771  struct Definition<pcl::PCLPointCloud2>
772  {
773  static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
774  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
775  };
776 
777  template<> struct HasHeader<pcl::PCLPointCloud2> : TrueType {};
778  } // namespace ros::message_traits
779 
780  namespace serialization
781  {
782  /*
783  * Provide a custom serialization for pcl::PCLPointCloud2
784  */
785  template<>
786  struct Serializer<pcl::PCLPointCloud2>
787  {
788  template<typename Stream>
789  inline static void write(Stream& stream, const pcl::PCLPointCloud2& m)
790  {
791  std_msgs::Header header;
792  pcl_conversions::fromPCL(m.header, header);
793  stream.next(header);
794  stream.next(m.height);
795  stream.next(m.width);
796  std::vector<sensor_msgs::PointField> pfs;
797  pcl_conversions::fromPCL(m.fields, pfs);
798  stream.next(pfs);
799  stream.next(m.is_bigendian);
800  stream.next(m.point_step);
801  stream.next(m.row_step);
802  stream.next(m.data);
803  stream.next(m.is_dense);
804  }
805 
806  template<typename Stream>
807  inline static void read(Stream& stream, pcl::PCLPointCloud2& m)
808  {
809  std_msgs::Header header;
810  stream.next(header);
811  pcl_conversions::toPCL(header, m.header);
812  stream.next(m.height);
813  stream.next(m.width);
814  std::vector<sensor_msgs::PointField> pfs;
815  stream.next(pfs);
816  pcl_conversions::toPCL(pfs, m.fields);
817  stream.next(m.is_bigendian);
818  stream.next(m.point_step);
819  stream.next(m.row_step);
820  stream.next(m.data);
821  stream.next(m.is_dense);
822  }
823 
824  inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m)
825  {
826  uint32_t length = 0;
827 
828  std_msgs::Header header;
829  pcl_conversions::fromPCL(m.header, header);
830  length += serializationLength(header);
831  length += 4; // height
832  length += 4; // width
833  std::vector<sensor_msgs::PointField> pfs;
834  pcl_conversions::fromPCL(m.fields, pfs);
835  length += serializationLength(pfs); // fields
836  length += 1; // is_bigendian
837  length += 4; // point_step
838  length += 4; // row_step
839  length += 4; // data's size
840  length += m.data.size() * sizeof(std::uint8_t);
841  length += 1; // is_dense
842 
843  return length;
844  }
845  };
846 
847  /*
848  * Provide a custom serialization for pcl::PCLPointField
849  */
850  template<>
851  struct Serializer<pcl::PCLPointField>
852  {
853  template<typename Stream>
854  inline static void write(Stream& stream, const pcl::PCLPointField& m)
855  {
856  stream.next(m.name);
857  stream.next(m.offset);
858  stream.next(m.datatype);
859  stream.next(m.count);
860  }
861 
862  template<typename Stream>
863  inline static void read(Stream& stream, pcl::PCLPointField& m)
864  {
865  stream.next(m.name);
866  stream.next(m.offset);
867  stream.next(m.datatype);
868  stream.next(m.count);
869  }
870 
871  inline static uint32_t serializedLength(const pcl::PCLPointField& m)
872  {
873  uint32_t length = 0;
874 
875  length += serializationLength(m.name);
876  length += serializationLength(m.offset);
877  length += serializationLength(m.datatype);
878  length += serializationLength(m.count);
879 
880  return length;
881  }
882  };
883 
884  /*
885  * Provide a custom serialization for pcl::PCLHeader
886  */
887  template<>
888  struct Serializer<pcl::PCLHeader>
889  {
890  template<typename Stream>
891  inline static void write(Stream& stream, const pcl::PCLHeader& m)
892  {
893  std_msgs::Header header;
895  stream.next(header);
896  }
897 
898  template<typename Stream>
899  inline static void read(Stream& stream, pcl::PCLHeader& m)
900  {
901  std_msgs::Header header;
902  stream.next(header);
904  }
905 
906  inline static uint32_t serializedLength(const pcl::PCLHeader& m)
907  {
908  uint32_t length = 0;
909 
910  std_msgs::Header header;
912  length += serializationLength(header);
913 
914  return length;
915  }
916  };
917  } // namespace ros::serialization
918 
919 } // namespace ros
920 
921 
922 #endif /* PCL_CONVERSIONS_H__ */
pcl::createMapping
void createMapping(const std::vector< sensor_msgs::PointField > &msg_fields, MsgFieldMap &field_map)
Definition: pcl_conversions.h:579
ros::message_traits::TrueType
pcl
Definition: pcl_conversions.h:471
pcl::moveFromROSMsg
void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
Definition: pcl_conversions.h:569
boost::shared_ptr
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
Definition: pcl_conversions.h:488
ros::serialization::Serializer< pcl::PCLPointCloud2 >::read
static void read(Stream &stream, pcl::PCLPointCloud2 &m)
Definition: pcl_conversions.h:807
ros::serialization::Serializer< pcl::PCLPointField >::serializedLength
static uint32_t serializedLength(const pcl::PCLPointField &m)
Definition: pcl_conversions.h:871
pcl::moveToROSMsg
void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
Definition: pcl_conversions.h:511
pcl_conversions
Definition: pcl_conversions.h:75
pcl::io::destructiveSavePCDFile
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)
Definition: pcl_conversions.h:602
ros
ros.h
ros::serialization::Serializer< pcl::PCLPointCloud2 >::write
static void write(Stream &stream, const pcl::PCLPointCloud2 &m)
Definition: pcl_conversions.h:789
ros::serialization::serializationLength
uint32_t serializationLength(const boost::array< T, N > &t)
ros::serialization::Stream
ros::message_traits::DataType
ros::message_traits::Definition< pcl::PCLPointCloud2 >::value
static const char * value()
Definition: pcl_conversions.h:773
ros::message_traits::DataType< pcl::PCLPointCloud2 >::value
static const char * value()
Definition: pcl_conversions.h:766
ros::message_traits::HasHeader
ros::serialization::Serializer< pcl::PCLHeader >::serializedLength
static uint32_t serializedLength(const pcl::PCLHeader &m)
Definition: pcl_conversions.h:906
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
Definition: pcl_conversions.h:162
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
Definition: pcl_conversions.h:561
ros::message_traits::Definition< pcl::PCLPointCloud2 >::value
static const char * value(const pcl::PCLPointCloud2 &)
Definition: pcl_conversions.h:774
ros::serialization::Serializer< pcl::PCLPointField >::write
static void write(Stream &stream, const pcl::PCLPointField &m)
Definition: pcl_conversions.h:854
ros::message_traits::DataType< pcl::PCLPointCloud2 >::value
static const char * value(const pcl::PCLPointCloud2 &)
Definition: pcl_conversions.h:767
ros::DefaultMessageCreator
ros::message_traits::MD5Sum< pcl::PCLPointCloud2 >::value
static const char * value(const pcl::PCLPointCloud2 &)
Definition: pcl_conversions.h:753
ros::message_traits::DataType::value
static const char * value()
pcl_conversions::fromPCL
void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
Definition: pcl_conversions.h:80
ros::serialization::Serializer< pcl::PCLPointCloud2 >::serializedLength
static uint32_t serializedLength(const pcl::PCLPointCloud2 &m)
Definition: pcl_conversions.h:824
ros::message_traits::MD5Sum::value
static const char * value()
pcl::concatenatePointCloud
bool concatenatePointCloud(const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out)
Definition: pcl_conversions.h:627
pcl_conversions::moveToPCL
void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
Definition: pcl_conversions.h:187
d
d
ros::DefaultMessageCreator::operator()
boost::shared_ptr< M > operator()()
ros::message_traits::MD5Sum
pcl::io::loadPCDFile
int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
Definition: pcl_conversions.h:614
pcl_conversions::copyImageMetaData
void copyImageMetaData(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
Definition: pcl_conversions.h:169
pcl_conversions::toPCL
void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp)
Definition: pcl_conversions.h:86
pcl_conversions::copyPCLImageMetaData
void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
Definition: pcl_conversions.h:144
TimeBase< Time, Duration >::fromNSec
Time & fromNSec(uint64_t t)
ros::Time
ros::serialization::Serializer< pcl::PCLPointField >::read
static void read(Stream &stream, pcl::PCLPointField &m)
Definition: pcl_conversions.h:863
pcl_conversions::copyPCLPointCloud2MetaData
void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
Definition: pcl_conversions.h:238
pcl::toROSMsg
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
Definition: pcl_conversions.h:501
TimeBase< Time, Duration >::toNSec
uint64_t toNSec() const
pcl_conversions::copyPointCloud2MetaData
void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
Definition: pcl_conversions.h:265
ros::message_traits::Definition::value
static const char * value()
ros::serialization::Serializer< pcl::PCLHeader >::write
static void write(Stream &stream, const pcl::PCLHeader &m)
Definition: pcl_conversions.h:891
ROS_STATIC_ASSERT
#define ROS_STATIC_ASSERT(cond)
ros::message_traits::Definition
header
const std::string header
ros::serialization::Serializer< pcl::PCLHeader >::read
static void read(Stream &stream, pcl::PCLHeader &m)
Definition: pcl_conversions.h:899
pcl::io::savePCDFile
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)
Definition: pcl_conversions.h:591
ros::message_traits::MD5Sum< pcl::PCLPointCloud2 >::value
static const char * value()
Definition: pcl_conversions.h:752
ros::serialization::Serializer
pcl::getFieldIndex
int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
Definition: pcl_conversions.h:475
pcl_conversions::internal::move
void move(std::vector< T > &a, std::vector< T > &b)
Definition: pcl_conversions.h:356


pcl_conversions
Author(s): William Woodall
autogenerated on Sun Oct 3 2021 02:52:50