conversions.cpp
Go to the documentation of this file.
00001 // $Id:  $
00002 
00011 /*
00012  * Copyright (c) 2010, A. Hornung, University of Freiburg
00013  * All rights reserved.
00014  *
00015  * Redistribution and use in source and binary forms, with or without
00016  * modification, are permitted provided that the following conditions are met:
00017  *
00018  *     * Redistributions of source code must retain the above copyright
00019  *       notice, this list of conditions and the following disclaimer.
00020  *     * Redistributions in binary form must reproduce the above copyright
00021  *       notice, this list of conditions and the following disclaimer in the
00022  *       documentation and/or other materials provided with the distribution.
00023  *     * Neither the name of the University of Freiburg nor the names of its
00024  *       contributors may be used to endorse or promote products derived from
00025  *       this software without specific prior written permission.
00026  *
00027  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00028  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00029  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00030  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00031  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00032  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00033  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00034  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00035  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00036  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00037  * POSSIBILITY OF SUCH DAMAGE.
00038  */
00039 
00040 #include <sensor_msgs/point_cloud2_iterator.h>
00041 #include <octomap_ros/conversions.h>
00042 
00043 namespace octomap {
00044 
00052   void pointsOctomapToPointCloud2(const point3d_list& points, sensor_msgs::PointCloud2& cloud){
00053     // make sure the channel is valid
00054     std::vector<sensor_msgs::PointField>::const_iterator field_iter = cloud.fields.begin(), field_end =
00055         cloud.fields.end();
00056     bool has_x, has_y, has_z;
00057     has_x = has_y = has_z = false;
00058     while (field_iter != field_end) {
00059       if ((field_iter->name == "x") || (field_iter->name == "X"))
00060         has_x = true;
00061       if ((field_iter->name == "y") || (field_iter->name == "Y"))
00062         has_y = true;
00063       if ((field_iter->name == "z") || (field_iter->name == "Z"))
00064         has_z = true;
00065       ++field_iter;
00066     }
00067 
00068     if ((!has_x) || (!has_y) || (!has_z))
00069       throw std::runtime_error("One of the fields xyz does not exist");
00070 
00071     sensor_msgs::PointCloud2Modifier pcd_modifier(cloud);
00072     pcd_modifier.resize(points.size());
00073 
00074     sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
00075     sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
00076     sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
00077 
00078     for (point3d_list::const_iterator it = points.begin(); it != points.end(); ++it, ++iter_x, ++iter_y, ++iter_z) {
00079       *iter_x = it->x();
00080       *iter_y = it->y();
00081       *iter_z = it->z();
00082     }
00083   }
00084 
00085 
00092   void pointCloud2ToOctomap(const sensor_msgs::PointCloud2& cloud, Pointcloud& octomapCloud){
00093     octomapCloud.reserve(cloud.data.size() / cloud.point_step);
00094 
00095     sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
00096     sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
00097     sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
00098 
00099     for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
00100       // Check if the point is invalid
00101       if (!std::isnan (*iter_x) && !std::isnan (*iter_y) && !std::isnan (*iter_z))
00102         octomapCloud.push_back(*iter_x, *iter_y, *iter_z);
00103     }
00104   }
00105 
00106 
00107 }
00108 
00109 


octomap_ros
Author(s): Armin Hornung
autogenerated on Sat Jun 8 2019 20:07:37