planning_scene_display.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  Copyright (c) 2013, Ioan A. Sucan
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Ioan Sucan */
00037 
00038 #include <moveit/planning_scene_rviz_plugin/planning_scene_display.h>
00039 #include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
00040 #include <moveit/rviz_plugin_render_tools/octomap_render.h>
00041 
00042 #include <rviz/visualization_manager.h>
00043 #include <rviz/robot/robot.h>
00044 #include <rviz/robot/robot_link.h>
00045 
00046 #include <rviz/properties/property.h>
00047 #include <rviz/properties/string_property.h>
00048 #include <rviz/properties/bool_property.h>
00049 #include <rviz/properties/float_property.h>
00050 #include <rviz/properties/ros_topic_property.h>
00051 #include <rviz/properties/color_property.h>
00052 #include <rviz/properties/enum_property.h>
00053 #include <rviz/display_context.h>
00054 #include <rviz/frame_manager.h>
00055 #include <tf/transform_listener.h>
00056 
00057 #include <OgreSceneManager.h>
00058 #include <OgreSceneNode.h>
00059 
00060 namespace moveit_rviz_plugin
00061 {
00062 // ******************************************************************************************
00063 // Base class contructor
00064 // ******************************************************************************************
00065 PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool show_scene_robot)
00066   : Display(), model_is_loading_(false), planning_scene_needs_render_(true), current_scene_time_(0.0f)
00067 {
00068   move_group_ns_property_ = new rviz::StringProperty("Move Group Namespace", "", "The name of the ROS namespace in "
00069                                                                                  "which the move_group node is running",
00070                                                      this, SLOT(changedMoveGroupNS()), this);
00071   robot_description_property_ = new rviz::StringProperty(
00072       "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
00073       this, SLOT(changedRobotDescription()), this);
00074 
00075   if (listen_to_planning_scene)
00076     planning_scene_topic_property_ =
00077         new rviz::RosTopicProperty("Planning Scene Topic", "move_group/monitored_planning_scene",
00078                                    ros::message_traits::datatype<moveit_msgs::PlanningScene>(),
00079                                    "The topic on which the moveit_msgs::PlanningScene messages are received", this,
00080                                    SLOT(changedPlanningSceneTopic()), this);
00081   else
00082     planning_scene_topic_property_ = NULL;
00083 
00084   // Planning scene category -------------------------------------------------------------------------------------------
00085   scene_category_ = new rviz::Property("Scene Geometry", QVariant(), "", this);
00086 
00087   scene_name_property_ = new rviz::StringProperty("Scene Name", "(noname)", "Shows the name of the planning scene",
00088                                                   scene_category_, SLOT(changedSceneName()), this);
00089   scene_name_property_->setShouldBeSaved(false);
00090   scene_enabled_property_ =
00091       new rviz::BoolProperty("Show Scene Geometry", true, "Indicates whether planning scenes should be displayed",
00092                              scene_category_, SLOT(changedSceneEnabled()), this);
00093 
00094   scene_alpha_property_ = new rviz::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry",
00095                                                   scene_category_, SLOT(changedSceneAlpha()), this);
00096   scene_alpha_property_->setMin(0.0);
00097   scene_alpha_property_->setMax(1.0);
00098 
00099   scene_color_property_ = new rviz::ColorProperty(
00100       "Scene Color", QColor(50, 230, 50), "The color for the planning scene obstacles (if a color is not defined)",
00101       scene_category_, SLOT(changedSceneColor()), this);
00102 
00103   octree_render_property_ = new rviz::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.",
00104                                                    scene_category_, SLOT(changedOctreeRenderMode()), this);
00105 
00106   octree_render_property_->addOption("Occupied Voxels", OCTOMAP_OCCUPIED_VOXELS);
00107   octree_render_property_->addOption("Free Voxels", OCTOMAP_FREE_VOXELS);
00108   octree_render_property_->addOption("All Voxels", OCTOMAP_FREE_VOXELS | OCTOMAP_OCCUPIED_VOXELS);
00109 
00110   octree_coloring_property_ = new rviz::EnumProperty("Voxel Coloring", "Z-Axis", "Select voxel coloring mode",
00111                                                      scene_category_, SLOT(changedOctreeColorMode()), this);
00112 
00113   octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR);
00114   octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR);
00115 
00116   scene_display_time_property_ =
00117       new rviz::FloatProperty("Scene Display Time", 0.2f, "The amount of wall-time to wait in between rendering "
00118                                                           "updates to the planning scene (if any)",
00119                               scene_category_, SLOT(changedSceneDisplayTime()), this);
00120   scene_display_time_property_->setMin(0.0001);
00121 
00122   if (show_scene_robot)
00123   {
00124     robot_category_ = new rviz::Property("Scene Robot", QVariant(), "", this);
00125 
00126     scene_robot_visual_enabled_property_ = new rviz::BoolProperty(
00127         "Show Robot Visual", true, "Indicates whether the robot state specified by the planning scene should be "
00128                                    "displayed as defined for visualisation purposes.",
00129         robot_category_, SLOT(changedSceneRobotVisualEnabled()), this);
00130 
00131     scene_robot_collision_enabled_property_ = new rviz::BoolProperty(
00132         "Show Robot Collision", false, "Indicates whether the robot state specified by the planning scene should be "
00133                                        "displayed as defined for collision detection purposes.",
00134         robot_category_, SLOT(changedSceneRobotCollisionEnabled()), this);
00135 
00136     robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 1.0f, "Specifies the alpha for the robot links",
00137                                                     robot_category_, SLOT(changedRobotSceneAlpha()), this);
00138     robot_alpha_property_->setMin(0.0);
00139     robot_alpha_property_->setMax(1.0);
00140 
00141     attached_body_color_property_ =
00142         new rviz::ColorProperty("Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies",
00143                                 robot_category_, SLOT(changedAttachedBodyColor()), this);
00144   }
00145   else
00146   {
00147     robot_category_ = NULL;
00148     scene_robot_visual_enabled_property_ = NULL;
00149     scene_robot_collision_enabled_property_ = NULL;
00150     robot_alpha_property_ = NULL;
00151     attached_body_color_property_ = NULL;
00152   }
00153 }
00154 
00155 // ******************************************************************************************
00156 // Deconstructor
00157 // ******************************************************************************************
00158 PlanningSceneDisplay::~PlanningSceneDisplay()
00159 {
00160   clearJobs();
00161 
00162   planning_scene_render_.reset();
00163   if (context_ && context_->getSceneManager() && planning_scene_node_)
00164     context_->getSceneManager()->destroySceneNode(planning_scene_node_->getName());
00165   if (planning_scene_robot_)
00166     planning_scene_robot_.reset();
00167   planning_scene_monitor_.reset();
00168 }
00169 
00170 void PlanningSceneDisplay::clearJobs()
00171 {
00172   background_process_.clear();
00173   {
00174     boost::unique_lock<boost::mutex> ulock(main_loop_jobs_lock_);
00175     main_loop_jobs_.clear();
00176   }
00177 }
00178 
00179 void PlanningSceneDisplay::onInitialize()
00180 {
00181   Display::onInitialize();
00182 
00183   // the scene node that contains everything
00184   planning_scene_node_ = scene_node_->createChildSceneNode();
00185 
00186   if (robot_category_)
00187   {
00188     planning_scene_robot_.reset(
00189         new RobotStateVisualization(planning_scene_node_, context_, "Planning Scene", robot_category_));
00190     planning_scene_robot_->setVisible(true);
00191     planning_scene_robot_->setVisualVisible(scene_robot_visual_enabled_property_->getBool());
00192     planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());
00193     changedRobotSceneAlpha();
00194     changedAttachedBodyColor();
00195   }
00196 }
00197 
00198 void PlanningSceneDisplay::reset()
00199 {
00200   planning_scene_render_.reset();
00201   if (planning_scene_robot_)
00202     planning_scene_robot_->clear();
00203 
00204   addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
00205   Display::reset();
00206 
00207   if (planning_scene_robot_)
00208   {
00209     planning_scene_robot_->setVisible(true);
00210     planning_scene_robot_->setVisualVisible(scene_robot_visual_enabled_property_->getBool());
00211     planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());
00212   }
00213 }
00214 
00215 void PlanningSceneDisplay::addBackgroundJob(const boost::function<void()>& job, const std::string& name)
00216 {
00217   background_process_.addJob(job, name);
00218 }
00219 
00220 void PlanningSceneDisplay::spawnBackgroundJob(const boost::function<void()>& job)
00221 {
00222   boost::thread t(job);
00223 }
00224 
00225 void PlanningSceneDisplay::addMainLoopJob(const boost::function<void()>& job)
00226 {
00227   boost::unique_lock<boost::mutex> ulock(main_loop_jobs_lock_);
00228   main_loop_jobs_.push_back(job);
00229 }
00230 
00231 void PlanningSceneDisplay::waitForAllMainLoopJobs()
00232 {
00233   boost::unique_lock<boost::mutex> ulock(main_loop_jobs_lock_);
00234   while (!main_loop_jobs_.empty())
00235     main_loop_jobs_empty_condition_.wait(ulock);
00236 }
00237 
00238 void PlanningSceneDisplay::executeMainLoopJobs()
00239 {
00240   main_loop_jobs_lock_.lock();
00241   while (!main_loop_jobs_.empty())
00242   {
00243     boost::function<void()> fn = main_loop_jobs_.front();
00244     main_loop_jobs_.pop_front();
00245     main_loop_jobs_lock_.unlock();
00246     try
00247     {
00248       fn();
00249     }
00250     catch (std::runtime_error& ex)
00251     {
00252       ROS_ERROR("Exception caught executing main loop job: %s", ex.what());
00253     }
00254     catch (...)
00255     {
00256       ROS_ERROR("Exception caught executing main loop job");
00257     }
00258     main_loop_jobs_lock_.lock();
00259   }
00260   main_loop_jobs_empty_condition_.notify_all();
00261   main_loop_jobs_lock_.unlock();
00262 }
00263 
00264 const planning_scene_monitor::PlanningSceneMonitorPtr& PlanningSceneDisplay::getPlanningSceneMonitor()
00265 {
00266   return planning_scene_monitor_;
00267 }
00268 
00269 const std::string PlanningSceneDisplay::getMoveGroupNS() const
00270 {
00271   return move_group_ns_property_->getStdString();
00272 }
00273 
00274 const robot_model::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() const
00275 {
00276   if (planning_scene_monitor_)
00277     return planning_scene_monitor_->getRobotModel();
00278   else
00279   {
00280     static robot_model::RobotModelConstPtr empty;
00281     return empty;
00282   }
00283 }
00284 
00285 bool PlanningSceneDisplay::waitForCurrentRobotState(const ros::Time& t)
00286 {
00287   if (planning_scene_monitor_)
00288     return planning_scene_monitor_->waitForCurrentRobotState(t);
00289   return false;
00290 }
00291 
00292 planning_scene_monitor::LockedPlanningSceneRO PlanningSceneDisplay::getPlanningSceneRO() const
00293 {
00294   return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);
00295 }
00296 
00297 planning_scene_monitor::LockedPlanningSceneRW PlanningSceneDisplay::getPlanningSceneRW()
00298 {
00299   return planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_);
00300 }
00301 
00302 void PlanningSceneDisplay::changedAttachedBodyColor()
00303 {
00304   queueRenderSceneGeometry();
00305 }
00306 
00307 void PlanningSceneDisplay::changedSceneColor()
00308 {
00309   queueRenderSceneGeometry();
00310 }
00311 
00312 void PlanningSceneDisplay::changedMoveGroupNS()
00313 {
00314   if (isEnabled())
00315     reset();
00316 }
00317 
00318 void PlanningSceneDisplay::changedRobotDescription()
00319 {
00320   if (isEnabled())
00321     reset();
00322 }
00323 
00324 void PlanningSceneDisplay::changedSceneName()
00325 {
00326   planning_scene_monitor::LockedPlanningSceneRW ps = getPlanningSceneRW();
00327   if (ps)
00328     ps->setName(scene_name_property_->getStdString());
00329 }
00330 
00331 void PlanningSceneDisplay::renderPlanningScene()
00332 {
00333   if (planning_scene_render_ && planning_scene_needs_render_)
00334   {
00335     QColor color = scene_color_property_->getColor();
00336     rviz::Color env_color(color.redF(), color.greenF(), color.blueF());
00337     if (attached_body_color_property_)
00338       color = attached_body_color_property_->getColor();
00339     rviz::Color attached_color(color.redF(), color.greenF(), color.blueF());
00340 
00341     try
00342     {
00343       const planning_scene_monitor::LockedPlanningSceneRO& ps = getPlanningSceneRO();
00344       planning_scene_render_->renderPlanningScene(
00345           ps, env_color, attached_color, static_cast<OctreeVoxelRenderMode>(octree_render_property_->getOptionInt()),
00346           static_cast<OctreeVoxelColorMode>(octree_coloring_property_->getOptionInt()),
00347           scene_alpha_property_->getFloat());
00348     }
00349     catch (...)
00350     {
00351       ROS_ERROR("Exception thrown while rendering planning scene");
00352     }
00353     planning_scene_needs_render_ = false;
00354     planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
00355   }
00356 }
00357 
00358 void PlanningSceneDisplay::changedSceneAlpha()
00359 {
00360   queueRenderSceneGeometry();
00361 }
00362 
00363 void PlanningSceneDisplay::changedRobotSceneAlpha()
00364 {
00365   if (planning_scene_robot_)
00366   {
00367     planning_scene_robot_->setAlpha(robot_alpha_property_->getFloat());
00368     queueRenderSceneGeometry();
00369   }
00370 }
00371 
00372 void PlanningSceneDisplay::changedPlanningSceneTopic()
00373 {
00374   if (planning_scene_monitor_ && planning_scene_topic_property_)
00375   {
00376     planning_scene_monitor_->startSceneMonitor(planning_scene_topic_property_->getStdString());
00377     std::string service_name = planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE;
00378     if (!getMoveGroupNS().empty())
00379       service_name = ros::names::append(getMoveGroupNS(), service_name);
00380     planning_scene_monitor_->requestPlanningSceneState(service_name);
00381   }
00382 }
00383 
00384 void PlanningSceneDisplay::changedSceneDisplayTime()
00385 {
00386 }
00387 
00388 void PlanningSceneDisplay::changedOctreeRenderMode()
00389 {
00390 }
00391 
00392 void PlanningSceneDisplay::changedOctreeColorMode()
00393 {
00394 }
00395 
00396 void PlanningSceneDisplay::changedSceneRobotVisualEnabled()
00397 {
00398   if (isEnabled() && planning_scene_robot_)
00399   {
00400     planning_scene_robot_->setVisible(true);
00401     planning_scene_robot_->setVisualVisible(scene_robot_visual_enabled_property_->getBool());
00402   }
00403 }
00404 
00405 void PlanningSceneDisplay::changedSceneRobotCollisionEnabled()
00406 {
00407   if (isEnabled() && planning_scene_robot_)
00408   {
00409     planning_scene_robot_->setVisible(true);
00410     planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());
00411   }
00412 }
00413 
00414 void PlanningSceneDisplay::changedSceneEnabled()
00415 {
00416   if (planning_scene_render_)
00417     planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
00418 }
00419 
00420 void PlanningSceneDisplay::setGroupColor(rviz::Robot* robot, const std::string& group_name, const QColor& color)
00421 {
00422   if (getRobotModel())
00423   {
00424     const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name);
00425     if (jmg)
00426     {
00427       const std::vector<std::string>& links = jmg->getLinkModelNamesWithCollisionGeometry();
00428       for (std::size_t i = 0; i < links.size(); ++i)
00429         setLinkColor(robot, links[i], color);
00430     }
00431   }
00432 }
00433 
00434 void PlanningSceneDisplay::unsetAllColors(rviz::Robot* robot)
00435 {
00436   if (getRobotModel())
00437   {
00438     const std::vector<std::string>& links = getRobotModel()->getLinkModelNamesWithCollisionGeometry();
00439     for (std::size_t i = 0; i < links.size(); ++i)
00440       unsetLinkColor(robot, links[i]);
00441   }
00442 }
00443 
00444 void PlanningSceneDisplay::unsetGroupColor(rviz::Robot* robot, const std::string& group_name)
00445 {
00446   if (getRobotModel())
00447   {
00448     const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name);
00449     if (jmg)
00450     {
00451       const std::vector<std::string>& links = jmg->getLinkModelNamesWithCollisionGeometry();
00452       for (std::size_t i = 0; i < links.size(); ++i)
00453         unsetLinkColor(robot, links[i]);
00454     }
00455   }
00456 }
00457 
00458 void PlanningSceneDisplay::setLinkColor(const std::string& link_name, const QColor& color)
00459 {
00460   if (planning_scene_robot_)
00461     setLinkColor(&planning_scene_robot_->getRobot(), link_name, color);
00462 }
00463 
00464 void PlanningSceneDisplay::unsetLinkColor(const std::string& link_name)
00465 {
00466   if (planning_scene_robot_)
00467     unsetLinkColor(&planning_scene_robot_->getRobot(), link_name);
00468 }
00469 
00470 void PlanningSceneDisplay::setLinkColor(rviz::Robot* robot, const std::string& link_name, const QColor& color)
00471 {
00472   rviz::RobotLink* link = robot->getLink(link_name);
00473 
00474   // Check if link exists
00475   if (link)
00476     link->setColor(color.redF(), color.greenF(), color.blueF());
00477 }
00478 
00479 void PlanningSceneDisplay::unsetLinkColor(rviz::Robot* robot, const std::string& link_name)
00480 {
00481   rviz::RobotLink* link = robot->getLink(link_name);
00482 
00483   // Check if link exists
00484   if (link)
00485     link->unsetColor();
00486 }
00487 
00488 // ******************************************************************************************
00489 // Load
00490 // ******************************************************************************************
00491 planning_scene_monitor::PlanningSceneMonitorPtr PlanningSceneDisplay::createPlanningSceneMonitor()
00492 {
00493   return planning_scene_monitor::PlanningSceneMonitorPtr(new planning_scene_monitor::PlanningSceneMonitor(
00494       robot_description_property_->getStdString(), context_->getFrameManager()->getTFClientPtr(),
00495       getNameStd() + "_planning_scene_monitor"));
00496 }
00497 
00498 void PlanningSceneDisplay::clearRobotModel()
00499 {
00500   planning_scene_render_.reset();
00501   planning_scene_monitor_.reset();  // this so that the destructor of the PlanningSceneMonitor gets called before a new
00502                                     // instance of a scene monitor is constructed
00503 }
00504 
00505 void PlanningSceneDisplay::loadRobotModel()
00506 {
00507   // wait for other robot loadRobotModel() calls to complete;
00508   boost::mutex::scoped_lock _(robot_model_loading_lock_);
00509   model_is_loading_ = true;
00510 
00511   // we need to make sure the clearing of the robot model is in the main thread,
00512   // so that rendering operations do not have data removed from underneath,
00513   // so the next function is executed in the main loop
00514   addMainLoopJob(boost::bind(&PlanningSceneDisplay::clearRobotModel, this));
00515 
00516   waitForAllMainLoopJobs();
00517 
00518   planning_scene_monitor::PlanningSceneMonitorPtr psm = createPlanningSceneMonitor();
00519   if (psm->getPlanningScene())
00520   {
00521     planning_scene_monitor_.swap(psm);
00522     addMainLoopJob(boost::bind(&PlanningSceneDisplay::onRobotModelLoaded, this));
00523     setStatus(rviz::StatusProperty::Ok, "PlanningScene", "Planning Scene Loaded Successfully");
00524     waitForAllMainLoopJobs();
00525   }
00526   else
00527   {
00528     setStatus(rviz::StatusProperty::Error, "PlanningScene", "No Planning Scene Loaded");
00529   }
00530 
00531   if (planning_scene_monitor_)
00532     planning_scene_monitor_->addUpdateCallback(
00533         boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1));
00534 
00535   model_is_loading_ = false;
00536 }
00537 
00538 void PlanningSceneDisplay::onRobotModelLoaded()
00539 {
00540   changedPlanningSceneTopic();
00541   planning_scene_render_.reset(new PlanningSceneRender(planning_scene_node_, context_, planning_scene_robot_));
00542   planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
00543 
00544   const planning_scene_monitor::LockedPlanningSceneRO& ps = getPlanningSceneRO();
00545   if (planning_scene_robot_)
00546   {
00547     planning_scene_robot_->load(*getRobotModel()->getURDF());
00548     robot_state::RobotState* rs = new robot_state::RobotState(ps->getCurrentState());
00549     rs->update();
00550     planning_scene_robot_->update(robot_state::RobotStateConstPtr(rs));
00551   }
00552 
00553   bool oldState = scene_name_property_->blockSignals(true);
00554   scene_name_property_->setStdString(ps->getName());
00555   scene_name_property_->blockSignals(oldState);
00556 }
00557 
00558 void PlanningSceneDisplay::sceneMonitorReceivedUpdate(
00559     planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
00560 {
00561   onSceneMonitorReceivedUpdate(update_type);
00562 }
00563 
00564 void PlanningSceneDisplay::onSceneMonitorReceivedUpdate(
00565     planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
00566 {
00567   bool oldState = scene_name_property_->blockSignals(true);
00568   getPlanningSceneRW()->getCurrentStateNonConst().update();
00569   scene_name_property_->setStdString(getPlanningSceneRO()->getName());
00570   scene_name_property_->blockSignals(oldState);
00571 
00572   planning_scene_needs_render_ = true;
00573 }
00574 
00575 void PlanningSceneDisplay::onEnable()
00576 {
00577   Display::onEnable();
00578 
00579   addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
00580 
00581   if (planning_scene_robot_)
00582   {
00583     planning_scene_robot_->setVisible(true);
00584     planning_scene_robot_->setVisualVisible(scene_robot_visual_enabled_property_->getBool());
00585     planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());
00586   }
00587   if (planning_scene_render_)
00588     planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
00589 
00590   calculateOffsetPosition();
00591 }
00592 
00593 // ******************************************************************************************
00594 // Disable
00595 // ******************************************************************************************
00596 void PlanningSceneDisplay::onDisable()
00597 {
00598   if (planning_scene_monitor_)
00599   {
00600     planning_scene_monitor_->stopSceneMonitor();
00601     if (planning_scene_render_)
00602       planning_scene_render_->getGeometryNode()->setVisible(false);
00603   }
00604   if (planning_scene_robot_)
00605     planning_scene_robot_->setVisible(false);
00606   Display::onDisable();
00607 }
00608 
00609 void PlanningSceneDisplay::queueRenderSceneGeometry()
00610 {
00611   planning_scene_needs_render_ = true;
00612 }
00613 
00614 void PlanningSceneDisplay::update(float wall_dt, float ros_dt)
00615 {
00616   Display::update(wall_dt, ros_dt);
00617 
00618   executeMainLoopJobs();
00619 
00620   if (planning_scene_monitor_)
00621     updateInternal(wall_dt, ros_dt);
00622 }
00623 
00624 void PlanningSceneDisplay::updateInternal(float wall_dt, float ros_dt)
00625 {
00626   current_scene_time_ += wall_dt;
00627   if (current_scene_time_ > scene_display_time_property_->getFloat())
00628   {
00629     renderPlanningScene();
00630     calculateOffsetPosition();
00631     current_scene_time_ = 0.0f;
00632   }
00633 }
00634 
00635 void PlanningSceneDisplay::load(const rviz::Config& config)
00636 {
00637   Display::load(config);
00638 }
00639 
00640 void PlanningSceneDisplay::save(rviz::Config config) const
00641 {
00642   Display::save(config);
00643 }
00644 
00645 // ******************************************************************************************
00646 // Calculate Offset Position
00647 // ******************************************************************************************
00648 void PlanningSceneDisplay::calculateOffsetPosition()
00649 {
00650   if (!getRobotModel())
00651     return;
00652 
00653   Ogre::Vector3 position;
00654   Ogre::Quaternion orientation;
00655 
00656   context_->getFrameManager()->getTransform(getRobotModel()->getModelFrame(), ros::Time(0), position, orientation);
00657 
00658   planning_scene_node_->setPosition(position);
00659   planning_scene_node_->setOrientation(orientation);
00660 }
00661 
00662 void PlanningSceneDisplay::fixedFrameChanged()
00663 {
00664   Display::fixedFrameChanged();
00665   calculateOffsetPosition();
00666 }
00667 
00668 }  // namespace moveit_rviz_plugin


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