conversions.cpp
Go to the documentation of this file.
1 // $Id: $
2 
11 /*
12  * Copyright (c) 2010, A. Hornung, University of Freiburg
13  * All rights reserved.
14  *
15  * Redistribution and use in source and binary forms, with or without
16  * modification, are permitted provided that the following conditions are met:
17  *
18  * * Redistributions of source code must retain the above copyright
19  * notice, this list of conditions and the following disclaimer.
20  * * Redistributions in binary form must reproduce the above copyright
21  * notice, this list of conditions and the following disclaimer in the
22  * documentation and/or other materials provided with the distribution.
23  * * Neither the name of the University of Freiburg nor the names of its
24  * contributors may be used to endorse or promote products derived from
25  * this software without specific prior written permission.
26  *
27  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
28  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
29  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
30  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
31  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
32  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
33  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
34  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
35  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
36  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
37  * POSSIBILITY OF SUCH DAMAGE.
38  */
39 
42 
43 namespace octomap {
44 
52  void pointsOctomapToPointCloud2(const point3d_list& points, sensor_msgs::PointCloud2& cloud){
53  // make sure the channel is valid
54  std::vector<sensor_msgs::PointField>::const_iterator field_iter = cloud.fields.begin(), field_end =
55  cloud.fields.end();
56  bool has_x, has_y, has_z;
57  has_x = has_y = has_z = false;
58  while (field_iter != field_end) {
59  if ((field_iter->name == "x") || (field_iter->name == "X"))
60  has_x = true;
61  if ((field_iter->name == "y") || (field_iter->name == "Y"))
62  has_y = true;
63  if ((field_iter->name == "z") || (field_iter->name == "Z"))
64  has_z = true;
65  ++field_iter;
66  }
67 
68  if ((!has_x) || (!has_y) || (!has_z))
69  throw std::runtime_error("One of the fields xyz does not exist");
70 
71  sensor_msgs::PointCloud2Modifier pcd_modifier(cloud);
72  pcd_modifier.resize(points.size());
73 
74  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
75  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
76  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
77 
78  for (point3d_list::const_iterator it = points.begin(); it != points.end(); ++it, ++iter_x, ++iter_y, ++iter_z) {
79  *iter_x = it->x();
80  *iter_y = it->y();
81  *iter_z = it->z();
82  }
83  }
84 
85 
92  void pointCloud2ToOctomap(const sensor_msgs::PointCloud2& cloud, Pointcloud& octomapCloud){
93  octomapCloud.reserve(cloud.data.size() / cloud.point_step);
94 
98 
99  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
100  // Check if the point is invalid
101  if (!std::isnan (*iter_x) && !std::isnan (*iter_y) && !std::isnan (*iter_z))
102  octomapCloud.push_back(*iter_x, *iter_y, *iter_z);
103  }
104  }
105 
106 
107 }
108 
109 
void push_back(float x, float y, float z)
void pointCloud2ToOctomap(const sensor_msgs::PointCloud2 &cloud, Pointcloud &octomapCloud)
Conversion from a sensor_msgs::PointCLoud2 to octomap::Pointcloud, used internally in OctoMap...
Definition: conversions.cpp:92
std::list< octomath::Vector3 > point3d_list
void reserve(size_t size)
void pointsOctomapToPointCloud2(const point3d_list &points, sensor_msgs::PointCloud2 &cloud)
Conversion from octomap::point3d_list (e.g. all occupied nodes from getOccupied()) to sensor_msgs::Po...
Definition: conversions.cpp:52


octomap_ros
Author(s): Armin Hornung
autogenerated on Mon Jun 10 2019 14:03:16