render_shapes.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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: Ioan Sucan */
00036 
00037 #include <moveit/rviz_plugin_render_tools/render_shapes.h>
00038 #include <moveit/rviz_plugin_render_tools/octomap_render.h>
00039 #include <geometric_shapes/mesh_operations.h>
00040 
00041 #include <OgreSceneNode.h>
00042 #include <OgreSceneManager.h>
00043 #include <OgreManualObject.h>
00044 #include <OgreMaterialManager.h>
00045 
00046 #include <rviz/ogre_helpers/shape.h>
00047 #include <rviz/ogre_helpers/mesh_shape.h>
00048 
00049 #include <rviz/display_context.h>
00050 #include <rviz/robot/robot.h>
00051 
00052 #include <boost/lexical_cast.hpp>
00053 #include <boost/math/constants/constants.hpp>
00054 #include <boost/scoped_ptr.hpp>
00055 
00056 namespace moveit_rviz_plugin
00057 {
00058 RenderShapes::RenderShapes(rviz::DisplayContext* context) : context_(context)
00059 {
00060 }
00061 
00062 RenderShapes::~RenderShapes()
00063 {
00064   clear();
00065 }
00066 
00067 void RenderShapes::clear()
00068 {
00069   scene_shapes_.clear();
00070   octree_voxel_grids_.clear();
00071 }
00072 
00073 void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, const Eigen::Affine3d& p,
00074                                OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode,
00075                                const rviz::Color& color, float alpha)
00076 {
00077   rviz::Shape* ogre_shape = NULL;
00078 
00079   // we don't know how to render cones directly, but we can convert them to a mesh
00080   if (s->type == shapes::CONE)
00081   {
00082     boost::scoped_ptr<shapes::Mesh> m(shapes::createMeshFromShape(static_cast<const shapes::Cone&>(*s)));
00083     if (m)
00084       renderShape(node, m.get(), p, octree_voxel_rendering, octree_color_mode, color, alpha);
00085     return;
00086   }
00087 
00088   switch (s->type)
00089   {
00090     case shapes::SPHERE:
00091     {
00092       ogre_shape = new rviz::Shape(rviz::Shape::Sphere, context_->getSceneManager(), node);
00093       double d = 2.0 * static_cast<const shapes::Sphere*>(s)->radius;
00094       ogre_shape->setScale(Ogre::Vector3(d, d, d));
00095     }
00096     break;
00097     case shapes::BOX:
00098     {
00099       ogre_shape = new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(), node);
00100       const double* sz = static_cast<const shapes::Box*>(s)->size;
00101       ogre_shape->setScale(Ogre::Vector3(sz[0], sz[1], sz[2]));
00102     }
00103     break;
00104     case shapes::CYLINDER:
00105     {
00106       ogre_shape = new rviz::Shape(rviz::Shape::Cylinder, context_->getSceneManager(), node);
00107       double d = 2.0 * static_cast<const shapes::Cylinder*>(s)->radius;
00108       double z = static_cast<const shapes::Cylinder*>(s)->length;
00109       ogre_shape->setScale(Ogre::Vector3(d, z, d));  // the shape has z as major axis, but the rendered cylinder has y
00110                                                      // as major axis (assuming z is upright);
00111     }
00112     break;
00113     case shapes::MESH:
00114     {
00115       const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(s);
00116       if (mesh->triangle_count > 0)
00117       {
00118         rviz::MeshShape* m = new rviz::MeshShape(context_->getSceneManager(), node);
00119         ogre_shape = m;
00120 
00121         Ogre::Vector3 normal(0.0, 0.0, 0.0);
00122         for (unsigned int i = 0; i < mesh->triangle_count; ++i)
00123         {
00124           unsigned int i3 = i * 3;
00125           if (mesh->triangle_normals && !mesh->vertex_normals)
00126           {
00127             normal.x = mesh->triangle_normals[i3];
00128             normal.y = mesh->triangle_normals[i3 + 1];
00129             normal.z = mesh->triangle_normals[i3 + 2];
00130           }
00131 
00132           for (int k = 0; k < 3; ++k)
00133           {
00134             unsigned int vi = 3 * mesh->triangles[i3 + k];
00135             Ogre::Vector3 v(mesh->vertices[vi], mesh->vertices[vi + 1], mesh->vertices[vi + 2]);
00136             if (mesh->vertex_normals)
00137             {
00138               Ogre::Vector3 n(mesh->vertex_normals[vi], mesh->vertex_normals[vi + 1], mesh->vertex_normals[vi + 2]);
00139               m->addVertex(v, n);
00140             }
00141             else if (mesh->triangle_normals)
00142               m->addVertex(v, normal);
00143             else
00144               m->addVertex(v);
00145           }
00146         }
00147         m->endTriangles();
00148       }
00149     }
00150     break;
00151 
00152     case shapes::OCTREE:
00153     {
00154       OcTreeRenderPtr octree(new OcTreeRender(static_cast<const shapes::OcTree*>(s)->octree, octree_voxel_rendering,
00155                                               octree_color_mode, 0u, context_->getSceneManager(), node));
00156 
00157       octree_voxel_grids_.push_back(octree);
00158     }
00159     break;
00160 
00161     default:
00162       break;
00163   }
00164 
00165   if (ogre_shape)
00166   {
00167     ogre_shape->setColor(color.r_, color.g_, color.b_, alpha);
00168     Ogre::Vector3 position(p.translation().x(), p.translation().y(), p.translation().z());
00169     Eigen::Quaterniond q(p.rotation());
00170     Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z());
00171 
00172     if (s->type == shapes::CYLINDER)
00173     {
00174       // in geometric shapes, the z axis of the cylinder is it height;
00175       // for the rviz shape, the y axis is the height; we add a transform to fix this
00176       static Ogre::Quaternion fix(Ogre::Radian(boost::math::constants::pi<double>() / 2.0),
00177                                   Ogre::Vector3(1.0, 0.0, 0.0));
00178       orientation = orientation * fix;
00179     }
00180 
00181     ogre_shape->setPosition(position);
00182     ogre_shape->setOrientation(orientation);
00183     scene_shapes_.push_back(boost::shared_ptr<rviz::Shape>(ogre_shape));
00184   }
00185 }
00186 }


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:25:00