point_cloud_conversion.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id$
00035  *
00036  */
00037 
00038 #ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
00039 #define SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
00040 
00041 #include <sensor_msgs/PointCloud.h>
00042 #include <sensor_msgs/PointCloud2.h>
00043 #include <sensor_msgs/point_field_conversion.h>
00044 
00049 namespace sensor_msgs
00050 {
00052 
00056 static inline int getPointCloud2FieldIndex (const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
00057 {
00058   // Get the index we need
00059   for (size_t d = 0; d < cloud.fields.size (); ++d)
00060     if (cloud.fields[d].name == field_name)
00061       return (d);
00062   return (-1);
00063 }
00064 
00066 
00070 static inline bool convertPointCloudToPointCloud2 (const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
00071 {
00072   output.header = input.header;
00073   output.width  = input.points.size ();
00074   output.height = 1;
00075   output.fields.resize (3 + input.channels.size ());
00076   // Convert x/y/z to fields
00077   output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
00078   int offset = 0;
00079   // All offsets are *4, as all field data types are float32
00080   for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
00081   {
00082     output.fields[d].offset = offset;
00083     output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
00084     output.fields[d].count  = 1;
00085   }
00086   output.point_step = offset;
00087   output.row_step   = output.point_step * output.width;
00088   // Convert the remaining of the channels to fields
00089   for (size_t d = 0; d < input.channels.size (); ++d)
00090     output.fields[3 + d].name = input.channels[d].name;
00091   output.data.resize (input.points.size () * output.point_step);
00092   output.is_bigendian = false;  // @todo ?
00093   output.is_dense     = false;
00094 
00095   // Copy the data points
00096   for (size_t cp = 0; cp < input.points.size (); ++cp)
00097   {
00098     memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
00099     memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
00100     memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
00101     for (size_t d = 0; d < input.channels.size (); ++d)
00102     {
00103       if (input.channels[d].values.size() == input.points.size())
00104       {
00105         memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
00106       }
00107     }
00108   }
00109   return (true);
00110 }
00111 
00113 
00117 static inline bool convertPointCloud2ToPointCloud (const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
00118 {
00119 
00120   output.header = input.header;
00121   output.points.resize (input.width * input.height);
00122   output.channels.resize (input.fields.size () - 3);
00123   // Get the x/y/z field offsets
00124   int x_idx = getPointCloud2FieldIndex (input, "x");
00125   int y_idx = getPointCloud2FieldIndex (input, "y");
00126   int z_idx = getPointCloud2FieldIndex (input, "z");
00127   if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00128   {
00129     std::cerr << "x/y/z coordinates not found! Cannot convert to sensor_msgs::PointCloud!" << std::endl;
00130     return (false);
00131   }
00132   int x_offset = input.fields[x_idx].offset;
00133   int y_offset = input.fields[y_idx].offset;
00134   int z_offset = input.fields[z_idx].offset;
00135   uint8_t x_datatype = input.fields[x_idx].datatype;
00136   uint8_t y_datatype = input.fields[y_idx].datatype;
00137   uint8_t z_datatype = input.fields[z_idx].datatype;
00138    
00139   // Convert the fields to channels
00140   int cur_c = 0;
00141   for (size_t d = 0; d < input.fields.size (); ++d)
00142   {
00143     if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
00144       continue;
00145     output.channels[cur_c].name = input.fields[d].name;
00146     output.channels[cur_c].values.resize (output.points.size ());
00147     cur_c++;
00148   }
00149 
00150   // Copy the data points
00151   for (size_t cp = 0; cp < output.points.size (); ++cp)
00152   {
00153     // Copy x/y/z
00154     output.points[cp].x = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + x_offset], x_datatype);
00155     output.points[cp].y = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + y_offset], y_datatype);
00156     output.points[cp].z = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + z_offset], z_datatype);
00157     // Copy the rest of the data
00158     int cur_c = 0;
00159     for (size_t d = 0; d < input.fields.size (); ++d)
00160     {
00161       if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
00162         continue;
00163       output.channels[cur_c++].values[cp] = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + input.fields[d].offset], input.fields[d].datatype);
00164     }
00165   }
00166   return (true);
00167 }
00168 }
00169 #endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H


sensor_msgs
Author(s):
autogenerated on Thu Jun 6 2019 18:15:45