octomap_render.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, 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 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 
00035 /* Author: Julius Kammerl */
00036 
00037 #include <moveit/rviz_plugin_render_tools/octomap_render.h>
00038 
00039 #include <octomap_msgs/Octomap.h>
00040 #include <octomap/octomap.h>
00041 
00042 #include <OgreSceneNode.h>
00043 #include <OgreSceneManager.h>
00044 
00045 #include <rviz/ogre_helpers/point_cloud.h>
00046 
00047 namespace moveit_rviz_plugin
00048 {
00049 typedef std::vector<rviz::PointCloud::Point> VPoint;
00050 typedef std::vector<VPoint> VVPoint;
00051 
00052 OcTreeRender::OcTreeRender(const boost::shared_ptr<const octomap::OcTree>& octree,
00053                            OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode,
00054                            std::size_t max_octree_depth, Ogre::SceneManager* scene_manager,
00055                            Ogre::SceneNode* parent_node = NULL)
00056   : octree_(octree), colorFactor_(0.8)
00057 {
00058   if (!parent_node)
00059   {
00060     parent_node = scene_manager_->getRootSceneNode();
00061   }
00062 
00063   if (!max_octree_depth)
00064   {
00065     octree_depth_ = octree->getTreeDepth();
00066   }
00067   else
00068   {
00069     octree_depth_ = std::min(max_octree_depth, (std::size_t)octree->getTreeDepth());
00070   }
00071 
00072   scene_node_ = parent_node->createChildSceneNode();
00073 
00074   cloud_.resize(octree_depth_);
00075 
00076   for (std::size_t i = 0; i < octree_depth_; ++i)
00077   {
00078     std::stringstream sname;
00079     sname << "PointCloud Nr." << i;
00080     cloud_[i] = new rviz::PointCloud();
00081     cloud_[i]->setName(sname.str());
00082     cloud_[i]->setRenderMode(rviz::PointCloud::RM_BOXES);
00083     scene_node_->attachObject(cloud_[i]);
00084   }
00085 
00086   octreeDecoding(octree, octree_voxel_rendering, octree_color_mode);
00087 }
00088 
00089 OcTreeRender::~OcTreeRender()
00090 {
00091   scene_node_->detachAllObjects();
00092 
00093   for (std::size_t i = 0; i < octree_depth_; ++i)
00094   {
00095     delete cloud_[i];
00096   }
00097 }
00098 
00099 // method taken from octomap_server package
00100 void OcTreeRender::setColor(double z_pos, double min_z, double max_z, double color_factor,
00101                             rviz::PointCloud::Point* point)
00102 {
00103   int i;
00104   double m, n, f;
00105 
00106   double s = 1.0;
00107   double v = 1.0;
00108 
00109   double h = (1.0 - std::min(std::max((z_pos - min_z) / (max_z - min_z), 0.0), 1.0)) * color_factor;
00110 
00111   h -= floor(h);
00112   h *= 6;
00113   i = floor(h);
00114   f = h - i;
00115   if (!(i & 1))
00116     f = 1 - f;  // if i is even
00117   m = v * (1 - s);
00118   n = v * (1 - s * f);
00119 
00120   switch (i)
00121   {
00122     case 6:
00123     case 0:
00124       point->setColor(v, n, m);
00125       break;
00126     case 1:
00127       point->setColor(n, v, m);
00128       break;
00129     case 2:
00130       point->setColor(m, v, n);
00131       break;
00132     case 3:
00133       point->setColor(m, n, v);
00134       break;
00135     case 4:
00136       point->setColor(n, m, v);
00137       break;
00138     case 5:
00139       point->setColor(v, m, n);
00140       break;
00141     default:
00142       point->setColor(1, 0.5, 0.5);
00143       break;
00144   }
00145 }
00146 
00147 void OcTreeRender::octreeDecoding(const boost::shared_ptr<const octomap::OcTree>& octree,
00148                                   OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode)
00149 {
00150   VVPoint pointBuf_;
00151   pointBuf_.resize(octree_depth_);
00152 
00153   // get dimensions of octree
00154   double minX, minY, minZ, maxX, maxY, maxZ;
00155   octree->getMetricMin(minX, minY, minZ);
00156   octree->getMetricMax(maxX, maxY, maxZ);
00157 
00158   unsigned int render_mode_mask = static_cast<unsigned int>(octree_voxel_rendering);
00159 
00160   size_t pointCount = 0;
00161   {
00162     // traverse all leafs in the tree:
00163     for (octomap::OcTree::iterator it = octree->begin(octree_depth_), end = octree->end(); it != end; ++it)
00164     {
00165       bool display_voxel = false;
00166 
00167       // the left part evaluates to 1 for free voxels and 2 for occupied voxels
00168       if (((int)octree->isNodeOccupied(*it) + 1) & render_mode_mask)
00169       {
00170         // check if current voxel has neighbors on all sides -> no need to be displayed
00171         bool allNeighborsFound = true;
00172 
00173         octomap::OcTreeKey key;
00174         octomap::OcTreeKey nKey = it.getKey();
00175 
00176         for (key[2] = nKey[2] - 1; allNeighborsFound && key[2] <= nKey[2] + 1; ++key[2])
00177         {
00178           for (key[1] = nKey[1] - 1; allNeighborsFound && key[1] <= nKey[1] + 1; ++key[1])
00179           {
00180             for (key[0] = nKey[0] - 1; allNeighborsFound && key[0] <= nKey[0] + 1; ++key[0])
00181             {
00182               if (key != nKey)
00183               {
00184                 octomap::OcTreeNode* node = octree->search(key);
00185 
00186                 // the left part evaluates to 1 for free voxels and 2 for occupied voxels
00187                 if (!(node && (((int)octree->isNodeOccupied(node)) + 1) & render_mode_mask))
00188                 {
00189                   // we do not have a neighbor => break!
00190                   allNeighborsFound = false;
00191                 }
00192               }
00193             }
00194           }
00195         }
00196 
00197         display_voxel |= !allNeighborsFound;
00198       }
00199 
00200       if (display_voxel)
00201       {
00202         rviz::PointCloud::Point newPoint;
00203 
00204         newPoint.position.x = it.getX();
00205         newPoint.position.y = it.getY();
00206         newPoint.position.z = it.getZ();
00207 
00208         float cell_probability;
00209 
00210         switch (octree_color_mode)
00211         {
00212           case OCTOMAP_Z_AXIS_COLOR:
00213             setColor(newPoint.position.z, minZ, maxZ, colorFactor_, &newPoint);
00214             break;
00215           case OCTOMAP_PROBABLILTY_COLOR:
00216             cell_probability = it->getOccupancy();
00217             newPoint.setColor((1.0f - cell_probability), cell_probability, 0.0);
00218             break;
00219           default:
00220             break;
00221         }
00222 
00223         // push to point vectors
00224         unsigned int depth = it.getDepth();
00225         pointBuf_[depth - 1].push_back(newPoint);
00226 
00227         ++pointCount;
00228       }
00229     }
00230   }
00231 
00232   for (size_t i = 0; i < octree_depth_; ++i)
00233   {
00234     double size = octree->getNodeSize(i + 1);
00235 
00236     cloud_[i]->clear();
00237     cloud_[i]->setDimensions(size, size, size);
00238 
00239     cloud_[i]->addPoints(&pointBuf_[i].front(), pointBuf_[i].size());
00240     pointBuf_[i].clear();
00241   }
00242 }
00243 }


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:22:14