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 
00048 namespace sensor_msgs
00049 {
00051 
00055 static inline int getPointCloud2FieldIndex (const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
00056 {
00057   // Get the index we need
00058   for (size_t d = 0; d < cloud.fields.size (); ++d)
00059     if (cloud.fields[d].name == field_name)
00060       return (d);
00061   return (-1);
00062 }
00063 
00065 
00069 static inline bool convertPointCloudToPointCloud2 (const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
00070 {
00071   output.header = input.header;
00072   output.width  = input.points.size ();
00073   output.height = 1;
00074   output.fields.resize (3 + input.channels.size ());
00075   // Convert x/y/z to fields
00076   output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
00077   int offset = 0;
00078   // All offsets are *4, as all field data types are float32
00079   for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
00080   {
00081     output.fields[d].offset = offset;
00082     output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
00083     output.fields[d].count  = 1;
00084   }
00085   output.point_step = offset;
00086   output.row_step   = output.point_step * output.width;
00087   // Convert the remaining of the channels to fields
00088   for (size_t d = 0; d < input.channels.size (); ++d)
00089     output.fields[3 + d].name = input.channels[d].name;
00090   output.data.resize (input.points.size () * output.point_step);
00091   output.is_bigendian = false;  // @todo ?
00092   output.is_dense     = false;
00093 
00094   // Copy the data points
00095   for (size_t cp = 0; cp < input.points.size (); ++cp)
00096   {
00097     memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
00098     memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
00099     memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
00100     for (size_t d = 0; d < input.channels.size (); ++d)
00101     {
00102       if (input.channels[d].values.size() == input.points.size())
00103       {
00104         memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
00105       }
00106     }
00107   }
00108   return (true);
00109 }
00110 
00112 
00116 static inline bool convertPointCloud2ToPointCloud (const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
00117 {
00118   // Get all fields size and check if we have non-float32 values
00119   for (size_t d = 0; d < input.fields.size (); ++d)
00120   {
00121     if (input.fields[d].datatype != sensor_msgs::PointField::FLOAT32)
00122     {
00123       std::cerr << boost::str(boost::format("sensor_msgs::PointCloud accepts only float32 values, but field %d (%s) has field type %d!")%(int)d% input.fields[d].name%input.fields[d].datatype) << std::endl;
00124     }
00125   }
00126 
00127   output.header = input.header;
00128   output.points.resize (input.width * input.height);
00129   output.channels.resize (input.fields.size () - 3);
00130   // Get the x/y/z field offsets
00131   int x_idx = getPointCloud2FieldIndex (input, "x");
00132   int y_idx = getPointCloud2FieldIndex (input, "y");
00133   int z_idx = getPointCloud2FieldIndex (input, "z");
00134   if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00135   {
00136     std::cerr << "x/y/z coordinates not found! Cannot convert to sensor_msgs::PointCloud!" << std::endl;
00137     return (false);
00138   }
00139   int x_offset = input.fields[x_idx].offset;
00140   int y_offset = input.fields[y_idx].offset;
00141   int z_offset = input.fields[z_idx].offset;
00142    
00143   // Convert the fields to channels
00144   int cur_c = 0;
00145   for (size_t d = 0; d < input.fields.size (); ++d)
00146   {
00147     if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
00148       continue;
00149     output.channels[cur_c].name = input.fields[d].name;
00150     output.channels[cur_c].values.resize (output.points.size ());
00151     cur_c++;
00152   }
00153 
00154   // Copy the data points
00155   for (size_t cp = 0; cp < output.points.size (); ++cp)
00156   {
00157     // Copy x/y/z
00158     memcpy (&output.points[cp].x, &input.data[cp * input.point_step + x_offset], sizeof (float));
00159     memcpy (&output.points[cp].y, &input.data[cp * input.point_step + y_offset], sizeof (float));
00160     memcpy (&output.points[cp].z, &input.data[cp * input.point_step + z_offset], sizeof (float));
00161     // Copy the rest of the data
00162     int cur_c = 0;
00163     for (size_t d = 0; d < input.fields.size (); ++d)
00164     {
00165       if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
00166         continue;
00167       memcpy (&output.channels[cur_c++].values[cp], &input.data[cp * input.point_step + input.fields[d].offset], sizeof (float));
00168     }
00169   }
00170   return (true);
00171 }
00172 }
00173 #endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H


sensor_msgs
Author(s):
autogenerated on Wed Aug 26 2015 11:01:24