pcl_conversions.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2013, Open Source Robotics Foundation, Inc.
00005  * Copyright (c) 2010-2012, Willow Garage, Inc.
00006  * All rights reserved.
00007  *
00008  * Redistribution and use in source and binary forms, with or without
00009  * modification, are permitted provided that the following conditions
00010  * are met:
00011  *
00012  *  * Redistributions of source code must retain the above copyright
00013  *    notice, this list of conditions and the following disclaimer.
00014  *  * Redistributions in binary form must reproduce the above
00015  *    copyright notice, this list of conditions and the following
00016  *    disclaimer in the documentation and/or other materials provided
00017  *    with the distribution.
00018  *  * Neither the name of Open Source Robotics Foundation, Inc. nor
00019  *    the names of its contributors may be used to endorse or promote
00020  *    products derived from this software without specific prior
00021  *    written permission.
00022  *
00023  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  * POSSIBILITY OF SUCH DAMAGE.
00035  */
00036 
00037 #ifndef PCL_CONVERSIONS_H__
00038 #define PCL_CONVERSIONS_H__
00039 
00040 #include <vector>
00041 
00042 #include <ros/ros.h>
00043 
00044 #include <pcl/conversions.h>
00045 
00046 #include <pcl/PCLHeader.h>
00047 #include <std_msgs/Header.h>
00048 
00049 #include <pcl/PCLImage.h>
00050 #include <sensor_msgs/Image.h>
00051 
00052 #include <pcl/PCLPointField.h>
00053 #include <sensor_msgs/PointField.h>
00054 
00055 #include <pcl/PCLPointCloud2.h>
00056 #include <sensor_msgs/PointCloud2.h>
00057 
00058 #include <pcl/PointIndices.h>
00059 #include <pcl_msgs/PointIndices.h>
00060 
00061 #include <pcl/ModelCoefficients.h>
00062 #include <pcl_msgs/ModelCoefficients.h>
00063 
00064 #include <pcl/Vertices.h>
00065 #include <pcl_msgs/Vertices.h>
00066 
00067 #include <pcl/PolygonMesh.h>
00068 #include <pcl_msgs/PolygonMesh.h>
00069 
00070 #include <pcl/io/pcd_io.h>
00071 
00072 #include <Eigen/StdVector>
00073 #include <Eigen/Geometry>
00074 
00075 namespace pcl_conversions {
00076 
00079   inline
00080   void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
00081   {
00082     stamp.fromNSec(pcl_stamp * 1000ull);  // Convert from us to ns
00083   }
00084 
00085   inline
00086   void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
00087   {
00088     pcl_stamp = stamp.toNSec() / 1000ull;  // Convert from ns to us
00089   }
00090 
00091   inline
00092   ros::Time fromPCL(const pcl::uint64_t &pcl_stamp)
00093   {
00094     ros::Time stamp;
00095     fromPCL(pcl_stamp, stamp);
00096     return stamp;
00097   }
00098 
00099   inline
00100   pcl::uint64_t toPCL(const ros::Time &stamp)
00101   {
00102     pcl::uint64_t pcl_stamp;
00103     toPCL(stamp, pcl_stamp);
00104     return pcl_stamp;
00105   }
00106 
00109   inline
00110   void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header)
00111   {
00112     fromPCL(pcl_header.stamp, header.stamp);
00113     header.seq = pcl_header.seq;
00114     header.frame_id = pcl_header.frame_id;
00115   }
00116 
00117   inline
00118   void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header)
00119   {
00120     toPCL(header.stamp, pcl_header.stamp);
00121     pcl_header.seq = header.seq;
00122     pcl_header.frame_id = header.frame_id;
00123   }
00124 
00125   inline
00126   std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
00127   {
00128     std_msgs::Header header;
00129     fromPCL(pcl_header, header);
00130     return header;
00131   }
00132 
00133   inline
00134   pcl::PCLHeader toPCL(const std_msgs::Header &header)
00135   {
00136     pcl::PCLHeader pcl_header;
00137     toPCL(header, pcl_header);
00138     return pcl_header;
00139   }
00140 
00143   inline
00144   void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
00145   {
00146     fromPCL(pcl_image.header, image.header);
00147     image.height = pcl_image.height;
00148     image.width = pcl_image.width;
00149     image.encoding = pcl_image.encoding;
00150     image.is_bigendian = pcl_image.is_bigendian;
00151     image.step = pcl_image.step;
00152   }
00153 
00154   inline
00155   void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
00156   {
00157     copyPCLImageMetaData(pcl_image, image);
00158     image.data = pcl_image.data;
00159   }
00160 
00161   inline
00162   void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
00163   {
00164     copyPCLImageMetaData(pcl_image, image);
00165     image.data.swap(pcl_image.data);
00166   }
00167 
00168   inline
00169   void copyImageMetaData(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
00170   {
00171     toPCL(image.header, pcl_image.header);
00172     pcl_image.height = image.height;
00173     pcl_image.width = image.width;
00174     pcl_image.encoding = image.encoding;
00175     pcl_image.is_bigendian = image.is_bigendian;
00176     pcl_image.step = image.step;
00177   }
00178 
00179   inline
00180   void toPCL(const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
00181   {
00182     copyImageMetaData(image, pcl_image);
00183     pcl_image.data = image.data;
00184   }
00185 
00186   inline
00187   void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
00188   {
00189     copyImageMetaData(image, pcl_image);
00190     pcl_image.data.swap(image.data);
00191   }
00192 
00195   inline
00196   void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf)
00197   {
00198     pf.name = pcl_pf.name;
00199     pf.offset = pcl_pf.offset;
00200     pf.datatype = pcl_pf.datatype;
00201     pf.count = pcl_pf.count;
00202   }
00203 
00204   inline
00205   void fromPCL(const std::vector<pcl::PCLPointField> &pcl_pfs, std::vector<sensor_msgs::PointField> &pfs)
00206   {
00207     pfs.resize(pcl_pfs.size());
00208     std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();
00209     int i = 0;
00210     for(; it != pcl_pfs.end(); ++it, ++i) {
00211       fromPCL(*(it), pfs[i]);
00212     }
00213   }
00214 
00215   inline
00216   void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf)
00217   {
00218     pcl_pf.name = pf.name;
00219     pcl_pf.offset = pf.offset;
00220     pcl_pf.datatype = pf.datatype;
00221     pcl_pf.count = pf.count;
00222   }
00223 
00224   inline
00225   void toPCL(const std::vector<sensor_msgs::PointField> &pfs, std::vector<pcl::PCLPointField> &pcl_pfs)
00226   {
00227     pcl_pfs.resize(pfs.size());
00228     std::vector<sensor_msgs::PointField>::const_iterator it = pfs.begin();
00229     int i = 0;
00230     for(; it != pfs.end(); ++it, ++i) {
00231       toPCL(*(it), pcl_pfs[i]);
00232     }
00233   }
00234 
00237   inline
00238   void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
00239   {
00240     fromPCL(pcl_pc2.header, pc2.header);
00241     pc2.height = pcl_pc2.height;
00242     pc2.width = pcl_pc2.width;
00243     fromPCL(pcl_pc2.fields, pc2.fields);
00244     pc2.is_bigendian = pcl_pc2.is_bigendian;
00245     pc2.point_step = pcl_pc2.point_step;
00246     pc2.row_step = pcl_pc2.row_step;
00247     pc2.is_dense = pcl_pc2.is_dense;
00248   }
00249 
00250   inline
00251   void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
00252   {
00253     copyPCLPointCloud2MetaData(pcl_pc2, pc2);
00254     pc2.data = pcl_pc2.data;
00255   }
00256 
00257   inline
00258   void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
00259   {
00260     copyPCLPointCloud2MetaData(pcl_pc2, pc2);
00261     pc2.data.swap(pcl_pc2.data);
00262   }
00263 
00264   inline
00265   void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
00266   {
00267     toPCL(pc2.header, pcl_pc2.header);
00268     pcl_pc2.height = pc2.height;
00269     pcl_pc2.width = pc2.width;
00270     toPCL(pc2.fields, pcl_pc2.fields);
00271     pcl_pc2.is_bigendian = pc2.is_bigendian;
00272     pcl_pc2.point_step = pc2.point_step;
00273     pcl_pc2.row_step = pc2.row_step;
00274     pcl_pc2.is_dense = pc2.is_dense;
00275   }
00276 
00277   inline
00278   void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
00279   {
00280     copyPointCloud2MetaData(pc2, pcl_pc2);
00281     pcl_pc2.data = pc2.data;
00282   }
00283 
00284   inline
00285   void moveToPCL(sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
00286   {
00287     copyPointCloud2MetaData(pc2, pcl_pc2);
00288     pcl_pc2.data.swap(pc2.data);
00289   }
00290 
00293   inline
00294   void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi)
00295   {
00296     fromPCL(pcl_pi.header, pi.header);
00297     pi.indices = pcl_pi.indices;
00298   }
00299 
00300   inline
00301   void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::PointIndices &pi)
00302   {
00303     fromPCL(pcl_pi.header, pi.header);
00304     pi.indices.swap(pcl_pi.indices);
00305   }
00306 
00307   inline
00308   void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi)
00309   {
00310     toPCL(pi.header, pcl_pi.header);
00311     pcl_pi.indices = pi.indices;
00312   }
00313 
00314   inline
00315   void moveToPCL(pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi)
00316   {
00317     toPCL(pi.header, pcl_pi.header);
00318     pcl_pi.indices.swap(pi.indices);
00319   }
00320 
00323   inline
00324   void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
00325   {
00326     fromPCL(pcl_mc.header, mc.header);
00327     mc.values = pcl_mc.values;
00328   }
00329 
00330   inline
00331   void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
00332   {
00333     fromPCL(pcl_mc.header, mc.header);
00334     mc.values.swap(pcl_mc.values);
00335   }
00336 
00337   inline
00338   void toPCL(const pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
00339   {
00340     toPCL(mc.header, pcl_mc.header);
00341     pcl_mc.values = mc.values;
00342   }
00343 
00344   inline
00345   void moveToPCL(pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
00346   {
00347     toPCL(mc.header, pcl_mc.header);
00348     pcl_mc.values.swap(mc.values);
00349   }
00350 
00353   inline
00354   void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert)
00355   {
00356     vert.vertices = pcl_vert.vertices;
00357   }
00358 
00359   inline
00360   void fromPCL(const std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::Vertices> &verts)
00361   {
00362     verts.resize(pcl_verts.size());
00363     std::vector<pcl::Vertices>::const_iterator it = pcl_verts.begin();
00364     std::vector<pcl_msgs::Vertices>::iterator jt = verts.begin();
00365     for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
00366       fromPCL(*(it), *(jt));
00367     }
00368   }
00369 
00370   inline
00371   void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::Vertices &vert)
00372   {
00373     vert.vertices.swap(pcl_vert.vertices);
00374   }
00375 
00376   inline
00377   void fromPCL(std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::Vertices> &verts)
00378   {
00379     verts.resize(pcl_verts.size());
00380     std::vector<pcl::Vertices>::iterator it = pcl_verts.begin();
00381     std::vector<pcl_msgs::Vertices>::iterator jt = verts.begin();
00382     for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
00383       moveFromPCL(*(it), *(jt));
00384     }
00385   }
00386 
00387   inline
00388   void toPCL(const pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert)
00389   {
00390     pcl_vert.vertices = vert.vertices;
00391   }
00392 
00393   inline
00394   void toPCL(const std::vector<pcl_msgs::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
00395   {
00396     pcl_verts.resize(verts.size());
00397     std::vector<pcl_msgs::Vertices>::const_iterator it = verts.begin();
00398     std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
00399     for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
00400       toPCL(*(it), *(jt));
00401     }
00402   }
00403 
00404   inline
00405   void moveToPCL(pcl_msgs::Vertices &vert, pcl::Vertices &pcl_vert)
00406   {
00407     pcl_vert.vertices.swap(vert.vertices);
00408   }
00409 
00410   inline
00411   void moveToPCL(std::vector<pcl_msgs::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
00412   {
00413     pcl_verts.resize(verts.size());
00414     std::vector<pcl_msgs::Vertices>::iterator it = verts.begin();
00415     std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
00416     for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
00417       moveToPCL(*(it), *(jt));
00418     }
00419   }
00420 
00423   inline
00424   void fromPCL(const pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh)
00425   {
00426     fromPCL(pcl_mesh.header, mesh.header);
00427     fromPCL(pcl_mesh.cloud, mesh.cloud);
00428     fromPCL(pcl_mesh.polygons, mesh.polygons);
00429   }
00430 
00431   inline
00432   void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::PolygonMesh &mesh)
00433   {
00434     fromPCL(pcl_mesh.header, mesh.header);
00435     moveFromPCL(pcl_mesh.cloud, mesh.cloud);
00436   }
00437 
00438   inline
00439   void toPCL(const pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
00440   {
00441     toPCL(mesh.header, pcl_mesh.header);
00442     toPCL(mesh.cloud, pcl_mesh.cloud);
00443     toPCL(mesh.polygons, pcl_mesh.polygons);
00444   }
00445 
00446   inline
00447   void moveToPCL(pcl_msgs::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
00448   {
00449     toPCL(mesh.header, pcl_mesh.header);
00450     moveToPCL(mesh.cloud, pcl_mesh.cloud);
00451     moveToPCL(mesh.polygons, pcl_mesh.polygons);
00452   }
00453 
00454 } // namespace pcl_conversions
00455 
00456 namespace pcl {
00457 
00460   inline int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
00461   {
00462     // Get the index we need
00463     for (size_t d = 0; d < cloud.fields.size(); ++d) {
00464       if (cloud.fields[d].name == field_name) {
00465         return (static_cast<int>(d));
00466       }
00467     }
00468     return (-1);
00469   }
00470 
00473   inline std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
00474   {
00475     std::string result;
00476     for (size_t i = 0; i < cloud.fields.size () - 1; ++i) {
00477       result += cloud.fields[i].name + " ";
00478     }
00479     result += cloud.fields[cloud.fields.size () - 1].name;
00480     return (result);
00481   }
00482 
00485   inline
00486   void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
00487   {
00488     pcl::PCLPointCloud2 pcl_cloud;
00489     pcl_conversions::toPCL(cloud, pcl_cloud);
00490     pcl::PCLImage pcl_image;
00491     pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
00492     pcl_conversions::moveFromPCL(pcl_image, image);
00493   }
00494 
00495   inline
00496   void moveToROSMsg(sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
00497   {
00498     pcl::PCLPointCloud2 pcl_cloud;
00499     pcl_conversions::moveToPCL(cloud, pcl_cloud);
00500     pcl::PCLImage pcl_image;
00501     pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
00502     pcl_conversions::moveFromPCL(pcl_image, image);
00503   }
00504 
00505   template<typename T> void
00506   toROSMsg (const pcl::PointCloud<T> &cloud, sensor_msgs::Image& msg)
00507   {
00508     // Ease the user's burden on specifying width/height for unorganized datasets
00509     if (cloud.width == 0 && cloud.height == 0)
00510     {
00511       throw std::runtime_error("Needs to be a dense like cloud!!");
00512     }
00513     else
00514     {
00515       if (cloud.points.size () != cloud.width * cloud.height)
00516         throw std::runtime_error("The width and height do not match the cloud size!");
00517       msg.height = cloud.height;
00518       msg.width = cloud.width;
00519     }
00520 
00521     // sensor_msgs::image_encodings::BGR8;
00522     msg.encoding = "bgr8";
00523     msg.step = msg.width * sizeof (uint8_t) * 3;
00524     msg.data.resize (msg.step * msg.height);
00525     for (size_t y = 0; y < cloud.height; y++)
00526     {
00527       for (size_t x = 0; x < cloud.width; x++)
00528       {
00529         uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
00530         memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t));
00531       }
00532     }
00533   }
00534 
00537   template<typename T>
00538   void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
00539   {
00540     pcl::PCLPointCloud2 pcl_pc2;
00541     pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
00542     pcl_conversions::moveFromPCL(pcl_pc2, cloud);
00543   }
00544 
00545   template<typename T>
00546   void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
00547   {
00548     pcl::PCLPointCloud2 pcl_pc2;
00549     pcl_conversions::toPCL(cloud, pcl_pc2);
00550     pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
00551   }
00552 
00553   template<typename T>
00554   void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
00555   {
00556     pcl::PCLPointCloud2 pcl_pc2;
00557     pcl_conversions::moveToPCL(cloud, pcl_pc2);
00558     pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
00559   }
00560 
00563   template<typename PointT>
00564   void createMapping(const std::vector<sensor_msgs::PointField>& msg_fields, MsgFieldMap& field_map)
00565   {
00566     std::vector<pcl::PCLPointField> pcl_msg_fields;
00567     pcl_conversions::toPCL(msg_fields, pcl_msg_fields);
00568     return createMapping<PointT>(pcl_msg_fields, field_map);
00569   }
00570 
00571   namespace io {
00572 
00575     inline int
00576     savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud,
00577                 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
00578                 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
00579                 const bool binary_mode = false)
00580     {
00581       pcl::PCLPointCloud2 pcl_cloud;
00582       pcl_conversions::toPCL(cloud, pcl_cloud);
00583       return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
00584     }
00585 
00586     inline int
00587     destructiveSavePCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud,
00588                            const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
00589                            const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
00590                            const bool binary_mode = false)
00591     {
00592       pcl::PCLPointCloud2 pcl_cloud;
00593       pcl_conversions::moveToPCL(cloud, pcl_cloud);
00594       return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
00595     }
00596 
00599     inline int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
00600     {
00601       pcl::PCLPointCloud2 pcl_cloud;
00602       int ret = pcl::io::loadPCDFile(file_name, pcl_cloud);
00603       pcl_conversions::moveFromPCL(pcl_cloud, cloud);
00604       return ret;
00605     }
00606 
00607   } // namespace io
00608 
00611   inline
00612   bool concatenatePointCloud (const sensor_msgs::PointCloud2 &cloud1,
00613                               const sensor_msgs::PointCloud2 &cloud2,
00614                               sensor_msgs::PointCloud2 &cloud_out)
00615   {
00616     //if one input cloud has no points, but the other input does, just return the cloud with points
00617     if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0)
00618     {
00619       cloud_out = cloud2;
00620       return (true);
00621     }
00622     else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
00623     {
00624       cloud_out = cloud1;
00625       return (true);
00626     }
00627 
00628     bool strip = false;
00629     for (size_t i = 0; i < cloud1.fields.size (); ++i)
00630       if (cloud1.fields[i].name == "_")
00631         strip = true;
00632 
00633     for (size_t i = 0; i < cloud2.fields.size (); ++i)
00634       if (cloud2.fields[i].name == "_")
00635         strip = true;
00636 
00637     if (!strip && cloud1.fields.size () != cloud2.fields.size ())
00638     {
00639       PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
00640       return (false);
00641     }
00642 
00643     // Copy cloud1 into cloud_out
00644     cloud_out = cloud1;
00645     size_t nrpts = cloud_out.data.size ();
00646     // Height = 1 => no more organized
00647     cloud_out.width    = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
00648     cloud_out.height   = 1;
00649     if (!cloud1.is_dense || !cloud2.is_dense)
00650       cloud_out.is_dense = false;
00651     else
00652       cloud_out.is_dense = true;
00653 
00654     // We need to strip the extra padding fields
00655     if (strip)
00656     {
00657       // Get the field sizes for the second cloud
00658       std::vector<sensor_msgs::PointField> fields2;
00659       std::vector<int> fields2_sizes;
00660       for (size_t j = 0; j < cloud2.fields.size (); ++j)
00661       {
00662         if (cloud2.fields[j].name == "_")
00663           continue;
00664 
00665         fields2_sizes.push_back (cloud2.fields[j].count *
00666                                  pcl::getFieldSize (cloud2.fields[j].datatype));
00667         fields2.push_back (cloud2.fields[j]);
00668       }
00669 
00670       cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);
00671 
00672       // Copy the second cloud
00673       for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
00674       {
00675         int i = 0;
00676         for (size_t j = 0; j < fields2.size (); ++j)
00677         {
00678           if (cloud1.fields[i].name == "_")
00679           {
00680             ++i;
00681             continue;
00682           }
00683 
00684           // We're fine with the special RGB vs RGBA use case
00685           if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") ||
00686               (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") ||
00687               (cloud1.fields[i].name == fields2[j].name))
00688           {
00689             memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]),
00690                     reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
00691                     fields2_sizes[j]);
00692             ++i;  // increment the field size i
00693           }
00694         }
00695       }
00696     }
00697     else
00698     {
00699       for (size_t i = 0; i < cloud1.fields.size (); ++i)
00700       {
00701         // We're fine with the special RGB vs RGBA use case
00702         if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") ||
00703             (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb"))
00704           continue;
00705         // Otherwise we need to make sure the names are the same
00706         if (cloud1.fields[i].name != cloud2.fields[i].name)
00707         {
00708           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 ());
00709           return (false);
00710         }
00711       }
00712       cloud_out.data.resize (nrpts + cloud2.data.size ());
00713       memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
00714     }
00715     return (true);
00716   }
00717 
00718 } // namespace pcl
00719 
00720 namespace ros
00721 {
00722   template<>
00723   struct DefaultMessageCreator<pcl::PCLPointCloud2>
00724   {
00725     boost::shared_ptr<pcl::PCLPointCloud2> operator() ()
00726     {
00727       boost::shared_ptr<pcl::PCLPointCloud2> msg(new pcl::PCLPointCloud2());
00728       return msg;
00729     }
00730   };
00731 
00732   namespace message_traits
00733   {
00734     template<>
00735     struct MD5Sum<pcl::PCLPointCloud2>
00736     {
00737       static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); }
00738       static const char* value(const pcl::PCLPointCloud2&) { return value(); }
00739 
00740       static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
00741       static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
00742 
00743       // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
00744       ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
00745       ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
00746     };
00747 
00748     template<>
00749     struct DataType<pcl::PCLPointCloud2>
00750     {
00751       static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
00752       static const char* value(const pcl::PCLPointCloud2&) { return value(); }
00753     };
00754 
00755     template<>
00756     struct Definition<pcl::PCLPointCloud2>
00757     {
00758       static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
00759       static const char* value(const pcl::PCLPointCloud2&) { return value(); }
00760     };
00761 
00762     template<> struct HasHeader<pcl::PCLPointCloud2> : TrueType {};
00763   } // namespace ros::message_traits
00764 
00765   namespace serialization
00766   {
00767     /*
00768      * Provide a custom serialization for pcl::PCLPointCloud2
00769      */
00770     template<>
00771     struct Serializer<pcl::PCLPointCloud2>
00772     {
00773       template<typename Stream>
00774       inline static void write(Stream& stream, const pcl::PCLPointCloud2& m)
00775       {
00776         std_msgs::Header header;
00777         pcl_conversions::fromPCL(m.header, header);
00778         stream.next(header);
00779         stream.next(m.height);
00780         stream.next(m.width);
00781         std::vector<sensor_msgs::PointField> pfs;
00782         pcl_conversions::fromPCL(m.fields, pfs);
00783         stream.next(pfs);
00784         stream.next(m.is_bigendian);
00785         stream.next(m.point_step);
00786         stream.next(m.row_step);
00787         stream.next(m.data);
00788         stream.next(m.is_dense);
00789       }
00790 
00791       template<typename Stream>
00792       inline static void read(Stream& stream, pcl::PCLPointCloud2& m)
00793       {
00794         std_msgs::Header header;
00795         stream.next(header);
00796         pcl_conversions::toPCL(header, m.header);
00797         stream.next(m.height);
00798         stream.next(m.width);
00799         std::vector<sensor_msgs::PointField> pfs;
00800         stream.next(pfs);
00801         pcl_conversions::toPCL(pfs, m.fields);
00802         stream.next(m.is_bigendian);
00803         stream.next(m.point_step);
00804         stream.next(m.row_step);
00805         stream.next(m.data);
00806         stream.next(m.is_dense);
00807       }
00808 
00809       inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m)
00810       {
00811         uint32_t length = 0;
00812 
00813         std_msgs::Header header;
00814         pcl_conversions::fromPCL(m.header, header);
00815         length += serializationLength(header);
00816         length += 4; // height
00817         length += 4; // width
00818         std::vector<sensor_msgs::PointField> pfs;
00819         pcl_conversions::fromPCL(m.fields, pfs);
00820         length += serializationLength(pfs); // fields
00821         length += 1; // is_bigendian
00822         length += 4; // point_step
00823         length += 4; // row_step
00824         length += 4; // data's size
00825         length += m.data.size() * sizeof(pcl::uint8_t);
00826         length += 1; // is_dense
00827 
00828         return length;
00829       }
00830     };
00831 
00832     /*
00833      * Provide a custom serialization for pcl::PCLPointField
00834      */
00835     template<>
00836     struct Serializer<pcl::PCLPointField>
00837     {
00838       template<typename Stream>
00839       inline static void write(Stream& stream, const pcl::PCLPointField& m)
00840       {
00841         stream.next(m.name);
00842         stream.next(m.offset);
00843         stream.next(m.datatype);
00844         stream.next(m.count);
00845       }
00846 
00847       template<typename Stream>
00848       inline static void read(Stream& stream, pcl::PCLPointField& m)
00849       {
00850         stream.next(m.name);
00851         stream.next(m.offset);
00852         stream.next(m.datatype);
00853         stream.next(m.count);
00854       }
00855 
00856       inline static uint32_t serializedLength(const pcl::PCLPointField& m)
00857       {
00858         uint32_t length = 0;
00859 
00860         length += serializationLength(m.name);
00861         length += serializationLength(m.offset);
00862         length += serializationLength(m.datatype);
00863         length += serializationLength(m.count);
00864 
00865         return length;
00866       }
00867     };
00868 
00869     /*
00870      * Provide a custom serialization for pcl::PCLHeader
00871      */
00872     template<>
00873     struct Serializer<pcl::PCLHeader>
00874     {
00875       template<typename Stream>
00876       inline static void write(Stream& stream, const pcl::PCLHeader& m)
00877       {
00878         std_msgs::Header header;
00879         pcl_conversions::fromPCL(m, header);
00880         stream.next(header);
00881       }
00882 
00883       template<typename Stream>
00884       inline static void read(Stream& stream, pcl::PCLHeader& m)
00885       {
00886         std_msgs::Header header;
00887         stream.next(header);
00888         pcl_conversions::toPCL(header, m);
00889       }
00890 
00891       inline static uint32_t serializedLength(const pcl::PCLHeader& m)
00892       {
00893         uint32_t length = 0;
00894 
00895         std_msgs::Header header;
00896         pcl_conversions::fromPCL(m, header);
00897         length += serializationLength(header);
00898 
00899         return length;
00900       }
00901     };
00902   } // namespace ros::serialization
00903 
00904 } // namespace ros
00905 
00906 
00907 #endif /* PCL_CONVERSIONS_H__ */


pcl_conversions
Author(s): William Woodall
autogenerated on Thu Nov 3 2016 03:30:00