moveit2
The MoveIt Motion Planning Framework for ROS 2.
octomap_render.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Julius Kammerl */
36 
38 
39 #include <octomap_msgs/msg/octomap.hpp>
40 #include <octomap/octomap.h>
41 
42 #include <OgreSceneNode.h>
43 #include <OgreSceneManager.h>
44 
45 namespace moveit_rviz_plugin
46 {
47 typedef std::vector<rviz_rendering::PointCloud::Point> VPoint;
48 typedef std::vector<VPoint> VVPoint;
49 
50 OcTreeRender::OcTreeRender(const std::shared_ptr<const octomap::OcTree>& octree,
51  OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode,
52  std::size_t max_octree_depth, Ogre::SceneNode* parent_node)
53  : octree_(octree), colorFactor_(0.8)
54 {
55  if (!max_octree_depth)
56  {
57  octree_depth_ = octree->getTreeDepth();
58  }
59  else
60  {
61  octree_depth_ = std::min(max_octree_depth, (std::size_t)octree->getTreeDepth());
62  }
63 
64  scene_node_ = parent_node->createChildSceneNode();
65 
66  cloud_.resize(octree_depth_);
67 
68  for (std::size_t i = 0; i < octree_depth_; ++i)
69  {
70  std::stringstream sname;
71  sname << "PointCloud Nr." << i;
72  cloud_[i] = new rviz_rendering::PointCloud();
73  cloud_[i]->setName(sname.str());
74  cloud_[i]->setRenderMode(rviz_rendering::PointCloud::RM_BOXES);
75  scene_node_->attachObject(cloud_[i]);
76  }
77 
78  octreeDecoding(octree, octree_voxel_rendering, octree_color_mode);
79 }
80 
82 {
83  scene_node_->detachAllObjects();
84 
85  for (std::size_t i = 0; i < octree_depth_; ++i)
86  {
87  delete cloud_[i];
88  }
89 }
90 
91 void OcTreeRender::setPosition(const Ogre::Vector3& position)
92 {
93  scene_node_->setPosition(position);
94 }
95 
96 void moveit_rviz_plugin::OcTreeRender::setOrientation(const Ogre::Quaternion& orientation)
97 {
98  scene_node_->setOrientation(orientation);
99 }
100 
101 // method taken from octomap_server package
102 void OcTreeRender::setColor(double z_pos, double min_z, double max_z, double color_factor,
103  rviz_rendering::PointCloud::Point* point)
104 {
105  int i;
106  double m, n, f;
107 
108  double s = 1.0;
109  double v = 1.0;
110 
111  double h = (1.0 - std::min(std::max((z_pos - min_z) / (max_z - min_z), 0.0), 1.0)) * color_factor;
112 
113  h -= floor(h);
114  h *= 6;
115  i = floor(h);
116  f = h - i;
117  if (!(i & 1))
118  f = 1 - f; // if i is even
119  m = v * (1 - s);
120  n = v * (1 - s * f);
121 
122  switch (i)
123  {
124  case 6:
125  case 0:
126  point->setColor(v, n, m);
127  break;
128  case 1:
129  point->setColor(n, v, m);
130  break;
131  case 2:
132  point->setColor(m, v, n);
133  break;
134  case 3:
135  point->setColor(m, n, v);
136  break;
137  case 4:
138  point->setColor(n, m, v);
139  break;
140  case 5:
141  point->setColor(v, m, n);
142  break;
143  default:
144  point->setColor(1, 0.5, 0.5);
145  break;
146  }
147 }
148 
149 void OcTreeRender::octreeDecoding(const std::shared_ptr<const octomap::OcTree>& octree,
150  OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode)
151 {
152  VVPoint point_buf;
153  point_buf.resize(octree_depth_);
154 
155  // get dimensions of octree
156  double min_x, min_y, min_z, max_x, max_y, max_z;
157  octree->getMetricMin(min_x, min_y, min_z);
158  octree->getMetricMax(max_x, max_y, max_z);
159 
160  unsigned int render_mode_mask = static_cast<unsigned int>(octree_voxel_rendering);
161 
162  size_t point_count = 0;
163  {
164  int step_size = 1 << (octree->getTreeDepth() - octree_depth_); // for pruning of occluded voxels
165 
166  // traverse all leafs in the tree:
167  for (octomap::OcTree::iterator it = octree->begin(octree_depth_), end = octree->end(); it != end; ++it)
168  {
169  bool display_voxel = false;
170 
171  // the left part evaluates to 1 for free voxels and 2 for occupied voxels
172  if ((static_cast<int>(octree->isNodeOccupied(*it)) + 1) & render_mode_mask)
173  {
174  // check if current voxel has neighbors on all sides -> no need to be displayed
175  bool all_neighbors_found = true;
176 
177  octomap::OcTreeKey key;
178  octomap::OcTreeKey n_key = it.getIndexKey(); // key of the maximum-depth voxel at the current voxel corner
179 
180  // determine indices of potentially neighboring voxels for depths < maximum tree depth
181  // +/-1 at maximum depth, -1 and +depth_difference on other depths
182  int diff_base = 1 << (octree->getTreeDepth() - it.getDepth());
183  int diff[2] = { -1, diff_base };
184 
185  // cells with adjacent faces can occlude a voxel, iterate over the cases x,y,z (idxCase) and +/- (diff)
186  for (unsigned int idx_case = 0; idx_case < 3; ++idx_case)
187  {
188  int idx_0 = idx_case % 3;
189  int idx_1 = (idx_case + 1) % 3;
190  int idx_2 = (idx_case + 2) % 3;
191 
192  for (int i = 0; all_neighbors_found && i < 2; ++i)
193  {
194  key[idx_0] = n_key[idx_0] + diff[i];
195  // if rendering is restricted to treeDepth < maximum tree depth inner nodes with distance step_size can
196  // already occlude a voxel
197  for (key[idx_1] = n_key[idx_1] + diff[0] + 1; all_neighbors_found && key[idx_1] < n_key[idx_1] + diff[1];
198  key[idx_1] += step_size)
199  {
200  for (key[idx_2] = n_key[idx_2] + diff[0] + 1; all_neighbors_found && key[idx_2] < n_key[idx_2] + diff[1];
201  key[idx_2] += step_size)
202  {
203  octomap::OcTreeNode* node = octree->search(key, octree_depth_);
204 
205  // the left part evaluates to 1 for free voxels and 2 for occupied voxels
206  if (!(node && (((static_cast<int>(octree->isNodeOccupied(node))) + 1) & render_mode_mask)))
207  {
208  // we do not have a neighbor => break!
209  all_neighbors_found = false;
210  }
211  }
212  }
213  }
214  }
215 
216  display_voxel |= !all_neighbors_found;
217  }
218 
219  if (display_voxel)
220  {
221  rviz_rendering::PointCloud::Point new_point;
222 
223  new_point.position.x = it.getX();
224  new_point.position.y = it.getY();
225  new_point.position.z = it.getZ();
226 
227  float cell_probability;
228 
229  switch (octree_color_mode)
230  {
232  setColor(new_point.position.z, min_z, max_z, colorFactor_, &new_point);
233  break;
235  cell_probability = it->getOccupancy();
236  new_point.setColor((1.0f - cell_probability), cell_probability, 0.0);
237  break;
238  default:
239  break;
240  }
241 
242  // push to point vectors
243  unsigned int depth = it.getDepth();
244  point_buf[depth - 1].push_back(new_point);
245 
246  ++point_count;
247  }
248  }
249  }
250 
251  for (size_t i = 0; i < octree_depth_; ++i)
252  {
253  double size = octree->getNodeSize(i + 1);
254 
255  cloud_[i]->clear();
256  cloud_[i]->setDimensions(size, size, size);
257 
258  cloud_[i]->addPoints(point_buf[i].begin(), point_buf[i].end());
259  point_buf[i].clear();
260  }
261 }
262 } // namespace moveit_rviz_plugin
void setOrientation(const Ogre::Quaternion &orientation)
OcTreeRender(const std::shared_ptr< const octomap::OcTree > &octree, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, std::size_t max_octree_depth, Ogre::SceneNode *parent_node)
void setPosition(const Ogre::Vector3 &position)
std::vector< rviz_rendering::PointCloud::Point > VPoint
std::vector< VPoint > VVPoint