motion_planning_display.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */
36 
42 
44 #include <rviz/robot/robot.h>
45 #include <rviz/robot/robot_link.h>
46 
54 #include <rviz/display_context.h>
55 #include <rviz/frame_manager.h>
56 #include <rviz/panel_dock_widget.h>
58 #include <rviz/display_factory.h>
60 
61 #include <OgreSceneManager.h>
62 #include <OgreSceneNode.h>
64 
67 
68 #include <boost/format.hpp>
69 #include <boost/algorithm/string/replace.hpp>
70 #include <boost/algorithm/string/trim.hpp>
71 #include <boost/lexical_cast.hpp>
72 
73 #include <QShortcut>
74 
75 #include "ui_motion_planning_rviz_plugin_frame.h"
76 
77 namespace moveit_rviz_plugin
78 {
79 // ******************************************************************************************
80 // Base class contructor
81 // ******************************************************************************************
83  : PlanningSceneDisplay()
84  , text_to_display_(nullptr)
85  , private_handle_("~")
86  , frame_(nullptr)
87  , frame_dock_(nullptr)
88  , menu_handler_start_(new interactive_markers::MenuHandler)
89  , menu_handler_goal_(new interactive_markers::MenuHandler)
90  , int_marker_display_(nullptr)
91 {
92  // Category Groups
93  plan_category_ = new rviz::Property("Planning Request", QVariant(), "", this);
94  metrics_category_ = new rviz::Property("Planning Metrics", QVariant(), "", this);
95  path_category_ = new rviz::Property("Planned Path", QVariant(), "", this);
96 
97  // Metrics category -----------------------------------------------------------------------------------------
98  compute_weight_limit_property_ =
99  new rviz::BoolProperty("Show Weight Limit", false,
100  "Shows the weight limit at a particular pose for an end-effector", metrics_category_,
101  SLOT(changedShowWeightLimit()), this);
102 
103  show_manipulability_index_property_ =
104  new rviz::BoolProperty("Show Manipulability Index", false, "Shows the manipulability index for an end-effector",
105  metrics_category_, SLOT(changedShowManipulabilityIndex()), this);
106 
107  show_manipulability_property_ =
108  new rviz::BoolProperty("Show Manipulability", false, "Shows the manipulability for an end-effector",
109  metrics_category_, SLOT(changedShowManipulability()), this);
110 
111  show_joint_torques_property_ = new rviz::BoolProperty("Show Joint Torques", false,
112  "Shows the joint torques for a given configuration and payload",
113  metrics_category_, SLOT(changedShowJointTorques()), this);
114 
115  metrics_set_payload_property_ =
116  new rviz::FloatProperty("Payload", 1.0f, "Specify the payload at the end effector (kg)", metrics_category_,
117  SLOT(changedMetricsSetPayload()), this);
118  metrics_set_payload_property_->setMin(0.0);
119 
120  metrics_text_height_property_ = new rviz::FloatProperty("TextHeight", 0.08f, "Text height", metrics_category_,
121  SLOT(changedMetricsTextHeight()), this);
122  metrics_text_height_property_->setMin(0.001);
123 
124  // Planning request category -----------------------------------------------------------------------------------------
125 
126  planning_group_property_ =
127  new rviz::EnumProperty("Planning Group", "",
128  "The name of the group of links to plan for (from the ones defined in the SRDF)",
129  plan_category_, SLOT(changedPlanningGroup()), this);
130  show_workspace_property_ = new rviz::BoolProperty("Show Workspace", false,
131  "Shows the axis-aligned bounding box for "
132  "the workspace allowed for planning",
133  plan_category_, SLOT(changedWorkspace()), this);
134  query_start_state_property_ =
135  new rviz::BoolProperty("Query Start State", false, "Set a custom start state for the motion planning query",
136  plan_category_, SLOT(changedQueryStartState()), this);
137  query_goal_state_property_ =
138  new rviz::BoolProperty("Query Goal State", true, "Shows the goal state for the motion planning query",
139  plan_category_, SLOT(changedQueryGoalState()), this);
140  query_marker_scale_property_ =
141  new rviz::FloatProperty("Interactive Marker Size", 0.0f,
142  "Specifies scale of the interactive marker overlayed on the robot. 0 is auto scale.",
143  plan_category_, SLOT(changedQueryMarkerScale()), this);
144  query_marker_scale_property_->setMin(0.0f);
145 
146  query_start_color_property_ =
147  new rviz::ColorProperty("Start State Color", QColor(0, 255, 0), "The highlight color for the start state",
148  plan_category_, SLOT(changedQueryStartColor()), this);
149  query_start_alpha_property_ =
150  new rviz::FloatProperty("Start State Alpha", 1.0f, "Specifies the alpha for the robot links", plan_category_,
151  SLOT(changedQueryStartAlpha()), this);
152  query_start_alpha_property_->setMin(0.0);
153  query_start_alpha_property_->setMax(1.0);
154 
155  query_goal_color_property_ =
156  new rviz::ColorProperty("Goal State Color", QColor(250, 128, 0), "The highlight color for the goal state",
157  plan_category_, SLOT(changedQueryGoalColor()), this);
158 
159  query_goal_alpha_property_ =
160  new rviz::FloatProperty("Goal State Alpha", 1.0f, "Specifies the alpha for the robot links", plan_category_,
161  SLOT(changedQueryGoalAlpha()), this);
162  query_goal_alpha_property_->setMin(0.0);
163  query_goal_alpha_property_->setMax(1.0);
164 
165  query_colliding_link_color_property_ =
166  new rviz::ColorProperty("Colliding Link Color", QColor(255, 0, 0), "The highlight color for colliding links",
167  plan_category_, SLOT(changedQueryCollidingLinkColor()), this);
168 
169  query_outside_joint_limits_link_color_property_ =
170  new rviz::ColorProperty("Joint Violation Color", QColor(255, 0, 255),
171  "The highlight color for child links of joints that are outside bounds", plan_category_,
172  SLOT(changedQueryJointViolationColor()), this);
173 
174  // Trajectory playback / planned path category ---------------------------------------------
175  trajectory_visual_ = std::make_shared<TrajectoryVisualization>(path_category_, this);
176 
177  // Start background jobs
178  background_process_.setJobUpdateEvent([this](moveit::tools::BackgroundProcessing::JobEvent event,
179  const std::string& name) { backgroundJobUpdate(event, name); });
180 }
181 
182 // ******************************************************************************************
183 // Deconstructor
184 // ******************************************************************************************
185 MotionPlanningDisplay::~MotionPlanningDisplay()
186 {
187  background_process_.clearJobUpdateEvent();
188  clearJobs();
189 
190  query_robot_start_.reset();
191  query_robot_goal_.reset();
192 
193  delete text_to_display_;
194  delete int_marker_display_;
195  delete frame_dock_;
196 }
197 
198 void MotionPlanningDisplay::onInitialize()
199 {
200  PlanningSceneDisplay::onInitialize();
201 
202  // Planned Path Display
203  trajectory_visual_->onInitialize(planning_scene_node_, context_, update_nh_);
204  QColor qcolor = attached_body_color_property_->getColor();
205  trajectory_visual_->setDefaultAttachedObjectColor(qcolor);
206 
207  query_robot_start_ =
208  std::make_shared<RobotStateVisualization>(planning_scene_node_, context_, "Planning Request Start", nullptr);
209  query_robot_start_->setCollisionVisible(false);
210  query_robot_start_->setVisualVisible(true);
211  query_robot_start_->setVisible(query_start_state_property_->getBool());
212  std_msgs::ColorRGBA color;
213  qcolor = query_start_color_property_->getColor();
214  color.r = qcolor.redF();
215  color.g = qcolor.greenF();
216  color.b = qcolor.blueF();
217  color.a = 1.0f;
218  query_robot_start_->setDefaultAttachedObjectColor(color);
219 
220  query_robot_goal_ =
221  std::make_shared<RobotStateVisualization>(planning_scene_node_, context_, "Planning Request Goal", nullptr);
222  query_robot_goal_->setCollisionVisible(false);
223  query_robot_goal_->setVisualVisible(true);
224  query_robot_goal_->setVisible(query_goal_state_property_->getBool());
225  qcolor = query_goal_color_property_->getColor();
226  color.r = qcolor.redF();
227  color.g = qcolor.greenF();
228  color.b = qcolor.blueF();
229  query_robot_goal_->setDefaultAttachedObjectColor(color);
230 
231  rviz::WindowManagerInterface* window_context = context_->getWindowManager();
232  frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr);
233 
234  connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged()));
235  resetStatusTextColor();
236  addStatusText("Initialized.");
237 
238  // immediately switch to next trajectory display after planning
239  connect(frame_, SIGNAL(planningFinished()), trajectory_visual_.get(), SLOT(interruptCurrentDisplay()));
240 
241  if (window_context)
242  {
243  frame_dock_ = window_context->addPane(getName(), frame_);
244  connect(frame_dock_, SIGNAL(visibilityChanged(bool)), this, SLOT(motionPanelVisibilityChange(bool)));
245  frame_dock_->setIcon(getIcon());
246  }
247 
248  int_marker_display_ = context_->getDisplayFactory()->make("rviz/InteractiveMarkers");
249  int_marker_display_->initialize(context_);
250 
251  text_display_scene_node_ = planning_scene_node_->createChildSceneNode();
252  text_to_display_ = new rviz::MovableText("EMPTY");
253  text_to_display_->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_CENTER);
254  text_to_display_->setCharacterHeight(metrics_text_height_property_->getFloat());
255  text_to_display_->showOnTop();
256  text_to_display_->setVisible(false);
257  text_display_for_start_ = false;
258  text_display_scene_node_->attachObject(text_to_display_);
259 
260  if (context_ && context_->getWindowManager() && context_->getWindowManager()->getParentWindow())
261  {
262  QShortcut* im_reset_shortcut =
263  new QShortcut(QKeySequence("Ctrl+I"), context_->getWindowManager()->getParentWindow());
264  connect(im_reset_shortcut, SIGNAL(activated()), this, SLOT(resetInteractiveMarkers()));
265  }
266 }
267 
268 void MotionPlanningDisplay::motionPanelVisibilityChange(bool enable)
269 {
270  if (enable)
271  setEnabled(true);
272 }
273 
274 void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable)
275 {
276  if (enable)
277  {
278  planning_group_sub_ = node_handle_.subscribe("/rviz/moveit/select_planning_group", 1,
279  &MotionPlanningDisplay::selectPlanningGroupCallback, this);
280  }
281  else
282  {
283  planning_group_sub_.shutdown();
284  }
285 }
286 
287 void MotionPlanningDisplay::selectPlanningGroupCallback(const std_msgs::StringConstPtr& msg)
288 {
289  // synchronize ROS callback with main loop
290  addMainLoopJob([this, group = msg->data] { changePlanningGroup(group); });
291 }
292 
293 void MotionPlanningDisplay::reset()
294 {
295  text_to_display_->setVisible(false);
296 
297  query_robot_start_->clear();
298  query_robot_goal_->clear();
299 
300  PlanningSceneDisplay::reset();
301 
302  // Planned Path Display
303  trajectory_visual_->reset();
304 
305  bool enabled = this->isEnabled();
306  frame_->disable();
307  if (enabled)
308  {
309  frame_->enable();
310  query_robot_start_->setVisible(query_start_state_property_->getBool());
311  query_robot_goal_->setVisible(query_goal_state_property_->getBool());
312  }
313 }
314 
315 void MotionPlanningDisplay::setName(const QString& name)
316 {
317  BoolProperty::setName(name);
318  if (frame_dock_)
319  {
320  frame_dock_->setWindowTitle(name);
321  frame_dock_->setObjectName(name);
322  }
323  trajectory_visual_->setName(name);
324 }
325 
326 void MotionPlanningDisplay::backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent /*unused*/,
327  const std::string& /*unused*/)
328 {
329  addMainLoopJob([this] { updateBackgroundJobProgressBar(); });
330 }
331 
332 void MotionPlanningDisplay::updateBackgroundJobProgressBar()
333 {
334  if (!frame_)
335  return;
336  QProgressBar* p = frame_->ui_->background_job_progress;
337  int n = background_process_.getJobCount();
338 
339  if (n == 0)
340  {
341  p->hide();
342  p->setMaximum(0);
343  p->setValue(0);
344  }
345  else
346  {
347  if (p->maximum() < n) // increase max
348  {
349  p->setMaximum(n);
350  if (n > 1) // only show bar if there will be a progress to show
351  p->show();
352  }
353  else // progress
354  p->setValue(p->maximum() - n);
355  p->update();
356  }
357 }
358 
359 void MotionPlanningDisplay::changedShowWeightLimit()
360 {
361  if (text_display_for_start_)
362  {
363  if (query_start_state_property_->getBool())
364  displayMetrics(true);
365  }
366  else
367  {
368  if (query_goal_state_property_->getBool())
369  displayMetrics(false);
370  }
371 }
372 
373 void MotionPlanningDisplay::changedShowManipulabilityIndex()
374 {
375  if (text_display_for_start_)
376  {
377  if (query_start_state_property_->getBool())
378  displayMetrics(true);
379  }
380  else
381  {
382  if (query_goal_state_property_->getBool())
383  displayMetrics(false);
384  }
385 }
386 
387 void MotionPlanningDisplay::changedShowManipulability()
388 {
389  if (text_display_for_start_)
390  {
391  if (query_start_state_property_->getBool())
392  displayMetrics(true);
393  }
394  else
395  {
396  if (query_goal_state_property_->getBool())
397  displayMetrics(false);
398  }
399 }
400 
401 void MotionPlanningDisplay::changedShowJointTorques()
402 {
403  if (text_display_for_start_)
404  {
405  if (query_start_state_property_->getBool())
406  displayMetrics(true);
407  }
408  else
409  {
410  if (query_goal_state_property_->getBool())
411  displayMetrics(false);
412  }
413 }
414 
415 void MotionPlanningDisplay::changedMetricsSetPayload()
416 {
417  if (text_display_for_start_)
418  {
419  if (query_start_state_property_->getBool())
420  displayMetrics(true);
421  }
422  else
423  {
424  if (query_goal_state_property_->getBool())
425  displayMetrics(false);
426  }
427 }
428 
429 void MotionPlanningDisplay::changedMetricsTextHeight()
430 {
431  text_to_display_->setCharacterHeight(metrics_text_height_property_->getFloat());
432 }
433 
434 void MotionPlanningDisplay::displayTable(const std::map<std::string, double>& values, const Ogre::ColourValue& color,
435  const Ogre::Vector3& pos, const Ogre::Quaternion& orient)
436 {
437  // the line we want to render
438  std::stringstream ss;
439  for (const std::pair<const std::string, double>& value : values)
440  ss << boost::format("%-10s %-4.2f") % value.first % value.second << std::endl;
441 
442  if (ss.str().empty())
443  {
444  text_to_display_->setVisible(false);
445  return;
446  }
447 
448  text_to_display_->setCaption(ss.str());
449  text_to_display_->setColor(color);
450  text_display_scene_node_->setPosition(pos);
451  text_display_scene_node_->setOrientation(orient);
452 
453  // make sure the node is visible
454  text_to_display_->setVisible(true);
455 }
456 
457 void MotionPlanningDisplay::renderWorkspaceBox()
458 {
459  if (!frame_ || !show_workspace_property_->getBool())
460  {
461  if (workspace_box_)
462  workspace_box_.reset();
463  return;
464  }
465 
466  if (!workspace_box_)
467  {
468  workspace_box_ =
469  std::make_unique<rviz::Shape>(rviz::Shape::Cube, context_->getSceneManager(), planning_scene_node_);
470  workspace_box_->setColor(0.0f, 0.0f, 0.6f, 0.3f);
471  }
472 
473  Ogre::Vector3 center(frame_->ui_->wcenter_x->value(), frame_->ui_->wcenter_y->value(),
474  frame_->ui_->wcenter_z->value());
475  Ogre::Vector3 extents(frame_->ui_->wsize_x->value(), frame_->ui_->wsize_y->value(), frame_->ui_->wsize_z->value());
476  workspace_box_->setScale(extents);
477  workspace_box_->setPosition(center);
478 }
479 
480 void MotionPlanningDisplay::computeMetrics(bool start, const std::string& group, double payload)
481 {
482  if (!robot_interaction_)
483  return;
484  const std::vector<robot_interaction::EndEffectorInteraction>& eef = robot_interaction_->getActiveEndEffectors();
485  if (eef.empty())
486  return;
487  boost::mutex::scoped_lock slock(update_metrics_lock_);
488 
489  moveit::core::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState();
490  for (const robot_interaction::EndEffectorInteraction& ee : eef)
491  if (ee.parent_group == group)
492  computeMetricsInternal(computed_metrics_[std::make_pair(start, group)], ee, *state, payload);
493 }
494 
495 void MotionPlanningDisplay::computeMetricsInternal(std::map<std::string, double>& metrics,
497  const moveit::core::RobotState& state, double payload)
498 {
499  metrics.clear();
500  dynamics_solver::DynamicsSolverPtr ds;
501  std::map<std::string, dynamics_solver::DynamicsSolverPtr>::const_iterator it = dynamics_solver_.find(ee.parent_group);
502  if (it != dynamics_solver_.end())
503  ds = it->second;
504 
505  // Max payload
506  if (ds)
507  {
508  double max_payload;
509  unsigned int saturated_joint;
510  std::vector<double> joint_values;
511  state.copyJointGroupPositions(ee.parent_group, joint_values);
512  if (ds->getMaxPayload(joint_values, max_payload, saturated_joint))
513  {
514  metrics["max_payload"] = max_payload;
515  metrics["saturated_joint"] = saturated_joint;
516  }
517  std::vector<double> joint_torques;
518  joint_torques.resize(joint_values.size());
519  if (ds->getPayloadTorques(joint_values, payload, joint_torques))
520  {
521  for (std::size_t i = 0; i < joint_torques.size(); ++i)
522  {
523  std::stringstream stream;
524  stream << "torque[" << i << "]";
525  metrics[stream.str()] = joint_torques[i];
526  }
527  }
528  }
529 
530  if (kinematics_metrics_)
531  {
532  if (position_only_ik_.find(ee.parent_group) == position_only_ik_.end())
533  private_handle_.param(ee.parent_group + "/position_only_ik", position_only_ik_[ee.parent_group], false);
534 
535  double manipulability_index, manipulability;
536  bool position_ik = position_only_ik_[ee.parent_group];
537  if (kinematics_metrics_->getManipulabilityIndex(state, ee.parent_group, manipulability_index, position_ik))
538  metrics["manipulability_index"] = manipulability_index;
539  if (kinematics_metrics_->getManipulability(state, ee.parent_group, manipulability))
540  metrics["manipulability"] = manipulability;
541  }
542 }
543 
544 namespace
545 {
546 inline void copyItemIfExists(const std::map<std::string, double>& source, std::map<std::string, double>& dest,
547  const std::string& key)
548 {
549  std::map<std::string, double>::const_iterator it = source.find(key);
550  if (it != source.end())
551  dest[key] = it->second;
552 }
553 } // namespace
554 
555 void MotionPlanningDisplay::displayMetrics(bool start)
556 {
557  if (!robot_interaction_ || !planning_scene_monitor_)
558  return;
559 
560  static const Ogre::Quaternion ORIENTATION(1.0, 0.0, 0.0, 0.0);
561  const std::vector<robot_interaction::EndEffectorInteraction>& eef = robot_interaction_->getActiveEndEffectors();
562  if (eef.empty())
563  return;
564 
565  moveit::core::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState();
566 
567  for (const robot_interaction::EndEffectorInteraction& ee : eef)
568  {
569  Ogre::Vector3 position(0.0, 0.0, 0.0);
570  std::map<std::string, double> text_table;
571  const std::map<std::string, double>& metrics_table = computed_metrics_[std::make_pair(start, ee.parent_group)];
572  if (compute_weight_limit_property_->getBool())
573  {
574  copyItemIfExists(metrics_table, text_table, "max_payload");
575  copyItemIfExists(metrics_table, text_table, "saturated_joint");
576  }
577  if (show_manipulability_index_property_->getBool())
578  copyItemIfExists(metrics_table, text_table, "manipulability_index");
579  if (show_manipulability_property_->getBool())
580  copyItemIfExists(metrics_table, text_table, "manipulability");
581  if (show_joint_torques_property_->getBool())
582  {
583  std::size_t nj = getRobotModel()->getJointModelGroup(ee.parent_group)->getJointModelNames().size();
584  for (size_t j = 0; j < nj; ++j)
585  {
586  std::stringstream stream;
587  stream << "torque[" << j << "]";
588  copyItemIfExists(metrics_table, text_table, stream.str());
589  }
590  }
591 
592  const moveit::core::LinkModel* lm = nullptr;
593  const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(ee.parent_group);
594  if (jmg)
595  if (!jmg->getLinkModelNames().empty())
596  lm = state->getLinkModel(jmg->getLinkModelNames().back());
597  if (lm)
598  {
599  const Eigen::Vector3d& t = state->getGlobalLinkTransform(lm).translation();
600  position[0] = t.x();
601  position[1] = t.y();
602  position[2] = t.z() + 0.2; // \todo this should be a param
603  }
604  if (start)
605  displayTable(text_table, query_start_color_property_->getOgreColor(), position, ORIENTATION);
606  else
607  displayTable(text_table, query_goal_color_property_->getOgreColor(), position, ORIENTATION);
608  text_display_for_start_ = start;
609  }
610 }
611 
612 void MotionPlanningDisplay::drawQueryStartState()
613 {
614  if (!planning_scene_monitor_)
615  return;
616 
617  if (query_start_state_property_->getBool())
618  {
619  if (isEnabled())
620  {
621  moveit::core::RobotStateConstPtr state = getQueryStartState();
622 
623  // update link poses
624  query_robot_start_->update(state);
625  query_robot_start_->setVisible(true);
626 
627  // update link colors
628  std::vector<std::string> collision_links;
629  getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
630  status_links_start_.clear();
631  for (const std::string& collision_link : collision_links)
632  status_links_start_[collision_link] = COLLISION_LINK;
633  if (!collision_links.empty())
634  {
636  getPlanningSceneRO()->getCollidingPairs(pairs, *state);
637  setStatusTextColor(query_start_color_property_->getColor());
638  addStatusText("Start state colliding links:");
639  for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin(); it != pairs.end();
640  ++it)
641  addStatusText(it->first.first + " - " + it->first.second);
642  addStatusText(".");
643  }
644  if (!getCurrentPlanningGroup().empty())
645  {
646  const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup());
647  if (jmg)
648  {
649  std::vector<std::string> outside_bounds;
650  const std::vector<const moveit::core::JointModel*>& jmodels = jmg->getActiveJointModels();
651  for (const moveit::core::JointModel* jmodel : jmodels)
652  if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2))
653  {
654  outside_bounds.push_back(jmodel->getChildLinkModel()->getName());
655  status_links_start_[outside_bounds.back()] = OUTSIDE_BOUNDS_LINK;
656  }
657  if (!outside_bounds.empty())
658  {
659  setStatusTextColor(query_start_color_property_->getColor());
660  addStatusText("Links descending from joints that are outside bounds in start state:");
661  addStatusText(outside_bounds);
662  }
663  }
664  }
665  updateLinkColors();
666  // update metrics text
667  displayMetrics(true);
668  }
669  }
670  else
671  query_robot_start_->setVisible(false);
672  context_->queueRender();
673 }
674 
675 void MotionPlanningDisplay::resetStatusTextColor()
676 {
677  setStatusTextColor(Qt::darkGray);
678 }
679 
680 void MotionPlanningDisplay::setStatusTextColor(const QColor& color)
681 {
682  if (frame_)
683  frame_->ui_->status_text->setTextColor(color);
684 }
685 
686 void MotionPlanningDisplay::addStatusText(const std::string& text)
687 {
688  if (frame_)
689  frame_->ui_->status_text->append(QString::fromStdString(text));
690 }
691 
692 void MotionPlanningDisplay::addStatusText(const std::vector<std::string>& text)
693 {
694  for (const std::string& it : text)
695  addStatusText(it);
696 }
697 
698 void MotionPlanningDisplay::recomputeQueryStartStateMetrics()
699 {
700  std::string group = planning_group_property_->getStdString();
701  if (!group.empty())
702  computeMetrics(true, group, metrics_set_payload_property_->getFloat());
703 }
704 
705 void MotionPlanningDisplay::recomputeQueryGoalStateMetrics()
706 {
707  std::string group = planning_group_property_->getStdString();
708  if (!group.empty())
709  computeMetrics(false, group, metrics_set_payload_property_->getFloat());
710 }
711 
712 void MotionPlanningDisplay::changedQueryStartState()
713 {
714  if (!planning_scene_monitor_)
715  return;
716  setStatusTextColor(query_start_color_property_->getColor());
717  addStatusText("Changed start state");
718  drawQueryStartState();
719  addBackgroundJob([this] { publishInteractiveMarkers(true); }, "publishInteractiveMarkers");
720 }
721 
722 void MotionPlanningDisplay::changedQueryGoalState()
723 {
724  if (!planning_scene_monitor_)
725  return;
726  setStatusTextColor(query_goal_color_property_->getColor());
727  addStatusText("Changed goal state");
728  drawQueryGoalState();
729  addBackgroundJob([this] { publishInteractiveMarkers(true); }, "publishInteractiveMarkers");
730 }
731 
732 void MotionPlanningDisplay::drawQueryGoalState()
733 {
734  if (!planning_scene_monitor_)
735  return;
736  if (query_goal_state_property_->getBool())
737  {
738  if (isEnabled())
739  {
740  moveit::core::RobotStateConstPtr state = getQueryGoalState();
741 
742  // update link poses
743  query_robot_goal_->update(state);
744  query_robot_goal_->setVisible(true);
745 
746  // update link colors
747  std::vector<std::string> collision_links;
748  getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
749  status_links_goal_.clear();
750  for (const std::string& collision_link : collision_links)
751  status_links_goal_[collision_link] = COLLISION_LINK;
752  if (!collision_links.empty())
753  {
755  getPlanningSceneRO()->getCollidingPairs(pairs, *state);
756  setStatusTextColor(query_goal_color_property_->getColor());
757  addStatusText("Goal state colliding links:");
758  for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin(); it != pairs.end();
759  ++it)
760  addStatusText(it->first.first + " - " + it->first.second);
761  addStatusText(".");
762  }
763 
764  if (!getCurrentPlanningGroup().empty())
765  {
766  const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup());
767  if (jmg)
768  {
769  const std::vector<const moveit::core::JointModel*>& jmodels = jmg->getActiveJointModels();
770  std::vector<std::string> outside_bounds;
771  for (const moveit::core::JointModel* jmodel : jmodels)
772  if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2))
773  {
774  outside_bounds.push_back(jmodel->getChildLinkModel()->getName());
775  status_links_goal_[outside_bounds.back()] = OUTSIDE_BOUNDS_LINK;
776  }
777 
778  if (!outside_bounds.empty())
779  {
780  setStatusTextColor(query_goal_color_property_->getColor());
781  addStatusText("Links descending from joints that are outside bounds in goal state:");
782  addStatusText(outside_bounds);
783  }
784  }
785  }
786  updateLinkColors();
787 
788  // update metrics text
789  displayMetrics(false);
790  }
791  }
792  else
793  query_robot_goal_->setVisible(false);
794  context_->queueRender();
795 }
796 
797 void MotionPlanningDisplay::resetInteractiveMarkers()
798 {
799  query_start_state_->clearError();
800  query_goal_state_->clearError();
801  addBackgroundJob([this] { publishInteractiveMarkers(false); }, "publishInteractiveMarkers");
802 }
803 
804 void MotionPlanningDisplay::publishInteractiveMarkers(bool pose_update)
805 {
806  if (robot_interaction_)
807  {
808  if (pose_update &&
809  robot_interaction_->showingMarkers(query_start_state_) == query_start_state_property_->getBool() &&
810  robot_interaction_->showingMarkers(query_goal_state_) == query_goal_state_property_->getBool())
811  {
812  if (query_start_state_property_->getBool())
813  robot_interaction_->updateInteractiveMarkers(query_start_state_);
814  if (query_goal_state_property_->getBool())
815  robot_interaction_->updateInteractiveMarkers(query_goal_state_);
816  }
817  else
818  {
819  robot_interaction_->clearInteractiveMarkers();
820  if (query_start_state_property_->getBool())
821  robot_interaction_->addInteractiveMarkers(query_start_state_, query_marker_scale_property_->getFloat());
822  if (query_goal_state_property_->getBool())
823  robot_interaction_->addInteractiveMarkers(query_goal_state_, query_marker_scale_property_->getFloat());
824  robot_interaction_->publishInteractiveMarkers();
825  }
826  if (frame_)
827  {
828  frame_->updateExternalCommunication();
829  }
830  }
831 }
832 
833 void MotionPlanningDisplay::changedQueryStartColor()
834 {
835  std_msgs::ColorRGBA color;
836  QColor qcolor = query_start_color_property_->getColor();
837  color.r = qcolor.redF();
838  color.g = qcolor.greenF();
839  color.b = qcolor.blueF();
840  color.a = 1.0f;
841  query_robot_start_->setDefaultAttachedObjectColor(color);
842  changedQueryStartState();
843 }
844 
845 void MotionPlanningDisplay::changedQueryStartAlpha()
846 {
847  query_robot_start_->setAlpha(query_start_alpha_property_->getFloat());
848  changedQueryStartState();
849 }
850 
851 void MotionPlanningDisplay::changedQueryMarkerScale()
852 {
853  if (!planning_scene_monitor_)
854  return;
855 
856  if (isEnabled())
857  {
858  // Clear the interactive markers and re-add them:
859  publishInteractiveMarkers(false);
860  }
861 }
862 
863 void MotionPlanningDisplay::changedQueryGoalColor()
864 {
865  std_msgs::ColorRGBA color;
866  QColor qcolor = query_goal_color_property_->getColor();
867  color.r = qcolor.redF();
868  color.g = qcolor.greenF();
869  color.b = qcolor.blueF();
870  color.a = 1.0f;
871  query_robot_goal_->setDefaultAttachedObjectColor(color);
872  changedQueryGoalState();
873 }
874 
875 void MotionPlanningDisplay::changedQueryGoalAlpha()
876 {
877  query_robot_goal_->setAlpha(query_goal_alpha_property_->getFloat());
878  changedQueryGoalState();
879 }
880 
881 void MotionPlanningDisplay::changedQueryCollidingLinkColor()
882 {
883  changedQueryStartState();
884  changedQueryGoalState();
885 }
886 
887 void MotionPlanningDisplay::changedQueryJointViolationColor()
888 {
889  changedQueryStartState();
890  changedQueryGoalState();
891 }
892 
893 void MotionPlanningDisplay::changedAttachedBodyColor()
894 {
895  PlanningSceneDisplay::changedAttachedBodyColor();
896  // forward color to TrajectoryVisualization
897  const QColor& color = attached_body_color_property_->getColor();
898  trajectory_visual_->setDefaultAttachedObjectColor(color);
899 }
900 
901 void MotionPlanningDisplay::scheduleDrawQueryStartState(robot_interaction::InteractionHandler* /*unused*/,
902  bool error_state_changed)
903 {
904  if (!planning_scene_monitor_)
905  return;
906  addBackgroundJob([this, pose_update = !error_state_changed] { publishInteractiveMarkers(pose_update); },
907  "publishInteractiveMarkers");
908  updateQueryStartState();
909 }
910 
911 void MotionPlanningDisplay::scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* /*unused*/,
912  bool error_state_changed)
913 {
914  if (!planning_scene_monitor_)
915  return;
916  addBackgroundJob([this, pose_update = !error_state_changed] { publishInteractiveMarkers(pose_update); },
917  "publishInteractiveMarkers");
918  updateQueryGoalState();
919 }
920 
921 void MotionPlanningDisplay::updateQueryStartState()
922 {
923  queryStartStateChanged();
924  recomputeQueryStartStateMetrics();
925  addMainLoopJob([this] { changedQueryStartState(); });
926  context_->queueRender();
927 }
928 
929 void MotionPlanningDisplay::updateQueryGoalState()
930 {
931  queryGoalStateChanged();
932  recomputeQueryGoalStateMetrics();
933  addMainLoopJob([this] { changedQueryGoalState(); });
934  context_->queueRender();
935 }
936 
937 void MotionPlanningDisplay::rememberPreviousStartState()
938 {
939  *previous_state_ = *query_start_state_->getState();
940 }
941 
942 void MotionPlanningDisplay::setQueryStartState(const moveit::core::RobotState& start)
943 {
944  query_start_state_->setState(start);
945  updateQueryStartState();
946 }
947 
948 void MotionPlanningDisplay::setQueryGoalState(const moveit::core::RobotState& goal)
949 {
950  query_goal_state_->setState(goal);
951  updateQueryGoalState();
952 }
953 
954 void MotionPlanningDisplay::useApproximateIK(bool flag)
955 {
956  if (robot_interaction_)
957  {
960  robot_interaction_->getKinematicOptionsMap()->setOptions(
963  }
964 }
965 
966 bool MotionPlanningDisplay::isIKSolutionCollisionFree(moveit::core::RobotState* state,
967  const moveit::core::JointModelGroup* group,
968  const double* ik_solution) const
969 {
970  if (frame_->ui_->collision_aware_ik->isChecked() && planning_scene_monitor_)
971  {
972  state->setJointGroupPositions(group, ik_solution);
973  state->update();
974  bool res = !getPlanningSceneRO()->isStateColliding(*state, group->getName());
975  return res;
976  }
977  else
978  return true;
979 }
980 
981 void MotionPlanningDisplay::updateLinkColors()
982 {
983  unsetAllColors(&query_robot_start_->getRobot());
984  unsetAllColors(&query_robot_goal_->getRobot());
985  std::string group = planning_group_property_->getStdString();
986  if (!group.empty())
987  {
988  setGroupColor(&query_robot_start_->getRobot(), group, query_start_color_property_->getColor());
989  setGroupColor(&query_robot_goal_->getRobot(), group, query_goal_color_property_->getColor());
990 
991  for (std::map<std::string, LinkDisplayStatus>::const_iterator it = status_links_start_.begin();
992  it != status_links_start_.end(); ++it)
993  if (it->second == COLLISION_LINK)
994  setLinkColor(&query_robot_start_->getRobot(), it->first, query_colliding_link_color_property_->getColor());
995  else
996  setLinkColor(&query_robot_start_->getRobot(), it->first,
997  query_outside_joint_limits_link_color_property_->getColor());
998 
999  for (std::map<std::string, LinkDisplayStatus>::const_iterator it = status_links_goal_.begin();
1000  it != status_links_goal_.end(); ++it)
1001  if (it->second == COLLISION_LINK)
1002  setLinkColor(&query_robot_goal_->getRobot(), it->first, query_colliding_link_color_property_->getColor());
1003  else
1004  setLinkColor(&query_robot_goal_->getRobot(), it->first,
1005  query_outside_joint_limits_link_color_property_->getColor());
1006  }
1007 }
1008 
1009 void MotionPlanningDisplay::changePlanningGroup(const std::string& group)
1010 {
1011  if (!getRobotModel() || !robot_interaction_)
1012  return;
1014  if (getRobotModel()->hasJointModelGroup(group))
1015  {
1016  planning_group_property_->setStdString(group);
1017  }
1018  else
1019  ROS_ERROR("Group [%s] not found in the robot model.", group.c_str());
1020 }
1021 
1022 void MotionPlanningDisplay::changedPlanningGroup()
1023 {
1024  if (!getRobotModel() || !robot_interaction_)
1025  return;
1026 
1027  if (!planning_group_property_->getStdString().empty() &&
1028  !getRobotModel()->hasJointModelGroup(planning_group_property_->getStdString()))
1029  {
1030  planning_group_property_->setStdString("");
1031  return;
1032  }
1033  modified_groups_.insert(planning_group_property_->getStdString());
1034 
1035  robot_interaction_->decideActiveComponents(planning_group_property_->getStdString());
1036 
1037  updateQueryStartState();
1038  updateQueryGoalState();
1039  updateLinkColors();
1040 
1041  if (frame_)
1042  frame_->changePlanningGroup();
1043  addBackgroundJob([this] { publishInteractiveMarkers(false); }, "publishInteractiveMarkers");
1044 }
1045 
1046 void MotionPlanningDisplay::changedWorkspace()
1047 {
1048  renderWorkspaceBox();
1049 }
1050 
1051 std::string MotionPlanningDisplay::getCurrentPlanningGroup() const
1052 {
1053  return planning_group_property_->getStdString();
1055 
1056 void MotionPlanningDisplay::setQueryStateHelper(bool use_start_state, const std::string& state_name)
1057 {
1058  moveit::core::RobotState state = use_start_state ? *getQueryStartState() : *getQueryGoalState();
1059 
1060  std::string v = "<" + state_name + ">";
1061 
1062  if (v == "<random>")
1063  {
1064  if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup()))
1065  state.setToRandomPositions(jmg);
1066  }
1067  else if (v == "<current>")
1068  {
1069  const planning_scene_monitor::LockedPlanningSceneRO& ps = getPlanningSceneRO();
1070  if (ps)
1071  state = ps->getCurrentState();
1072  }
1073  else if (v == "<same as goal>")
1074  {
1075  state = *getQueryGoalState();
1076  }
1077  else if (v == "<same as start>")
1078  {
1079  state = *getQueryStartState();
1080  }
1081  else
1082  {
1083  // maybe it is a named state
1084  if (const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup()))
1085  state.setToDefaultValues(jmg, state_name);
1086  }
1087 
1088  use_start_state ? setQueryStartState(state) : setQueryGoalState(state);
1089 }
1090 
1091 void MotionPlanningDisplay::populateMenuHandler(std::shared_ptr<interactive_markers::MenuHandler>& mh)
1092 {
1093  typedef interactive_markers::MenuHandler immh;
1094  std::vector<std::string> state_names;
1095  state_names.push_back("random");
1096  state_names.push_back("current");
1097  state_names.push_back("same as start");
1098  state_names.push_back("same as goal");
1099 
1100  // hacky way to distinguish between the start and goal handlers...
1101  bool is_start = (mh.get() == menu_handler_start_.get());
1102 
1103  // Commands for changing the state
1104  immh::EntryHandle menu_states =
1105  mh->insert(is_start ? "Set start state to" : "Set goal state to", immh::FeedbackCallback());
1106  for (const std::string& state_name : state_names)
1107  {
1108  // Don't add "same as start" to the start state handler, and vice versa.
1109  if ((state_name == "same as start" && is_start) || (state_name == "same as goal" && !is_start))
1110  continue;
1111  mh->insert(menu_states, state_name,
1112  [this, is_start, state_name](auto&& /*unused*/) { setQueryStateHelper(is_start, state_name); });
1113  }
1114 
1115  // // Group commands, which end up being the same for both interaction handlers
1116  // const std::vector<std::string>& group_names = getRobotModel()->getJointModelGroupNames();
1117  // immh::EntryHandle menu_groups = mh->insert("Planning Group", immh::FeedbackCallback());
1118  // for (int i = 0; i < group_names.size(); ++i)
1119  // mh->insert(menu_groups, group_names[i],
1120  // [this, &name = group_names[i]] { changePlanningGroup(name); });
1121 }
1122 
1123 void MotionPlanningDisplay::clearRobotModel()
1124 {
1125  // Invalidate all references to the RobotModel ...
1126  if (frame_)
1127  frame_->clearRobotModel();
1128  if (trajectory_visual_)
1129  trajectory_visual_->clearRobotModel();
1130  previous_state_.reset();
1131  query_start_state_.reset();
1132  query_goal_state_.reset();
1133  kinematics_metrics_.reset();
1134  robot_interaction_.reset();
1135  dynamics_solver_.clear();
1136  // ... before calling the parent's method, which finally destroys the creating RobotModelLoader.
1137  PlanningSceneDisplay::clearRobotModel();
1138 }
1139 
1140 void MotionPlanningDisplay::onRobotModelLoaded()
1141 {
1142  PlanningSceneDisplay::onRobotModelLoaded();
1143  trajectory_visual_->onRobotModelLoaded(getRobotModel());
1144 
1145  std::string ns = "rviz_moveit_motion_planning_display";
1146  std::string robot_desc_ns = ros::names::parentNamespace(robot_description_property_->getStdString());
1147  if (!robot_desc_ns.empty())
1148  ns = ros::names::append(robot_desc_ns, ns);
1149  robot_interaction_ = std::make_shared<robot_interaction::RobotInteraction>(getRobotModel(), ns);
1151  o.state_validity_callback_ = [this](moveit::core::RobotState* robot_state,
1152  const moveit::core::JointModelGroup* joint_group,
1153  const double* joint_group_variable_values) {
1154  return isIKSolutionCollisionFree(robot_state, joint_group, joint_group_variable_values);
1155  };
1156  robot_interaction_->getKinematicOptionsMap()->setOptions(
1158 
1159  int_marker_display_->subProp("Update Topic")
1160  ->setValue(QString::fromStdString(robot_interaction_->getServerTopic() + "/update"));
1161  query_robot_start_->load(*getRobotModel()->getURDF());
1162  query_robot_goal_->load(*getRobotModel()->getURDF());
1163 
1164  // initialize previous state, start state, and goal state to current state
1165  previous_state_ = std::make_shared<moveit::core::RobotState>(getPlanningSceneRO()->getCurrentState());
1166  query_start_state_ = std::make_shared<robot_interaction::InteractionHandler>(
1167  robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient());
1168  query_goal_state_ = std::make_shared<robot_interaction::InteractionHandler>(
1169  robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient());
1170  query_start_state_->setUpdateCallback(
1171  [this](robot_interaction::InteractionHandler* handler, bool error_state_changed) {
1172  scheduleDrawQueryStartState(handler, error_state_changed);
1173  });
1174  query_goal_state_->setUpdateCallback([this](robot_interaction::InteractionHandler* handler, bool error_state_changed) {
1175  scheduleDrawQueryGoalState(handler, error_state_changed);
1176  });
1177 
1178  // Interactive marker menus
1179  populateMenuHandler(menu_handler_start_);
1180  populateMenuHandler(menu_handler_goal_);
1181  query_start_state_->setMenuHandler(menu_handler_start_);
1182  query_goal_state_->setMenuHandler(menu_handler_goal_);
1183 
1184  if (!planning_group_property_->getStdString().empty())
1185  if (!getRobotModel()->hasJointModelGroup(planning_group_property_->getStdString()))
1186  planning_group_property_->setStdString("");
1187 
1188  const std::vector<std::string>& groups = getRobotModel()->getJointModelGroupNames();
1189  planning_group_property_->clearOptions();
1190  for (const std::string& group : groups)
1191  planning_group_property_->addOptionStd(group);
1192  planning_group_property_->sortOptions();
1193  if (!groups.empty() && planning_group_property_->getStdString().empty())
1194  planning_group_property_->setStdString(groups[0]);
1195 
1196  modified_groups_.clear();
1197  kinematics_metrics_ = std::make_shared<kinematics_metrics::KinematicsMetrics>(getRobotModel());
1198 
1199  geometry_msgs::Vector3 gravity_vector;
1200  gravity_vector.x = 0.0;
1201  gravity_vector.y = 0.0;
1202  gravity_vector.z = 9.81;
1203 
1204  dynamics_solver_.clear();
1205  for (const std::string& group : groups)
1206  if (getRobotModel()->getJointModelGroup(group)->isChain())
1207  dynamics_solver_[group] =
1208  std::make_shared<dynamics_solver::DynamicsSolver>(getRobotModel(), group, gravity_vector);
1209 
1210  if (frame_)
1211  frame_->fillPlanningGroupOptions();
1212  changedPlanningGroup();
1213 }
1214 void MotionPlanningDisplay::onNewPlanningSceneState()
1215 {
1216  frame_->onNewPlanningSceneState();
1217 }
1218 
1219 void MotionPlanningDisplay::updateStateExceptModified(moveit::core::RobotState& dest,
1220  const moveit::core::RobotState& src)
1221 {
1222  moveit::core::RobotState src_copy = src;
1223  for (const std::string& modified_group : modified_groups_)
1224  {
1225  const moveit::core::JointModelGroup* jmg = dest.getJointModelGroup(modified_group);
1226  if (jmg)
1227  {
1228  std::vector<double> values_to_keep;
1229  dest.copyJointGroupPositions(jmg, values_to_keep);
1230  src_copy.setJointGroupPositions(jmg, values_to_keep);
1231  }
1232  }
1233 
1234  // overwrite the destination state
1235  dest = src_copy;
1236 }
1237 
1238 void MotionPlanningDisplay::updateQueryStates(const moveit::core::RobotState& current_state)
1239 {
1240  std::string group = planning_group_property_->getStdString();
1241  if (group.empty())
1242  return;
1243 
1244  if (query_start_state_)
1245  {
1246  moveit::core::RobotState start = *getQueryStartState();
1247  updateStateExceptModified(start, current_state);
1248  if (query_start_state_property_->getBool())
1249  setQueryStartState(start);
1250  }
1252  if (query_goal_state_)
1253  {
1254  moveit::core::RobotState goal = *getQueryGoalState();
1255  updateStateExceptModified(goal, current_state);
1256  if (query_goal_state_property_->getBool())
1257  setQueryGoalState(goal);
1258  }
1259 }
1260 
1261 void MotionPlanningDisplay::onSceneMonitorReceivedUpdate(
1263 {
1264  PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type);
1265  updateQueryStates(getPlanningSceneRO()->getCurrentState());
1266  if (frame_)
1267  frame_->sceneUpdate(update_type);
1268 }
1269 
1270 // ******************************************************************************************
1271 // Enable
1272 // ******************************************************************************************
1273 void MotionPlanningDisplay::onEnable()
1274 {
1275  PlanningSceneDisplay::onEnable();
1276 
1277  // Planned Path Display
1278  trajectory_visual_->onEnable();
1279 
1280  text_to_display_->setVisible(false);
1281 
1282  query_robot_start_->setVisible(query_start_state_property_->getBool());
1283  query_robot_goal_->setVisible(query_goal_state_property_->getBool());
1284 
1285  int_marker_display_->setEnabled(true);
1286  int_marker_display_->setFixedFrame(fixed_frame_);
1287 
1288  frame_->enable();
1289 }
1290 
1291 // ******************************************************************************************
1292 // Disable
1293 // ******************************************************************************************
1294 void MotionPlanningDisplay::onDisable()
1295 {
1296  if (robot_interaction_)
1297  robot_interaction_->clear();
1298  int_marker_display_->setEnabled(false);
1299 
1300  query_robot_start_->setVisible(false);
1301  query_robot_goal_->setVisible(false);
1302  text_to_display_->setVisible(false);
1303 
1304  PlanningSceneDisplay::onDisable();
1306  // Planned Path Display
1307  trajectory_visual_->onDisable();
1308 
1309  frame_->disable();
1310 }
1311 
1312 // ******************************************************************************************
1313 // Update
1314 // ******************************************************************************************
1315 void MotionPlanningDisplay::update(float wall_dt, float ros_dt)
1316 {
1317  if (int_marker_display_)
1318  int_marker_display_->update(wall_dt, ros_dt);
1319  if (frame_)
1320  frame_->updateSceneMarkers(wall_dt, ros_dt);
1321 
1322  PlanningSceneDisplay::update(wall_dt, ros_dt);
1323 }
1324 
1325 void MotionPlanningDisplay::updateInternal(float wall_dt, float ros_dt)
1327  PlanningSceneDisplay::updateInternal(wall_dt, ros_dt);
1328 
1329  // Planned Path Display
1330  trajectory_visual_->update(wall_dt, ros_dt);
1331 
1332  renderWorkspaceBox();
1333 }
1334 
1335 void MotionPlanningDisplay::load(const rviz::Config& config)
1336 {
1337  PlanningSceneDisplay::load(config);
1338  if (frame_)
1339  {
1340  float d;
1341  if (config.mapGetFloat("MoveIt_Planning_Time", &d))
1342  frame_->ui_->planning_time->setValue(d);
1343  int attempts;
1344  if (config.mapGetInt("MoveIt_Planning_Attempts", &attempts))
1345  frame_->ui_->planning_attempts->setValue(attempts);
1346  if (config.mapGetFloat("Velocity_Scaling_Factor", &d))
1347  frame_->ui_->velocity_scaling_factor->setValue(d);
1348  if (config.mapGetFloat("Acceleration_Scaling_Factor", &d))
1349  frame_->ui_->acceleration_scaling_factor->setValue(d);
1350 
1351  bool b;
1352  if (config.mapGetBool("MoveIt_Allow_Replanning", &b))
1353  frame_->ui_->allow_replanning->setChecked(b);
1354  if (config.mapGetBool("MoveIt_Allow_Sensor_Positioning", &b))
1355  frame_->ui_->allow_looking->setChecked(b);
1356  if (config.mapGetBool("MoveIt_Allow_External_Program", &b))
1357  frame_->ui_->allow_external_program->setChecked(b);
1358  if (config.mapGetBool("MoveIt_Use_Cartesian_Path", &b))
1359  frame_->ui_->use_cartesian_path->setChecked(b);
1360  if (config.mapGetBool("MoveIt_Use_Constraint_Aware_IK", &b))
1361  frame_->ui_->collision_aware_ik->setChecked(b);
1362  if (config.mapGetBool("MoveIt_Allow_Approximate_IK", &b))
1363  frame_->ui_->approximate_ik->setChecked(b);
1364  if (config.mapGetBool("JointsTab_Use_Radians", &b))
1365  frame_->joints_tab_->setUseRadians(true);
1366 
1367  rviz::Config workspace = config.mapGetChild("MoveIt_Workspace");
1368  rviz::Config ws_center = workspace.mapGetChild("Center");
1369  float val;
1370  if (ws_center.mapGetFloat("X", &val))
1371  frame_->ui_->wcenter_x->setValue(val);
1372  if (ws_center.mapGetFloat("Y", &val))
1373  frame_->ui_->wcenter_y->setValue(val);
1374  if (ws_center.mapGetFloat("Z", &val))
1375  frame_->ui_->wcenter_z->setValue(val);
1376 
1377  rviz::Config ws_size = workspace.mapGetChild("Size");
1378  if (ws_size.isValid())
1379  {
1380  if (ws_size.mapGetFloat("X", &val))
1381  frame_->ui_->wsize_x->setValue(val);
1382  if (ws_size.mapGetFloat("Y", &val))
1383  frame_->ui_->wsize_y->setValue(val);
1384  if (ws_size.mapGetFloat("Z", &val))
1385  frame_->ui_->wsize_z->setValue(val);
1386  }
1387  else
1388  {
1389  ros::NodeHandle nh(ros::names::append(getMoveGroupNS(), "move_group"));
1390  double val;
1391  if (nh.getParam("default_workspace_bounds", val))
1392  {
1393  frame_->ui_->wsize_x->setValue(val);
1394  frame_->ui_->wsize_y->setValue(val);
1395  frame_->ui_->wsize_z->setValue(val);
1396  }
1397  }
1398  }
1399 }
1400 
1401 void MotionPlanningDisplay::save(rviz::Config config) const
1402 {
1403  PlanningSceneDisplay::save(config);
1404  if (frame_)
1405  {
1406  // "Options" Section
1407  config.mapSetValue("MoveIt_Planning_Time", frame_->ui_->planning_time->value());
1408  config.mapSetValue("MoveIt_Planning_Attempts", frame_->ui_->planning_attempts->value());
1409  config.mapSetValue("Velocity_Scaling_Factor", frame_->ui_->velocity_scaling_factor->value());
1410  config.mapSetValue("Acceleration_Scaling_Factor", frame_->ui_->acceleration_scaling_factor->value());
1411 
1412  config.mapSetValue("MoveIt_Allow_Replanning", frame_->ui_->allow_replanning->isChecked());
1413  config.mapSetValue("MoveIt_Allow_Sensor_Positioning", frame_->ui_->allow_looking->isChecked());
1414  config.mapSetValue("MoveIt_Allow_External_Program", frame_->ui_->allow_external_program->isChecked());
1415  config.mapSetValue("MoveIt_Use_Cartesian_Path", frame_->ui_->use_cartesian_path->isChecked());
1416  config.mapSetValue("MoveIt_Use_Constraint_Aware_IK", frame_->ui_->collision_aware_ik->isChecked());
1417  config.mapSetValue("MoveIt_Allow_Approximate_IK", frame_->ui_->approximate_ik->isChecked());
1418  config.mapSetValue("JointsTab_Use_Radians", frame_->joints_tab_->useRadians());
1419 
1420  rviz::Config workspace = config.mapMakeChild("MoveIt_Workspace");
1421  rviz::Config ws_center = workspace.mapMakeChild("Center");
1422  ws_center.mapSetValue("X", frame_->ui_->wcenter_x->value());
1423  ws_center.mapSetValue("Y", frame_->ui_->wcenter_y->value());
1424  ws_center.mapSetValue("Z", frame_->ui_->wcenter_z->value());
1425  rviz::Config ws_size = workspace.mapMakeChild("Size");
1426  ws_size.mapSetValue("X", frame_->ui_->wsize_x->value());
1427  ws_size.mapSetValue("Y", frame_->ui_->wsize_y->value());
1428  ws_size.mapSetValue("Z", frame_->ui_->wsize_z->value());
1429  }
1430 }
1431 
1432 void MotionPlanningDisplay::fixedFrameChanged()
1434  PlanningSceneDisplay::fixedFrameChanged();
1435  if (int_marker_display_)
1436  int_marker_display_->setFixedFrame(fixed_frame_);
1437  changedPlanningGroup();
1438 }
1439 
1440 // Pick and place
1441 void MotionPlanningDisplay::clearPlaceLocationsDisplay()
1442 {
1443  for (std::shared_ptr<rviz::Shape>& place_location_shape : place_locations_display_)
1444  place_location_shape.reset();
1445  place_locations_display_.clear();
1446 }
1447 
1448 void MotionPlanningDisplay::visualizePlaceLocations(const std::vector<geometry_msgs::PoseStamped>& place_poses)
1449 {
1450  clearPlaceLocationsDisplay();
1451  place_locations_display_.resize(place_poses.size());
1452  for (std::size_t i = 0; i < place_poses.size(); ++i)
1453  {
1454  place_locations_display_[i] = std::make_shared<rviz::Shape>(rviz::Shape::Sphere, context_->getSceneManager());
1455  place_locations_display_[i]->setColor(1.0f, 0.0f, 0.0f, 0.3f);
1456  Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y, place_poses[i].pose.position.z);
1457  Ogre::Vector3 extents(0.02, 0.02, 0.02);
1458  place_locations_display_[i]->setScale(extents);
1459  place_locations_display_[i]->setPosition(center);
1460  }
1461 }
1462 
1463 } // namespace moveit_rviz_plugin
rviz::MovableText::V_CENTER
V_CENTER
moveit::core::LinkModel
moveit::core::JointModelGroup::getActiveJointModels
const std::vector< const JointModel * > & getActiveJointModels() const
collision_detection::CollisionResult::ContactMap
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
kinematics::KinematicsQueryOptions::return_approximate_solution
bool return_approximate_solution
rviz::Config::isValid
bool isValid() const
motion_planning_display.h
moveit_rviz_plugin::MotionPlanningDisplay::MotionPlanningDisplay
MotionPlanningDisplay()
Definition: motion_planning_display.cpp:114
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
rviz::BoolProperty
val
val
frame_manager.h
enum_property.h
moveit::core::RobotState
moveit::core::RobotState::copyJointGroupPositions
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
robot_interaction::EndEffectorInteraction::parent_group
std::string parent_group
robot_interaction::KinematicOptions::RETURN_APPROXIMATE_SOLUTION
RETURN_APPROXIMATE_SOLUTION
rviz::MovableText
shape.h
ros::names::parentNamespace
ROSCPP_DECL std::string parentNamespace(const std::string &name)
moveit::tools::BackgroundProcessing::JobEvent
JobEvent
robot_interaction::KinematicOptions::STATE_VALIDITY_CALLBACK
STATE_VALIDITY_CALLBACK
float_property.h
rviz::ColorProperty
rviz::MovableText::H_CENTER
H_CENTER
visualization_manager.h
rviz::EnumProperty
rviz::FloatProperty
planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType
SceneUpdateType
rviz::Property
interactive_markers::MenuHandler
rviz::Config::mapGetChild
Config mapGetChild(const QString &key) const
res
res
bool_property.h
moveit::core::RobotState::update
void update(bool force=false)
window_manager_interface.h
rviz::Config::mapMakeChild
Config mapMakeChild(const QString &key)
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
ROS_ERROR
#define ROS_ERROR(...)
value
float value
setup.d
d
Definition: setup.py:4
robot_interaction::KinematicOptions::state_validity_callback_
moveit::core::GroupStateValidityCallbackFn state_validity_callback_
rviz::WindowManagerInterface::getParentWindow
virtual QWidget * getParentWindow()=0
name
name
rviz::Shape::Sphere
Sphere
ORIENTATION
ORIENTATION
robot_interaction::EndEffectorInteraction
robot_interaction::KinematicOptions::options_
kinematics::KinematicsQueryOptions options_
moveit::core::JointModelGroup::getLinkModelNames
const std::vector< std::string > & getLinkModelNames() const
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit_rviz_plugin::MotionPlanningFrame
Definition: motion_planning_frame.h:98
start
ROSCPP_DECL void start()
f
f
ros::names::append
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
values
std::vector< double > values
rviz::WindowManagerInterface::addPane
virtual PanelDockWidget * addPane(const QString &name, QWidget *pane, Qt::DockWidgetArea area=Qt::LeftDockWidgetArea, bool floating=false)=0
planning_scene_monitor::LockedPlanningSceneRO
robot_interaction::KinematicOptions
property.h
moveit::core::JointModelGroup
display_factory.h
v
v
movable_text.h
robot_interaction::KinematicOptionsMap::ALL
static const std::string ALL
conversions.h
motion_planning_frame_joints_widget.h
kinematic_options_map.h
robot_state_visualization.h
interactive_markers
rviz::WindowManagerInterface
trajectory_tools.h
moveit::core::RobotState::setJointGroupPositions
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
string_property.h
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
extents
std::array< S, 6 > & extents()
rviz::Config::mapGetFloat
bool mapGetFloat(const QString &key, float *value_out) const
robot_interaction::InteractionHandler
t
dictionary t
panel_dock_widget.h
config
config
rviz::Config::setValue
void setValue(const QVariant &value)
rviz::Config
color_property.h
moveit::core::JointModel
ros_topic_property.h
rviz::Config::mapSetValue
void mapSetValue(const QString &key, const QVariant &value)
ros::NodeHandle
robot.h
rviz::Shape::Cube
Cube
display_context.h
robot_interaction::KinematicOptionsMap::DEFAULT
static const std::string DEFAULT
group
group


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:25