planning_scene_display.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * Copyright (c) 2013, Ioan A. Sucan
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan */
37 
41 
43 #include <rviz/robot/robot.h>
44 #include <rviz/robot/robot_link.h>
45 
53 #include <rviz/display_context.h>
54 #include <rviz/frame_manager.h>
55 #include <tf/transform_listener.h>
56 
57 #include <OgreSceneManager.h>
58 #include <OgreSceneNode.h>
59 
60 namespace moveit_rviz_plugin
61 {
62 // ******************************************************************************************
63 // Base class contructor
64 // ******************************************************************************************
65 PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool show_scene_robot)
66  : Display(), model_is_loading_(false), planning_scene_needs_render_(true), current_scene_time_(0.0f)
67 {
68  move_group_ns_property_ = new rviz::StringProperty("Move Group Namespace", "", "The name of the ROS namespace in "
69  "which the move_group node is running",
70  this, SLOT(changedMoveGroupNS()), this);
72  "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
73  this, SLOT(changedRobotDescription()), this);
74 
75  if (listen_to_planning_scene)
77  new rviz::RosTopicProperty("Planning Scene Topic", "move_group/monitored_planning_scene",
78  ros::message_traits::datatype<moveit_msgs::PlanningScene>(),
79  "The topic on which the moveit_msgs::PlanningScene messages are received", this,
80  SLOT(changedPlanningSceneTopic()), this);
81  else
83 
84  // Planning scene category -------------------------------------------------------------------------------------------
85  scene_category_ = new rviz::Property("Scene Geometry", QVariant(), "", this);
86 
87  scene_name_property_ = new rviz::StringProperty("Scene Name", "(noname)", "Shows the name of the planning scene",
88  scene_category_, SLOT(changedSceneName()), this);
91  new rviz::BoolProperty("Show Scene Geometry", true, "Indicates whether planning scenes should be displayed",
92  scene_category_, SLOT(changedSceneEnabled()), this);
93 
94  scene_alpha_property_ = new rviz::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry",
95  scene_category_, SLOT(changedSceneAlpha()), this);
98 
100  "Scene Color", QColor(50, 230, 50), "The color for the planning scene obstacles (if a color is not defined)",
101  scene_category_, SLOT(changedSceneColor()), this);
102 
103  octree_render_property_ = new rviz::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.",
105 
109 
110  octree_coloring_property_ = new rviz::EnumProperty("Voxel Coloring", "Z-Axis", "Select voxel coloring mode",
111  scene_category_, SLOT(changedOctreeColorMode()), this);
112 
115 
117  new rviz::FloatProperty("Scene Display Time", 0.2f, "The amount of wall-time to wait in between rendering "
118  "updates to the planning scene (if any)",
121 
122  if (show_scene_robot)
123  {
124  robot_category_ = new rviz::Property("Scene Robot", QVariant(), "", this);
125 
127  "Show Robot Visual", true, "Indicates whether the robot state specified by the planning scene should be "
128  "displayed as defined for visualisation purposes.",
130 
132  "Show Robot Collision", false, "Indicates whether the robot state specified by the planning scene should be "
133  "displayed as defined for collision detection purposes.",
135 
136  robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 1.0f, "Specifies the alpha for the robot links",
137  robot_category_, SLOT(changedRobotSceneAlpha()), this);
140 
142  new rviz::ColorProperty("Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies",
144  }
145  else
146  {
152  }
153 }
154 
155 // ******************************************************************************************
156 // Deconstructor
157 // ******************************************************************************************
159 {
160  clearJobs();
161 
162  planning_scene_render_.reset();
164  context_->getSceneManager()->destroySceneNode(planning_scene_node_->getName());
166  planning_scene_robot_.reset();
167  planning_scene_monitor_.reset();
168 }
169 
171 {
173  {
174  boost::unique_lock<boost::mutex> ulock(main_loop_jobs_lock_);
175  main_loop_jobs_.clear();
176  }
177 }
178 
180 {
181  Display::onInitialize();
182 
183  // the scene node that contains everything
184  planning_scene_node_ = scene_node_->createChildSceneNode();
185 
186  if (robot_category_)
187  {
188  planning_scene_robot_.reset(
190  planning_scene_robot_->setVisible(true);
195  }
196 }
197 
199 {
200  planning_scene_render_.reset();
202  planning_scene_robot_->clear();
203 
204  addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
205  Display::reset();
206 
208  {
209  planning_scene_robot_->setVisible(true);
212  }
213 }
214 
215 void PlanningSceneDisplay::addBackgroundJob(const boost::function<void()>& job, const std::string& name)
216 {
217  background_process_.addJob(job, name);
218 }
219 
220 void PlanningSceneDisplay::spawnBackgroundJob(const boost::function<void()>& job)
221 {
222  boost::thread t(job);
223 }
224 
225 void PlanningSceneDisplay::addMainLoopJob(const boost::function<void()>& job)
226 {
227  boost::unique_lock<boost::mutex> ulock(main_loop_jobs_lock_);
228  main_loop_jobs_.push_back(job);
229 }
230 
232 {
233  boost::unique_lock<boost::mutex> ulock(main_loop_jobs_lock_);
234  while (!main_loop_jobs_.empty())
236 }
237 
239 {
240  main_loop_jobs_lock_.lock();
241  while (!main_loop_jobs_.empty())
242  {
243  boost::function<void()> fn = main_loop_jobs_.front();
244  main_loop_jobs_.pop_front();
245  main_loop_jobs_lock_.unlock();
246  try
247  {
248  fn();
249  }
250  catch (std::exception& ex)
251  {
252  ROS_ERROR("Exception caught executing main loop job: %s", ex.what());
253  }
254  main_loop_jobs_lock_.lock();
255  }
256  main_loop_jobs_empty_condition_.notify_all();
257  main_loop_jobs_lock_.unlock();
258 }
259 
260 const planning_scene_monitor::PlanningSceneMonitorPtr& PlanningSceneDisplay::getPlanningSceneMonitor()
261 {
263 }
264 
265 const std::string PlanningSceneDisplay::getMoveGroupNS() const
266 {
268 }
269 
270 const robot_model::RobotModelConstPtr& PlanningSceneDisplay::getRobotModel() const
271 {
273  return planning_scene_monitor_->getRobotModel();
274  else
275  {
276  static robot_model::RobotModelConstPtr empty;
277  return empty;
278  }
279 }
280 
282 {
284  return planning_scene_monitor_->waitForCurrentRobotState(t);
285  return false;
286 }
287 
289 {
291 }
292 
294 {
296 }
297 
299 {
301 }
302 
304 {
306 }
307 
309 {
310  if (isEnabled())
311  reset();
312 }
313 
315 {
316  if (isEnabled())
317  reset();
318 }
319 
321 {
323  if (ps)
324  ps->setName(scene_name_property_->getStdString());
325 }
326 
328 {
329  QColor color = scene_color_property_->getColor();
330  rviz::Color env_color(color.redF(), color.greenF(), color.blueF());
333  rviz::Color attached_color(color.redF(), color.greenF(), color.blueF());
334 
335  try
336  {
338  planning_scene_render_->renderPlanningScene(
339  ps, env_color, attached_color, static_cast<OctreeVoxelRenderMode>(octree_render_property_->getOptionInt()),
340  static_cast<OctreeVoxelColorMode>(octree_coloring_property_->getOptionInt()),
342  }
343  catch (std::exception& ex)
344  {
345  ROS_ERROR("Caught %s while rendering planning scene", ex.what());
346  }
347  planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
348 }
349 
351 {
353 }
354 
356 {
358  {
361  }
362 }
363 
365 {
367  {
370  if (!getMoveGroupNS().empty())
371  service_name = ros::names::append(getMoveGroupNS(), service_name);
372  planning_scene_monitor_->requestPlanningSceneState(service_name);
373  }
374 }
375 
377 {
378 }
379 
381 {
382 }
383 
385 {
386 }
387 
389 {
391  {
392  planning_scene_robot_->setVisible(true);
395  }
396 }
397 
399 {
401  {
402  planning_scene_robot_->setVisible(true);
405  }
406 }
407 
409 {
411  planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
412 }
413 
414 void PlanningSceneDisplay::setGroupColor(rviz::Robot* robot, const std::string& group_name, const QColor& color)
415 {
416  if (getRobotModel())
417  {
418  const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name);
419  if (jmg)
420  {
421  const std::vector<std::string>& links = jmg->getLinkModelNamesWithCollisionGeometry();
422  for (std::size_t i = 0; i < links.size(); ++i)
423  setLinkColor(robot, links[i], color);
424  }
425  }
426 }
427 
429 {
430  if (getRobotModel())
431  {
432  const std::vector<std::string>& links = getRobotModel()->getLinkModelNamesWithCollisionGeometry();
433  for (std::size_t i = 0; i < links.size(); ++i)
434  unsetLinkColor(robot, links[i]);
435  }
436 }
437 
438 void PlanningSceneDisplay::unsetGroupColor(rviz::Robot* robot, const std::string& group_name)
439 {
440  if (getRobotModel())
441  {
442  const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(group_name);
443  if (jmg)
444  {
445  const std::vector<std::string>& links = jmg->getLinkModelNamesWithCollisionGeometry();
446  for (std::size_t i = 0; i < links.size(); ++i)
447  unsetLinkColor(robot, links[i]);
448  }
449  }
450 }
451 
452 void PlanningSceneDisplay::setLinkColor(const std::string& link_name, const QColor& color)
453 {
455  setLinkColor(&planning_scene_robot_->getRobot(), link_name, color);
456 }
457 
458 void PlanningSceneDisplay::unsetLinkColor(const std::string& link_name)
459 {
461  unsetLinkColor(&planning_scene_robot_->getRobot(), link_name);
462 }
463 
464 void PlanningSceneDisplay::setLinkColor(rviz::Robot* robot, const std::string& link_name, const QColor& color)
465 {
466  rviz::RobotLink* link = robot->getLink(link_name);
467 
468  // Check if link exists
469  if (link)
470  link->setColor(color.redF(), color.greenF(), color.blueF());
471 }
472 
473 void PlanningSceneDisplay::unsetLinkColor(rviz::Robot* robot, const std::string& link_name)
474 {
475  rviz::RobotLink* link = robot->getLink(link_name);
476 
477  // Check if link exists
478  if (link)
479  link->unsetColor();
480 }
481 
482 // ******************************************************************************************
483 // Load
484 // ******************************************************************************************
485 planning_scene_monitor::PlanningSceneMonitorPtr PlanningSceneDisplay::createPlanningSceneMonitor()
486 {
487  return planning_scene_monitor::PlanningSceneMonitorPtr(new planning_scene_monitor::PlanningSceneMonitor(
489  getNameStd() + "_planning_scene_monitor"));
490 }
491 
493 {
494  planning_scene_render_.reset();
495  planning_scene_monitor_.reset(); // this so that the destructor of the PlanningSceneMonitor gets called before a new
496  // instance of a scene monitor is constructed
497 }
498 
500 {
501  // wait for other robot loadRobotModel() calls to complete;
502  boost::mutex::scoped_lock _(robot_model_loading_lock_);
503  model_is_loading_ = true;
504 
505  // we need to make sure the clearing of the robot model is in the main thread,
506  // so that rendering operations do not have data removed from underneath,
507  // so the next function is executed in the main loop
509 
511 
512  planning_scene_monitor::PlanningSceneMonitorPtr psm = createPlanningSceneMonitor();
513  if (psm->getPlanningScene())
514  {
515  planning_scene_monitor_.swap(psm);
517  setStatus(rviz::StatusProperty::Ok, "PlanningScene", "Planning Scene Loaded Successfully");
519  }
520  else
521  {
522  setStatus(rviz::StatusProperty::Error, "PlanningScene", "No Planning Scene Loaded");
523  }
524 
526  planning_scene_monitor_->addUpdateCallback(
527  boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1));
528 
529  model_is_loading_ = false;
530 }
531 
533 {
536  planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
537 
540  {
541  planning_scene_robot_->load(*getRobotModel()->getURDF());
542  robot_state::RobotState* rs = new robot_state::RobotState(ps->getCurrentState());
543  rs->update();
544  planning_scene_robot_->update(robot_state::RobotStateConstPtr(rs));
545  }
546 
547  bool oldState = scene_name_property_->blockSignals(true);
548  scene_name_property_->setStdString(ps->getName());
549  scene_name_property_->blockSignals(oldState);
550 }
551 
554 {
555  onSceneMonitorReceivedUpdate(update_type);
556 }
557 
560 {
561  bool oldState = scene_name_property_->blockSignals(true);
562  getPlanningSceneRW()->getCurrentStateNonConst().update();
564  scene_name_property_->blockSignals(oldState);
565 
567 }
568 
570 {
571  Display::onEnable();
572 
573  addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
574 
576  {
577  planning_scene_robot_->setVisible(true);
580  }
582  planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
583 
586 }
587 
588 // ******************************************************************************************
589 // Disable
590 // ******************************************************************************************
592 {
594  {
595  planning_scene_monitor_->stopSceneMonitor();
597  planning_scene_render_->getGeometryNode()->setVisible(false);
598  }
600  planning_scene_robot_->setVisible(false);
601  Display::onDisable();
602 }
603 
605 {
607 }
608 
609 void PlanningSceneDisplay::update(float wall_dt, float ros_dt)
610 {
611  Display::update(wall_dt, ros_dt);
612 
614 
616  updateInternal(wall_dt, ros_dt);
617 }
618 
619 void PlanningSceneDisplay::updateInternal(float wall_dt, float ros_dt)
620 {
621  current_scene_time_ += wall_dt;
624  {
627  current_scene_time_ = 0.0f;
629  }
630 }
631 
633 {
634  Display::load(config);
635 }
636 
638 {
639  Display::save(config);
640 }
641 
642 // ******************************************************************************************
643 // Calculate Offset Position
644 // ******************************************************************************************
646 {
647  if (!getRobotModel())
648  return;
649 
650  Ogre::Vector3 position;
651  Ogre::Quaternion orientation;
652 
653  context_->getFrameManager()->getTransform(getRobotModel()->getModelFrame(), ros::Time(0), position, orientation);
654 
655  planning_scene_node_->setPosition(position);
656  planning_scene_node_->setOrientation(orientation);
657 }
658 
660 {
661  Display::fixedFrameChanged();
663 }
664 
665 } // namespace moveit_rviz_plugin
virtual QColor getColor() const
#define NULL
void setMin(float min)
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
std::string getNameStd() const
void setMax(float max)
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
void addJob(const JobCallback &job, const std::string &name)
void setGroupColor(rviz::Robot *robot, const std::string &group_name, const QColor &color)
PlanningSceneDisplay(bool listen_to_planning_scene=true, bool show_scene_robot=true)
Update the links of an rviz::Robot using a robot_state::RobotState.
rviz::RosTopicProperty * planning_scene_topic_property_
virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
DisplayContext * context_
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
f
void unsetLinkColor(const std::string &link_name)
virtual float getFloat() const
geometry_msgs::TransformStamped t
RobotLink * getLink(const std::string &name)
virtual void onRobotModelLoaded()
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
virtual void updateInternal(float wall_dt, float ros_dt)
Ogre::SceneNode * scene_node_
std::string getStdString()
bool isEnabled() const
virtual bool getBool() const
virtual void update(float wall_dt, float ros_dt)
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
virtual void addOption(const QString &option, int value=0)
const boost::shared_ptr< tf::TransformListener > & getTFClientPtr()
virtual void save(rviz::Config config) const
virtual FrameManager * getFrameManager() const =0
std::deque< boost::function< void()> > main_loop_jobs_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
Ogre::SceneNode * planning_scene_node_
displays planning scene with everything in it
void setShouldBeSaved(bool save)
bool waitForCurrentRobotState(const ros::Time &t=ros::Time::now())
wait for robot state more recent than t
virtual void load(const rviz::Config &config)
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called ...
virtual Ogre::SceneManager * getSceneManager() const =0
const robot_model::RobotModelConstPtr & getRobotModel() const
virtual planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor()
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void sceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
moveit::tools::BackgroundProcessing background_process_
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
void calculateOffsetPosition()
Set the scene node&#39;s position, given the target frame and the planning frame.
void unsetGroupColor(rviz::Robot *robot, const std::string &group_name)
boost::condition_variable main_loop_jobs_empty_condition_
void setLinkColor(const std::string &link_name, const QColor &color)
virtual int getOptionInt()
bool setStdString(const std::string &std_str)
#define ROS_ERROR(...)
void spawnBackgroundJob(const boost::function< void()> &job)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
virtual QString getName() const
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:19:09