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/Octomap.h>
40 #include <octomap/octomap.h>
41 
42 #include <OgreSceneNode.h>
43 #include <OgreSceneManager.h>
44 
46 
47 namespace moveit_rviz_plugin
48 {
49 typedef std::vector<rviz::PointCloud::Point> VPoint;
50 typedef std::vector<VPoint> VVPoint;
51 
52 OcTreeRender::OcTreeRender(const std::shared_ptr<const octomap::OcTree>& octree,
53  OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode,
54  std::size_t max_octree_depth, Ogre::SceneNode* parent_node)
55  : octree_(octree), colorFactor_(0.8)
56 {
57  if (!max_octree_depth)
58  {
59  octree_depth_ = octree->getTreeDepth();
60  }
61  else
62  {
63  octree_depth_ = std::min(max_octree_depth, (std::size_t)octree->getTreeDepth());
64  }
65 
66  scene_node_ = parent_node->createChildSceneNode();
67 
68  cloud_.resize(octree_depth_);
69 
70  for (std::size_t i = 0; i < octree_depth_; ++i)
71  {
72  std::stringstream sname;
73  sname << "PointCloud Nr." << i;
74  cloud_[i] = new rviz::PointCloud();
75  cloud_[i]->setName(sname.str());
76  cloud_[i]->setRenderMode(rviz::PointCloud::RM_BOXES);
77  scene_node_->attachObject(cloud_[i]);
78  }
79 
80  octreeDecoding(octree, octree_voxel_rendering, octree_color_mode);
81 }
82 
84 {
85  scene_node_->detachAllObjects();
86 
87  for (std::size_t i = 0; i < octree_depth_; ++i)
88  {
89  delete cloud_[i];
90  }
91  if (scene_node_->getParentSceneNode())
92  { // when parent scene was already removed, there is no need for this cleanup
93  scene_node_->getParentSceneNode()->removeChild(scene_node_);
94  delete scene_node_;
95  }
96 }
97 
98 void OcTreeRender::setPosition(const Ogre::Vector3& position)
99 {
100  scene_node_->setPosition(position);
101 }
102 
103 void moveit_rviz_plugin::OcTreeRender::setOrientation(const Ogre::Quaternion& orientation)
104 {
105  scene_node_->setOrientation(orientation);
106 }
107 
108 // method taken from octomap_server package
109 void OcTreeRender::setColor(double z_pos, double min_z, double max_z, double color_factor,
111 {
112  int i;
113  double m, n, f;
114 
115  double s = 1.0;
116  double v = 1.0;
117 
118  double h = (1.0 - std::min(std::max((z_pos - min_z) / (max_z - min_z), 0.0), 1.0)) * color_factor;
119 
120  h -= floor(h);
121  h *= 6;
122  i = floor(h);
123  f = h - i;
124  if (!(i & 1))
125  f = 1 - f; // if i is even
126  m = v * (1 - s);
127  n = v * (1 - s * f);
128 
129  switch (i)
130  {
131  case 6:
132  case 0:
133  point->setColor(v, n, m);
134  break;
135  case 1:
136  point->setColor(n, v, m);
137  break;
138  case 2:
139  point->setColor(m, v, n);
140  break;
141  case 3:
142  point->setColor(m, n, v);
143  break;
144  case 4:
145  point->setColor(n, m, v);
146  break;
147  case 5:
148  point->setColor(v, m, n);
149  break;
150  default:
151  point->setColor(1, 0.5, 0.5);
152  break;
153  }
154 }
155 
156 void OcTreeRender::octreeDecoding(const std::shared_ptr<const octomap::OcTree>& octree,
157  OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode)
158 {
159  VVPoint point_buf;
160  point_buf.resize(octree_depth_);
161 
162  // get dimensions of octree
163  double min_x, min_y, min_z, max_x, max_y, max_z;
164  octree->getMetricMin(min_x, min_y, min_z);
165  octree->getMetricMax(max_x, max_y, max_z);
166 
167  unsigned int render_mode_mask = static_cast<unsigned int>(octree_voxel_rendering);
168 
169  {
170  int step_size = 1 << (octree->getTreeDepth() - octree_depth_); // for pruning of occluded voxels
171 
172  // traverse all leafs in the tree:
173  for (octomap::OcTree::iterator it = octree->begin(octree_depth_), end = octree->end(); it != end; ++it)
174  {
175  bool display_voxel = false;
176 
177  // the left part evaluates to 1 for free voxels and 2 for occupied voxels
178  if (((int)octree->isNodeOccupied(*it) + 1) & render_mode_mask)
179  {
180  // check if current voxel has neighbors on all sides -> no need to be displayed
181  bool all_neighbors_found = true;
182 
183  octomap::OcTreeKey key;
184  octomap::OcTreeKey n_key = it.getIndexKey(); // key of the maximum-depth voxel at the current voxel corner
185 
186  // determine indices of potentially neighboring voxels for depths < maximum tree depth
187  // +/-1 at maximum depth, -1 and +depth_difference on other depths
188  int diff_base = 1 << (octree->getTreeDepth() - it.getDepth());
189  int diff[2] = { -1, diff_base };
190 
191  // cells with adjacent faces can occlude a voxel, iterate over the cases x,y,z (idxCase) and +/- (diff)
192  for (unsigned int idx_case = 0; idx_case < 3; ++idx_case)
193  {
194  int idx_0 = idx_case % 3;
195  int idx_1 = (idx_case + 1) % 3;
196  int idx_2 = (idx_case + 2) % 3;
197 
198  for (int i = 0; all_neighbors_found && i < 2; ++i)
199  {
200  key[idx_0] = n_key[idx_0] + diff[i];
201  // if rendering is restricted to treeDepth < maximum tree depth inner nodes with distance step_size can
202  // already occlude a voxel
203  for (key[idx_1] = n_key[idx_1] + diff[0] + 1; all_neighbors_found && key[idx_1] < n_key[idx_1] + diff[1];
204  key[idx_1] += step_size)
205  {
206  for (key[idx_2] = n_key[idx_2] + diff[0] + 1; all_neighbors_found && key[idx_2] < n_key[idx_2] + diff[1];
207  key[idx_2] += step_size)
208  {
209  octomap::OcTreeNode* node = octree->search(key, octree_depth_);
210 
211  // the left part evaluates to 1 for free voxels and 2 for occupied voxels
212  if (!(node && ((((int)octree->isNodeOccupied(node)) + 1) & render_mode_mask)))
213  {
214  // we do not have a neighbor => break!
215  all_neighbors_found = false;
216  }
217  }
218  }
219  }
220  }
221 
222  display_voxel |= !all_neighbors_found;
223  }
224 
225  if (display_voxel)
226  {
227  rviz::PointCloud::Point new_point;
228 
229  new_point.position.x = it.getX();
230  new_point.position.y = it.getY();
231  new_point.position.z = it.getZ();
232 
233  float cell_probability;
234 
235  switch (octree_color_mode)
236  {
238  setColor(new_point.position.z, min_z, max_z, colorFactor_, &new_point);
239  break;
241  cell_probability = it->getOccupancy();
242  new_point.setColor((1.0f - cell_probability), cell_probability, 0.0);
243  break;
244  default:
245  break;
246  }
247 
248  // push to point vectors
249  unsigned int depth = it.getDepth();
250  point_buf[depth - 1].push_back(new_point);
251  }
252  }
253  }
254 
255  for (size_t i = 0; i < octree_depth_; ++i)
256  {
257  double size = octree->getNodeSize(i + 1);
258 
259  cloud_[i]->clear();
260  cloud_[i]->setDimensions(size, size, size);
261 
262  cloud_[i]->addPoints(&point_buf[i].front(), point_buf[i].size());
263  point_buf[i].clear();
264  }
265 }
266 } // namespace moveit_rviz_plugin
moveit_rviz_plugin::OcTreeRender::colorFactor_
double colorFactor_
Definition: octomap_render.h:87
moveit_rviz_plugin::OcTreeRender::setOrientation
void setOrientation(const Ogre::Quaternion &orientation)
Definition: octomap_render.cpp:135
moveit_rviz_plugin::VPoint
std::vector< rviz::PointCloud::Point > VPoint
Definition: octomap_render.cpp:81
octomap_render.h
moveit_rviz_plugin::OcTreeRender::OcTreeRender
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)
Definition: octomap_render.cpp:84
s
XmlRpcServer s
moveit_rviz_plugin::OcTreeRender::cloud_
std::vector< rviz::PointCloud * > cloud_
Definition: octomap_render.h:82
moveit_rviz_plugin::OCTOMAP_Z_AXIS_COLOR
@ OCTOMAP_Z_AXIS_COLOR
Definition: octomap_render.h:60
moveit_rviz_plugin::OcTreeRender::setColor
void setColor(double z_pos, double min_z, double max_z, double color_factor, rviz::PointCloud::Point *point)
Definition: octomap_render.cpp:141
point_cloud.h
rviz::PointCloud
moveit_rviz_plugin::OctreeVoxelRenderMode
OctreeVoxelRenderMode
Definition: octomap_render.h:52
octomap::OcTreeNode
moveit_rviz_plugin::OcTreeRender::~OcTreeRender
virtual ~OcTreeRender()
Definition: octomap_render.cpp:115
rviz::PointCloud::Point::setColor
void setColor(float r, float g, float b, float a=1.0)
octomap::OcTreeKey
point
std::chrono::system_clock::time_point point
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::iterator
leaf_iterator iterator
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit_rviz_plugin::VVPoint
std::vector< VPoint > VVPoint
Definition: octomap_render.cpp:82
m
m
rviz::PointCloud::Point::position
Ogre::Vector3 position
f
f
moveit_rviz_plugin::OcTreeRender::setPosition
void setPosition(const Ogre::Vector3 &position)
Definition: octomap_render.cpp:130
moveit_rviz_plugin::OcTreeRender::octreeDecoding
void octreeDecoding(const std::shared_ptr< const octomap::OcTree > &octree, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode)
Definition: octomap_render.cpp:188
moveit_rviz_plugin::OCTOMAP_PROBABLILTY_COLOR
@ OCTOMAP_PROBABLILTY_COLOR
Definition: octomap_render.h:61
rviz::PointCloud::RM_BOXES
RM_BOXES
octomap.h
v
v
moveit_rviz_plugin::OcTreeRender::scene_node_
Ogre::SceneNode * scene_node_
Definition: octomap_render.h:85
moveit_rviz_plugin::OctreeVoxelColorMode
OctreeVoxelColorMode
Definition: octomap_render.h:58
moveit_rviz_plugin::OcTreeRender::octree_depth_
std::size_t octree_depth_
Definition: octomap_render.h:88


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:26