planning_scene_render.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/planning_scene_render.h>
00038 #include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
00039 #include <moveit/rviz_plugin_render_tools/render_shapes.h>
00040 #include <rviz/display_context.h>
00041 
00042 #include <OgreSceneNode.h>
00043 #include <OgreSceneManager.h>
00044 
00045 namespace moveit_rviz_plugin
00046 {
00047 PlanningSceneRender::PlanningSceneRender(Ogre::SceneNode* node, rviz::DisplayContext* context,
00048                                          const RobotStateVisualizationPtr& robot)
00049   : planning_scene_geometry_node_(node->createChildSceneNode()), context_(context), scene_robot_(robot)
00050 {
00051   render_shapes_.reset(new RenderShapes(context));
00052 }
00053 
00054 PlanningSceneRender::~PlanningSceneRender()
00055 {
00056   context_->getSceneManager()->destroySceneNode(planning_scene_geometry_node_->getName());
00057 }
00058 
00059 void PlanningSceneRender::clear()
00060 {
00061   render_shapes_->clear();
00062 }
00063 
00064 void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningSceneConstPtr& scene,
00065                                               const rviz::Color& default_env_color,
00066                                               const rviz::Color& default_attached_color,
00067                                               OctreeVoxelRenderMode octree_voxel_rendering,
00068                                               OctreeVoxelColorMode octree_color_mode, float default_scene_alpha)
00069 {
00070   if (!scene)
00071     return;
00072 
00073   clear();
00074 
00075   if (scene_robot_)
00076   {
00077     robot_state::RobotState* rs = new robot_state::RobotState(scene->getCurrentState());
00078     rs->update();
00079 
00080     std_msgs::ColorRGBA color;
00081     color.r = default_attached_color.r_;
00082     color.g = default_attached_color.g_;
00083     color.b = default_attached_color.b_;
00084     color.a = 1.0f;
00085     planning_scene::ObjectColorMap color_map;
00086     scene->getKnownObjectColors(color_map);
00087     scene_robot_->update(robot_state::RobotStateConstPtr(rs), color, color_map);
00088   }
00089 
00090   const std::vector<std::string>& ids = scene->getWorld()->getObjectIds();
00091   for (std::size_t i = 0; i < ids.size(); ++i)
00092   {
00093     collision_detection::CollisionWorld::ObjectConstPtr o = scene->getWorld()->getObject(ids[i]);
00094     rviz::Color color = default_env_color;
00095     float alpha = default_scene_alpha;
00096     if (scene->hasObjectColor(ids[i]))
00097     {
00098       const std_msgs::ColorRGBA& c = scene->getObjectColor(ids[i]);
00099       color.r_ = c.r;
00100       color.g_ = c.g;
00101       color.b_ = c.b;
00102       alpha = c.a;
00103     }
00104     for (std::size_t j = 0; j < o->shapes_.size(); ++j)
00105       render_shapes_->renderShape(planning_scene_geometry_node_, o->shapes_[j].get(), o->shape_poses_[j],
00106                                   octree_voxel_rendering, octree_color_mode, color, alpha);
00107   }
00108 }
00109 }


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