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 pcl::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, pcl::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 pcl::uint64_t &pcl_stamp)
93  {
94  ros::Time stamp;
95  fromPCL(pcl_stamp, stamp);
96  return stamp;
97  }
98 
99  inline
100  pcl::uint64_t toPCL(const ros::Time &stamp)
101  {
102  pcl::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  inline
354  void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert)
355  {
356  vert.vertices = pcl_vert.vertices;
357  }
358 
359  inline
360  void fromPCL(const std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::Vertices> &verts)
361  {
362  verts.resize(pcl_verts.size());
363  std::vector<pcl::Vertices>::const_iterator it = pcl_verts.begin();
364  std::vector<pcl_msgs::Vertices>::iterator jt = verts.begin();
365  for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
366  fromPCL(*(it), *(jt));
367  }
368  }
369 
370  inline
371  void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert)
372  {
373  vert.vertices.swap(pcl_vert.vertices);
374  }
375 
376  inline
377  void fromPCL(std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::Vertices> &verts)
378  {
379  verts.resize(pcl_verts.size());
380  std::vector<pcl::Vertices>::iterator it = pcl_verts.begin();
381  std::vector<pcl_msgs::Vertices>::iterator jt = verts.begin();
382  for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
383  moveFromPCL(*(it), *(jt));
384  }
385  }
386 
387  inline
388  void toPCL(const pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert)
389  {
390  pcl_vert.vertices = vert.vertices;
391  }
392 
393  inline
394  void toPCL(const std::vector<pcl_msgs::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
395  {
396  pcl_verts.resize(verts.size());
397  std::vector<pcl_msgs::Vertices>::const_iterator it = verts.begin();
398  std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
399  for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
400  toPCL(*(it), *(jt));
401  }
402  }
403 
404  inline
405  void moveToPCL(pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert)
406  {
407  pcl_vert.vertices.swap(vert.vertices);
408  }
409 
410  inline
411  void moveToPCL(std::vector<pcl_msgs::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
412  {
413  pcl_verts.resize(verts.size());
414  std::vector<pcl_msgs::Vertices>::iterator it = verts.begin();
415  std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
416  for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
417  moveToPCL(*(it), *(jt));
418  }
419  }
420 
423  inline
424  void fromPCL(const pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh)
425  {
426  fromPCL(pcl_mesh.header, mesh.header);
427  fromPCL(pcl_mesh.cloud, mesh.cloud);
428  fromPCL(pcl_mesh.polygons, mesh.polygons);
429  }
430 
431  inline
432  void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh)
433  {
434  fromPCL(pcl_mesh.header, mesh.header);
435  moveFromPCL(pcl_mesh.cloud, mesh.cloud);
436  }
437 
438  inline
439  void toPCL(const pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
440  {
441  toPCL(mesh.header, pcl_mesh.header);
442  toPCL(mesh.cloud, pcl_mesh.cloud);
443  toPCL(mesh.polygons, pcl_mesh.polygons);
444  }
445 
446  inline
447  void moveToPCL(pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
448  {
449  toPCL(mesh.header, pcl_mesh.header);
450  moveToPCL(mesh.cloud, pcl_mesh.cloud);
451  moveToPCL(mesh.polygons, pcl_mesh.polygons);
452  }
453 
454 } // namespace pcl_conversions
455 
456 namespace pcl {
457 
460  inline int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
461  {
462  // Get the index we need
463  for (size_t d = 0; d < cloud.fields.size(); ++d) {
464  if (cloud.fields[d].name == field_name) {
465  return (static_cast<int>(d));
466  }
467  }
468  return (-1);
469  }
470 
473  inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
474  {
475  std::string result;
476  for (size_t i = 0; i < cloud.fields.size () - 1; ++i) {
477  result += cloud.fields[i].name + " ";
478  }
479  result += cloud.fields[cloud.fields.size () - 1].name;
480  return (result);
481  }
482 
485  inline
486  void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
487  {
488  pcl::PCLPointCloud2 pcl_cloud;
489  pcl_conversions::toPCL(cloud, pcl_cloud);
490  pcl::PCLImage pcl_image;
491  pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
492  pcl_conversions::moveFromPCL(pcl_image, image);
493  }
494 
495  inline
496  void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
497  {
498  pcl::PCLPointCloud2 pcl_cloud;
499  pcl_conversions::moveToPCL(cloud, pcl_cloud);
500  pcl::PCLImage pcl_image;
501  pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
502  pcl_conversions::moveFromPCL(pcl_image, image);
503  }
504 
505  template<typename T> void
506  toROSMsg (const pcl::PointCloud<T> &cloud, sensor_msgs::Image& msg)
507  {
508  // Ease the user's burden on specifying width/height for unorganized datasets
509  if (cloud.width == 0 && cloud.height == 0)
510  {
511  throw std::runtime_error("Needs to be a dense like cloud!!");
512  }
513  else
514  {
515  if (cloud.points.size () != cloud.width * cloud.height)
516  throw std::runtime_error("The width and height do not match the cloud size!");
517  msg.height = cloud.height;
518  msg.width = cloud.width;
519  }
520 
521  // sensor_msgs::image_encodings::BGR8;
522  msg.encoding = "bgr8";
523  msg.step = msg.width * sizeof (uint8_t) * 3;
524  msg.data.resize (msg.step * msg.height);
525  for (size_t y = 0; y < cloud.height; y++)
526  {
527  for (size_t x = 0; x < cloud.width; x++)
528  {
529  uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
530  memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t));
531  }
532  }
533  }
534 
537  template<typename T>
538  void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
539  {
540  pcl::PCLPointCloud2 pcl_pc2;
541  pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
542  pcl_conversions::moveFromPCL(pcl_pc2, cloud);
543  }
544 
545  template<typename T>
546  void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
547  {
548  pcl::PCLPointCloud2 pcl_pc2;
549  pcl_conversions::toPCL(cloud, pcl_pc2);
550  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
551  }
552 
553  template<typename T>
554  void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
555  {
556  pcl::PCLPointCloud2 pcl_pc2;
557  pcl_conversions::moveToPCL(cloud, pcl_pc2);
558  pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
559  }
560 
563  template<typename PointT>
564  void createMapping(const std::vector<sensor_msgs::PointField>& msg_fields, MsgFieldMap& field_map)
565  {
566  std::vector<pcl::PCLPointField> pcl_msg_fields;
567  pcl_conversions::toPCL(msg_fields, pcl_msg_fields);
568  return createMapping<PointT>(pcl_msg_fields, field_map);
569  }
570 
571  namespace io {
572 
575  inline int
576  savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud,
577  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
578  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
579  const bool binary_mode = false)
580  {
581  pcl::PCLPointCloud2 pcl_cloud;
582  pcl_conversions::toPCL(cloud, pcl_cloud);
583  return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
584  }
585 
586  inline int
587  destructiveSavePCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud,
588  const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
589  const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
590  const bool binary_mode = false)
591  {
592  pcl::PCLPointCloud2 pcl_cloud;
593  pcl_conversions::moveToPCL(cloud, pcl_cloud);
594  return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
595  }
596 
599  inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
600  {
601  pcl::PCLPointCloud2 pcl_cloud;
602  int ret = pcl::io::loadPCDFile(file_name, pcl_cloud);
603  pcl_conversions::moveFromPCL(pcl_cloud, cloud);
604  return ret;
605  }
606 
607  } // namespace io
608 
611  inline
612  bool concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1,
613  const sensor_msgs::PointCloud2 &cloud2,
614  sensor_msgs::PointCloud2 &cloud_out)
615  {
616  //if one input cloud has no points, but the other input does, just return the cloud with points
617  if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0)
618  {
619  cloud_out = cloud2;
620  return (true);
621  }
622  else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
623  {
624  cloud_out = cloud1;
625  return (true);
626  }
627 
628  bool strip = false;
629  for (size_t i = 0; i < cloud1.fields.size (); ++i)
630  if (cloud1.fields[i].name == "_")
631  strip = true;
632 
633  for (size_t i = 0; i < cloud2.fields.size (); ++i)
634  if (cloud2.fields[i].name == "_")
635  strip = true;
636 
637  if (!strip && cloud1.fields.size () != cloud2.fields.size ())
638  {
639  PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
640  return (false);
641  }
642 
643  // Copy cloud1 into cloud_out
644  cloud_out = cloud1;
645  size_t nrpts = cloud_out.data.size ();
646  // Height = 1 => no more organized
647  cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
648  cloud_out.height = 1;
649  if (!cloud1.is_dense || !cloud2.is_dense)
650  cloud_out.is_dense = false;
651  else
652  cloud_out.is_dense = true;
653 
654  // We need to strip the extra padding fields
655  if (strip)
656  {
657  // Get the field sizes for the second cloud
658  std::vector<sensor_msgs::PointField> fields2;
659  std::vector<int> fields2_sizes;
660  for (size_t j = 0; j < cloud2.fields.size (); ++j)
661  {
662  if (cloud2.fields[j].name == "_")
663  continue;
664 
665  fields2_sizes.push_back (cloud2.fields[j].count *
666  pcl::getFieldSize (cloud2.fields[j].datatype));
667  fields2.push_back (cloud2.fields[j]);
668  }
669 
670  cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);
671 
672  // Copy the second cloud
673  for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
674  {
675  int i = 0;
676  for (size_t j = 0; j < fields2.size (); ++j)
677  {
678  if (cloud1.fields[i].name == "_")
679  {
680  ++i;
681  continue;
682  }
683 
684  // We're fine with the special RGB vs RGBA use case
685  if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") ||
686  (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") ||
687  (cloud1.fields[i].name == fields2[j].name))
688  {
689  memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]),
690  reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
691  fields2_sizes[j]);
692  ++i; // increment the field size i
693  }
694  }
695  }
696  }
697  else
698  {
699  for (size_t i = 0; i < cloud1.fields.size (); ++i)
700  {
701  // We're fine with the special RGB vs RGBA use case
702  if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") ||
703  (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb"))
704  continue;
705  // Otherwise we need to make sure the names are the same
706  if (cloud1.fields[i].name != cloud2.fields[i].name)
707  {
708  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 ());
709  return (false);
710  }
711  }
712  cloud_out.data.resize (nrpts + cloud2.data.size ());
713  memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
714  }
715  return (true);
716  }
717 
718 } // namespace pcl
719 
720 namespace ros
721 {
722  template<>
723  struct DefaultMessageCreator<pcl::PCLPointCloud2>
724  {
726  {
727  boost::shared_ptr<pcl::PCLPointCloud2> msg(new pcl::PCLPointCloud2());
728  return msg;
729  }
730  };
731 
732  namespace message_traits
733  {
734  template<>
735  struct MD5Sum<pcl::PCLPointCloud2>
736  {
737  static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); }
738  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
739 
740  static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
741  static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
742 
743  // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
744  ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
745  ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
746  };
747 
748  template<>
749  struct DataType<pcl::PCLPointCloud2>
750  {
751  static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
752  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
753  };
754 
755  template<>
756  struct Definition<pcl::PCLPointCloud2>
757  {
758  static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
759  static const char* value(const pcl::PCLPointCloud2&) { return value(); }
760  };
761 
762  template<> struct HasHeader<pcl::PCLPointCloud2> : TrueType {};
763  } // namespace ros::message_traits
764 
765  namespace serialization
766  {
767  /*
768  * Provide a custom serialization for pcl::PCLPointCloud2
769  */
770  template<>
771  struct Serializer<pcl::PCLPointCloud2>
772  {
773  template<typename Stream>
774  inline static void write(Stream& stream, const pcl::PCLPointCloud2& m)
775  {
776  std_msgs::Header header;
777  pcl_conversions::fromPCL(m.header, header);
778  stream.next(header);
779  stream.next(m.height);
780  stream.next(m.width);
781  std::vector<sensor_msgs::PointField> pfs;
782  pcl_conversions::fromPCL(m.fields, pfs);
783  stream.next(pfs);
784  stream.next(m.is_bigendian);
785  stream.next(m.point_step);
786  stream.next(m.row_step);
787  stream.next(m.data);
788  stream.next(m.is_dense);
789  }
790 
791  template<typename Stream>
792  inline static void read(Stream& stream, pcl::PCLPointCloud2& m)
793  {
794  std_msgs::Header header;
795  stream.next(header);
796  pcl_conversions::toPCL(header, m.header);
797  stream.next(m.height);
798  stream.next(m.width);
799  std::vector<sensor_msgs::PointField> pfs;
800  stream.next(pfs);
801  pcl_conversions::toPCL(pfs, m.fields);
802  stream.next(m.is_bigendian);
803  stream.next(m.point_step);
804  stream.next(m.row_step);
805  stream.next(m.data);
806  stream.next(m.is_dense);
807  }
808 
809  inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m)
810  {
811  uint32_t length = 0;
812 
813  std_msgs::Header header;
814  pcl_conversions::fromPCL(m.header, header);
815  length += serializationLength(header);
816  length += 4; // height
817  length += 4; // width
818  std::vector<sensor_msgs::PointField> pfs;
819  pcl_conversions::fromPCL(m.fields, pfs);
820  length += serializationLength(pfs); // fields
821  length += 1; // is_bigendian
822  length += 4; // point_step
823  length += 4; // row_step
824  length += 4; // data's size
825  length += m.data.size() * sizeof(pcl::uint8_t);
826  length += 1; // is_dense
827 
828  return length;
829  }
830  };
831 
832  /*
833  * Provide a custom serialization for pcl::PCLPointField
834  */
835  template<>
836  struct Serializer<pcl::PCLPointField>
837  {
838  template<typename Stream>
839  inline static void write(Stream& stream, const pcl::PCLPointField& m)
840  {
841  stream.next(m.name);
842  stream.next(m.offset);
843  stream.next(m.datatype);
844  stream.next(m.count);
845  }
846 
847  template<typename Stream>
848  inline static void read(Stream& stream, pcl::PCLPointField& m)
849  {
850  stream.next(m.name);
851  stream.next(m.offset);
852  stream.next(m.datatype);
853  stream.next(m.count);
854  }
855 
856  inline static uint32_t serializedLength(const pcl::PCLPointField& m)
857  {
858  uint32_t length = 0;
859 
860  length += serializationLength(m.name);
861  length += serializationLength(m.offset);
862  length += serializationLength(m.datatype);
863  length += serializationLength(m.count);
864 
865  return length;
866  }
867  };
868 
869  /*
870  * Provide a custom serialization for pcl::PCLHeader
871  */
872  template<>
873  struct Serializer<pcl::PCLHeader>
874  {
875  template<typename Stream>
876  inline static void write(Stream& stream, const pcl::PCLHeader& m)
877  {
878  std_msgs::Header header;
879  pcl_conversions::fromPCL(m, header);
880  stream.next(header);
881  }
882 
883  template<typename Stream>
884  inline static void read(Stream& stream, pcl::PCLHeader& m)
885  {
886  std_msgs::Header header;
887  stream.next(header);
888  pcl_conversions::toPCL(header, m);
889  }
890 
891  inline static uint32_t serializedLength(const pcl::PCLHeader& m)
892  {
893  uint32_t length = 0;
894 
895  std_msgs::Header header;
896  pcl_conversions::fromPCL(m, header);
897  length += serializationLength(header);
898 
899  return length;
900  }
901  };
902  } // namespace ros::serialization
903 
904 } // namespace ros
905 
906 
907 #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)
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)
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
#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)
uint64_t toNSec() const
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)
const std::string header
static void read(Stream &stream, pcl::PCLHeader &m)
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
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 toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
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 Wed Jun 5 2019 20:24:15