moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
43 #include <rviz_default_plugins/robot/robot.hpp>
44 #include <rviz_default_plugins/robot/robot_link.hpp>
46 
47 #include <rviz_common/properties/property.hpp>
48 #include <rviz_common/properties/string_property.hpp>
49 #include <rviz_common/properties/bool_property.hpp>
50 #include <rviz_common/properties/float_property.hpp>
51 #include <rviz_common/properties/ros_topic_property.hpp>
52 #include <rviz_common/properties/editable_enum_property.hpp>
53 #include <rviz_common/properties/color_property.hpp>
54 #include <rviz_common/display_context.hpp>
55 #include <rviz_common/frame_manager_iface.hpp>
56 #include <rviz_common/panel_dock_widget.hpp>
57 #include <rviz_common/window_manager_interface.hpp>
58 #include <rviz_common/display.hpp>
59 #include <rviz_rendering/objects/movable_text.hpp>
60 
61 #include <OgreSceneManager.h>
62 #include <OgreSceneNode.h>
63 #include <rviz_rendering/objects/shape.hpp>
64 
67 
68 #include <boost/format.hpp>
69 #include <boost/algorithm/string/replace.hpp>
70 #include <boost/algorithm/string/trim.hpp>
71 
72 #include <QShortcut>
73 
74 #include "ui_motion_planning_rviz_plugin_frame.h"
76 
77 namespace moveit_rviz_plugin
78 {
79 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_display");
80 
81 // ******************************************************************************************
82 // Base class constructor
83 // ******************************************************************************************
86  , text_to_display_(nullptr)
87  , frame_(nullptr)
88  , frame_dock_(nullptr)
89  , menu_handler_start_(std::make_shared<interactive_markers::MenuHandler>())
90  , menu_handler_goal_(std::make_shared<interactive_markers::MenuHandler>())
91  , int_marker_display_(nullptr)
92 {
93  // Category Groups
94  plan_category_ = new rviz_common::properties::Property("Planning Request", QVariant(), "", this);
95  metrics_category_ = new rviz_common::properties::Property("Planning Metrics", QVariant(), "", this);
96  path_category_ = new rviz_common::properties::Property("Planned Path", QVariant(), "", this);
97 
98  // Metrics category -----------------------------------------------------------------------------------------
100  new rviz_common::properties::BoolProperty("Show Weight Limit", false,
101  "Shows the weight limit at a particular pose for an end-effector",
102  metrics_category_, SLOT(changedShowWeightLimit()), this);
103 
105  new rviz_common::properties::BoolProperty("Show Manipulability Index", false,
106  "Shows the manipulability index for an end-effector", metrics_category_,
107  SLOT(changedShowManipulabilityIndex()), this);
108 
110  new rviz_common::properties::BoolProperty("Show Manipulability", false,
111  "Shows the manipulability for an end-effector", metrics_category_,
112  SLOT(changedShowManipulability()), this);
113 
115  new rviz_common::properties::BoolProperty("Show Joint Torques", false,
116  "Shows the joint torques for a given configuration and payload",
117  metrics_category_, SLOT(changedShowJointTorques()), this);
118 
120  new rviz_common::properties::FloatProperty("Payload", 1.0f, "Specify the payload at the end effector (kg)",
121  metrics_category_, SLOT(changedMetricsSetPayload()), this);
122  metrics_set_payload_property_->setMin(0.0);
123 
124  metrics_text_height_property_ = new rviz_common::properties::FloatProperty(
125  "TextHeight", 0.08f, "Text height", metrics_category_, SLOT(changedMetricsTextHeight()), this);
126  metrics_text_height_property_->setMin(0.001);
127 
128  // Planning request category -----------------------------------------------------------------------------------------
129 
130  planning_group_property_ = new rviz_common::properties::EditableEnumProperty(
131  "Planning Group", "", "The name of the group of links to plan for (from the ones defined in the SRDF)",
132  plan_category_, SLOT(changedPlanningGroup()), this);
133  show_workspace_property_ = new rviz_common::properties::BoolProperty("Show Workspace", false,
134  "Shows the axis-aligned bounding box for "
135  "the workspace allowed for planning",
136  plan_category_, SLOT(changedWorkspace()), this);
138  new rviz_common::properties::BoolProperty("Query Start State", false,
139  "Set a custom start state for the motion planning query",
140  plan_category_, SLOT(changedQueryStartState()), this);
142  new rviz_common::properties::BoolProperty("Query Goal State", true,
143  "Shows the goal state for the motion planning query", plan_category_,
144  SLOT(changedQueryGoalState()), this);
145  query_marker_scale_property_ = new rviz_common::properties::FloatProperty(
146  "Interactive Marker Size", 0.0f,
147  "Specifies scale of the interactive marker overlaid on the robot. 0 is auto scale.", plan_category_,
148  SLOT(changedQueryMarkerScale()), this);
149  query_marker_scale_property_->setMin(0.0f);
150 
152  new rviz_common::properties::ColorProperty("Start State Color", QColor(0, 255, 0),
153  "The highlight color for the start state", plan_category_,
154  SLOT(changedQueryStartColor()), this);
156  new rviz_common::properties::FloatProperty("Start State Alpha", 1.0f, "Specifies the alpha for the robot links",
157  plan_category_, SLOT(changedQueryStartAlpha()), this);
158  query_start_alpha_property_->setMin(0.0);
159  query_start_alpha_property_->setMax(1.0);
160 
162  new rviz_common::properties::ColorProperty("Goal State Color", QColor(250, 128, 0),
163  "The highlight color for the goal state", plan_category_,
164  SLOT(changedQueryGoalColor()), this);
165 
167  new rviz_common::properties::FloatProperty("Goal State Alpha", 1.0f, "Specifies the alpha for the robot links",
168  plan_category_, SLOT(changedQueryGoalAlpha()), this);
169  query_goal_alpha_property_->setMin(0.0);
170  query_goal_alpha_property_->setMax(1.0);
171 
173  new rviz_common::properties::ColorProperty("Colliding Link Color", QColor(255, 0, 0),
174  "The highlight color for colliding links", plan_category_,
175  SLOT(changedQueryCollidingLinkColor()), this);
176 
177  query_outside_joint_limits_link_color_property_ = new rviz_common::properties::ColorProperty(
178  "Joint Violation Color", QColor(255, 0, 255),
179  "The highlight color for child links of joints that are outside bounds", plan_category_,
180  SLOT(changedQueryJointViolationColor()), this);
181 
182  // Trajectory playback / planned path category ---------------------------------------------
183  trajectory_visual_ = std::make_shared<TrajectoryVisualization>(path_category_, this);
184 
185  // Start background jobs
187  const std::string& name) { backgroundJobUpdate(event, name); });
188 }
189 
190 // ******************************************************************************************
191 // Deconstructor
192 // ******************************************************************************************
194 {
196  clearJobs();
197 
198  query_robot_start_.reset();
199  query_robot_goal_.reset();
200 
201  delete text_to_display_;
202  delete int_marker_display_;
203  delete frame_dock_;
204 }
205 
207 {
209 
210  // Planned Path Display
211  trajectory_visual_->onInitialize(node_, planning_scene_node_, context_);
212  QColor qcolor = attached_body_color_property_->getColor();
213  trajectory_visual_->setDefaultAttachedObjectColor(qcolor);
214 
216  std::make_shared<RobotStateVisualization>(planning_scene_node_, context_, "Planning Request Start", nullptr);
217  query_robot_start_->setCollisionVisible(false);
218  query_robot_start_->setVisualVisible(true);
219  query_robot_start_->setVisible(query_start_state_property_->getBool());
220  std_msgs::msg::ColorRGBA color;
221  qcolor = query_start_color_property_->getColor();
222  color.r = qcolor.redF();
223  color.g = qcolor.greenF();
224  color.b = qcolor.blueF();
225  color.a = 1.0f;
226  query_robot_start_->setDefaultAttachedObjectColor(color);
227 
229  std::make_shared<RobotStateVisualization>(planning_scene_node_, context_, "Planning Request Goal", nullptr);
230  query_robot_goal_->setCollisionVisible(false);
231  query_robot_goal_->setVisualVisible(true);
232  query_robot_goal_->setVisible(query_goal_state_property_->getBool());
233  qcolor = query_goal_color_property_->getColor();
234  color.r = qcolor.redF();
235  color.g = qcolor.greenF();
236  color.b = qcolor.blueF();
237  query_robot_goal_->setDefaultAttachedObjectColor(color);
238 
239  rviz_common::WindowManagerInterface* window_context = context_->getWindowManager();
240  frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr);
241 
242  connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged()));
244  addStatusText("Initialized.");
245 
246  // immediately switch to next trajectory display after planning
247  connect(frame_, SIGNAL(planningFinished()), trajectory_visual_.get(), SLOT(interruptCurrentDisplay()));
248 
249  if (window_context)
250  {
251  frame_dock_ = window_context->addPane(getName(), frame_);
252  connect(frame_dock_, SIGNAL(visibilityChanged(bool)), this, SLOT(motionPanelVisibilityChange(bool)));
253  frame_dock_->setIcon(getIcon());
254  }
255 
257  int_marker_display_->initialize(context_);
258 
259  text_display_scene_node_ = planning_scene_node_->createChildSceneNode();
260  text_to_display_ = new rviz_rendering::MovableText("EMPTY");
261  text_to_display_->setTextAlignment(rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_CENTER);
262  text_to_display_->setCharacterHeight(metrics_text_height_property_->getFloat());
263  text_to_display_->showOnTop();
264  text_to_display_->setVisible(false);
265  text_display_for_start_ = false;
267 
268  if (context_ && context_->getWindowManager() && context_->getWindowManager()->getParentWindow())
269  {
270  QShortcut* im_reset_shortcut =
271  new QShortcut(QKeySequence("Ctrl+I"), context_->getWindowManager()->getParentWindow());
272  connect(im_reset_shortcut, SIGNAL(activated()), this, SLOT(resetInteractiveMarkers()));
273  }
274 }
275 
276 void MotionPlanningDisplay::motionPanelVisibilityChange(bool enable)
277 {
278  if (enable)
279  setEnabled(true);
280 }
281 
283 {
284  if (enable)
285  {
286  planning_group_sub_ = node_->create_subscription<std_msgs::msg::String>(
287  "/rviz/moveit/select_planning_group", rclcpp::SystemDefaultsQoS(),
288  [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return selectPlanningGroupCallback(msg); });
289  }
290  else
291  {
292  planning_group_sub_.reset();
293  }
294 }
295 
296 void MotionPlanningDisplay::selectPlanningGroupCallback(const std_msgs::msg::String::ConstSharedPtr& msg)
297 {
298  // synchronize ROS callback with main loop
299  addMainLoopJob([this, group = msg->data] { changePlanningGroup(group); });
300 }
301 
303 {
304  text_to_display_->setVisible(false);
305 
306  query_robot_start_->clear();
307  query_robot_goal_->clear();
308 
310 
311  // Planned Path Display
312  trajectory_visual_->reset();
313 
314  bool enabled = this->isEnabled();
315  frame_->disable();
316  if (enabled)
317  {
318  frame_->enable();
319  query_robot_start_->setVisible(query_start_state_property_->getBool());
320  query_robot_goal_->setVisible(query_goal_state_property_->getBool());
321  }
322 }
323 
325  const std::string& /*unused*/)
326 {
328 }
329 
331 {
332  if (!frame_)
333  return;
334  QProgressBar* p = frame_->ui_->background_job_progress;
336 
337  if (n == 0)
338  {
339  p->hide();
340  p->setMaximum(0);
341  p->setValue(0);
342  }
343  else
344  {
345  if (p->maximum() < n) // increase max
346  {
347  p->setMaximum(n);
348  if (n > 1) // only show bar if there will be a progress to show
349  p->show();
350  }
351  else // progress
352  p->setValue(p->maximum() - n);
353  p->update();
354  }
355 }
356 
357 void MotionPlanningDisplay::changedShowWeightLimit()
358 {
360  {
361  if (query_start_state_property_->getBool())
362  displayMetrics(true);
363  }
364  else
365  {
366  if (query_goal_state_property_->getBool())
367  displayMetrics(false);
368  }
369 }
370 
371 void MotionPlanningDisplay::changedShowManipulabilityIndex()
372 {
374  {
375  if (query_start_state_property_->getBool())
376  displayMetrics(true);
377  }
378  else
379  {
380  if (query_goal_state_property_->getBool())
381  displayMetrics(false);
382  }
383 }
384 
385 void MotionPlanningDisplay::changedShowManipulability()
386 {
388  {
389  if (query_start_state_property_->getBool())
390  displayMetrics(true);
391  }
392  else
393  {
394  if (query_goal_state_property_->getBool())
395  displayMetrics(false);
396  }
397 }
398 
399 void MotionPlanningDisplay::changedShowJointTorques()
400 {
402  {
403  if (query_start_state_property_->getBool())
404  displayMetrics(true);
405  }
406  else
407  {
408  if (query_goal_state_property_->getBool())
409  displayMetrics(false);
410  }
411 }
412 
413 void MotionPlanningDisplay::changedMetricsSetPayload()
414 {
416  {
417  if (query_start_state_property_->getBool())
418  displayMetrics(true);
419  }
420  else
421  {
422  if (query_goal_state_property_->getBool())
423  displayMetrics(false);
424  }
425 }
426 
427 void MotionPlanningDisplay::changedMetricsTextHeight()
428 {
429  text_to_display_->setCharacterHeight(metrics_text_height_property_->getFloat());
430 }
431 
432 void MotionPlanningDisplay::displayTable(const std::map<std::string, double>& values, const Ogre::ColourValue& color,
433  const Ogre::Vector3& pos, const Ogre::Quaternion& orient)
434 {
435  // the line we want to render
436  std::stringstream ss;
437  for (const std::pair<const std::string, double>& value : values)
438  ss << boost::format("%-10s %-4.2f") % value.first % value.second << '\n';
439 
440  if (ss.str().empty())
441  {
442  text_to_display_->setVisible(false);
443  return;
444  }
445 
446  text_to_display_->setCaption(ss.str());
447  text_to_display_->setColor(color);
448  text_display_scene_node_->setPosition(pos);
449  text_display_scene_node_->setOrientation(orient);
450 
451  // make sure the node is visible
452  text_to_display_->setVisible(true);
453 }
454 
456 {
457  if (!frame_ || !show_workspace_property_->getBool())
458  {
459  if (workspace_box_)
460  workspace_box_.reset();
461  return;
462  }
463 
464  if (!workspace_box_)
465  {
466  workspace_box_ = std::make_unique<rviz_rendering::Shape>(rviz_rendering::Shape::Cube, context_->getSceneManager(),
468  workspace_box_->setColor(0.0f, 0.0f, 0.6f, 0.3f);
469  }
470 
471  Ogre::Vector3 center(frame_->ui_->wcenter_x->value(), frame_->ui_->wcenter_y->value(),
472  frame_->ui_->wcenter_z->value());
473  Ogre::Vector3 extents(frame_->ui_->wsize_x->value(), frame_->ui_->wsize_y->value(), frame_->ui_->wsize_z->value());
474  workspace_box_->setScale(extents);
475  workspace_box_->setPosition(center);
476 }
477 
478 void MotionPlanningDisplay::computeMetrics(bool start, const std::string& group, double payload)
479 {
480  if (!robot_interaction_)
481  return;
482  const std::vector<robot_interaction::EndEffectorInteraction>& eef = robot_interaction_->getActiveEndEffectors();
483  if (eef.empty())
484  return;
485  std::scoped_lock slock(update_metrics_lock_);
486 
487  moveit::core::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState();
488  for (const robot_interaction::EndEffectorInteraction& ee : eef)
489  if (ee.parent_group == group)
490  computeMetricsInternal(computed_metrics_[std::make_pair(start, group)], ee, *state, payload);
491 }
492 
493 void MotionPlanningDisplay::computeMetricsInternal(std::map<std::string, double>& metrics,
495  const moveit::core::RobotState& state, double payload)
496 {
497  metrics.clear();
498  dynamics_solver::DynamicsSolverPtr ds;
499  std::map<std::string, dynamics_solver::DynamicsSolverPtr>::const_iterator it = dynamics_solver_.find(ee.parent_group);
500  if (it != dynamics_solver_.end())
501  ds = it->second;
502 
503  // Max payload
504  if (ds)
505  {
506  double max_payload;
507  unsigned int saturated_joint;
508  std::vector<double> joint_values;
509  state.copyJointGroupPositions(ee.parent_group, joint_values);
510  if (ds->getMaxPayload(joint_values, max_payload, saturated_joint))
511  {
512  metrics["max_payload"] = max_payload;
513  metrics["saturated_joint"] = saturated_joint;
514  }
515  std::vector<double> joint_torques;
516  joint_torques.resize(joint_values.size());
517  if (ds->getPayloadTorques(joint_values, payload, joint_torques))
518  {
519  for (std::size_t i = 0; i < joint_torques.size(); ++i)
520  {
521  std::stringstream stream;
522  stream << "torque[" << i << "]";
523  metrics[stream.str()] = joint_torques[i];
524  }
525  }
526  }
527 
529  {
530  if (position_only_ik_.find(ee.parent_group) == position_only_ik_.end())
531  node_->get_parameter_or(ee.parent_group + ".position_only_ik", position_only_ik_[ee.parent_group], false);
532 
533  double manipulability_index, manipulability;
534  bool position_ik = position_only_ik_[ee.parent_group];
535  if (kinematics_metrics_->getManipulabilityIndex(state, ee.parent_group, manipulability_index, position_ik))
536  metrics["manipulability_index"] = manipulability_index;
537  if (kinematics_metrics_->getManipulability(state, ee.parent_group, manipulability))
538  metrics["manipulability"] = manipulability;
539  }
540 }
541 
542 namespace
543 {
544 inline void copyItemIfExists(const std::map<std::string, double>& source, std::map<std::string, double>& dest,
545  const std::string& key)
546 {
547  std::map<std::string, double>::const_iterator it = source.find(key);
548  if (it != source.end())
549  dest[key] = it->second;
550 }
551 } // namespace
552 
554 {
556  return;
557 
558  static const Ogre::Quaternion ORIENTATION(1.0, 0.0, 0.0, 0.0);
559  const std::vector<robot_interaction::EndEffectorInteraction>& eef = robot_interaction_->getActiveEndEffectors();
560  if (eef.empty())
561  return;
562 
563  moveit::core::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState();
564 
565  for (const robot_interaction::EndEffectorInteraction& ee : eef)
566  {
567  Ogre::Vector3 position(0.0, 0.0, 0.0);
568  std::map<std::string, double> text_table;
569  const std::map<std::string, double>& metrics_table = computed_metrics_[std::make_pair(start, ee.parent_group)];
570  if (compute_weight_limit_property_->getBool())
571  {
572  copyItemIfExists(metrics_table, text_table, "max_payload");
573  copyItemIfExists(metrics_table, text_table, "saturated_joint");
574  }
576  copyItemIfExists(metrics_table, text_table, "manipulability_index");
577  if (show_manipulability_property_->getBool())
578  copyItemIfExists(metrics_table, text_table, "manipulability");
579  if (show_joint_torques_property_->getBool())
580  {
581  std::size_t nj = getRobotModel()->getJointModelGroup(ee.parent_group)->getJointModelNames().size();
582  for (size_t j = 0; j < nj; ++j)
583  {
584  std::stringstream stream;
585  stream << "torque[" << j << "]";
586  copyItemIfExists(metrics_table, text_table, stream.str());
587  }
588  }
589 
590  const moveit::core::LinkModel* lm = nullptr;
591  const moveit::core::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(ee.parent_group);
592  if (jmg)
593  if (!jmg->getLinkModelNames().empty())
594  lm = state->getLinkModel(jmg->getLinkModelNames().back());
595  if (lm)
596  {
597  const Eigen::Vector3d& t = state->getGlobalLinkTransform(lm).translation();
598  position[0] = t.x();
599  position[1] = t.y();
600  position[2] = t.z() + 0.2; // \todo this should be a param
601  }
602  if (start)
603  displayTable(text_table, query_start_color_property_->getOgreColor(), position, ORIENTATION);
604  else
605  displayTable(text_table, query_goal_color_property_->getOgreColor(), position, ORIENTATION);
606  text_display_for_start_ = start;
607  }
608 }
609 
611 {
613  return;
614 
615  if (query_start_state_property_->getBool())
616  {
617  if (isEnabled())
618  {
619  moveit::core::RobotStateConstPtr state = getQueryStartState();
620 
621  // update link poses
622  query_robot_start_->update(state);
623  query_robot_start_->setVisible(true);
624 
625  // update link colors
626  std::vector<std::string> collision_links;
627  getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
628  status_links_start_.clear();
629  for (const std::string& collision_link : collision_links)
630  status_links_start_[collision_link] = COLLISION_LINK;
631  if (!collision_links.empty())
632  {
634  getPlanningSceneRO()->getCollidingPairs(pairs, *state);
636  addStatusText("Start state colliding links:");
637  for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin(); it != pairs.end();
638  ++it)
639  addStatusText(it->first.first + " - " + it->first.second);
640  addStatusText(".");
641  }
642  if (!getCurrentPlanningGroup().empty())
643  {
644  const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup());
645  if (jmg)
646  {
647  std::vector<std::string> outside_bounds;
648  const std::vector<const moveit::core::JointModel*>& jmodels = jmg->getActiveJointModels();
649  for (const moveit::core::JointModel* jmodel : jmodels)
650  if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2))
651  {
652  outside_bounds.push_back(jmodel->getChildLinkModel()->getName());
653  status_links_start_[outside_bounds.back()] = OUTSIDE_BOUNDS_LINK;
654  }
655  if (!outside_bounds.empty())
656  {
658  addStatusText("Links descending from joints that are outside bounds in start state:");
659  addStatusText(outside_bounds);
660  }
661  }
662  }
664  // update metrics text
665  displayMetrics(true);
666  }
667  }
668  else
669  query_robot_start_->setVisible(false);
670  context_->queueRender();
671 }
672 
674 {
675  setStatusTextColor(Qt::darkGray);
676 }
677 
679 {
680  if (frame_)
681  frame_->ui_->status_text->setTextColor(color);
682 }
683 
685 {
686  if (frame_)
687  frame_->ui_->status_text->append(QString::fromStdString(text));
688 }
689 
690 void MotionPlanningDisplay::addStatusText(const std::vector<std::string>& text)
691 {
692  for (const std::string& it : text)
693  addStatusText(it);
694 }
695 
697 {
698  std::string group = planning_group_property_->getStdString();
699  if (!group.empty())
701 }
702 
704 {
705  std::string group = planning_group_property_->getStdString();
706  if (!group.empty())
708 }
709 
710 void MotionPlanningDisplay::changedQueryStartState()
711 {
713  return;
715  addStatusText("Changed start state");
717  addBackgroundJob([this] { publishInteractiveMarkers(true); }, "publishInteractiveMarkers");
718 }
719 
720 void MotionPlanningDisplay::changedQueryGoalState()
721 {
723  return;
725  addStatusText("Changed goal state");
727  addBackgroundJob([this] { publishInteractiveMarkers(true); }, "publishInteractiveMarkers");
728 }
729 
731 {
733  return;
734  if (query_goal_state_property_->getBool())
735  {
736  if (isEnabled())
737  {
738  moveit::core::RobotStateConstPtr state = getQueryGoalState();
739 
740  // update link poses
741  query_robot_goal_->update(state);
742  query_robot_goal_->setVisible(true);
743 
744  // update link colors
745  std::vector<std::string> collision_links;
746  getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
747  status_links_goal_.clear();
748  for (const std::string& collision_link : collision_links)
749  status_links_goal_[collision_link] = COLLISION_LINK;
750  if (!collision_links.empty())
751  {
753  getPlanningSceneRO()->getCollidingPairs(pairs, *state);
755  addStatusText("Goal state colliding links:");
756  for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin(); it != pairs.end();
757  ++it)
758  addStatusText(it->first.first + " - " + it->first.second);
759  addStatusText(".");
760  }
761 
762  if (!getCurrentPlanningGroup().empty())
763  {
764  const moveit::core::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup());
765  if (jmg)
766  {
767  const std::vector<const moveit::core::JointModel*>& jmodels = jmg->getActiveJointModels();
768  std::vector<std::string> outside_bounds;
769  for (const moveit::core::JointModel* jmodel : jmodels)
770  if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2))
771  {
772  outside_bounds.push_back(jmodel->getChildLinkModel()->getName());
773  status_links_goal_[outside_bounds.back()] = OUTSIDE_BOUNDS_LINK;
774  }
775 
776  if (!outside_bounds.empty())
777  {
779  addStatusText("Links descending from joints that are outside bounds in goal state:");
780  addStatusText(outside_bounds);
781  }
782  }
783  }
785 
786  // update metrics text
787  displayMetrics(false);
788  }
789  }
790  else
791  query_robot_goal_->setVisible(false);
792  context_->queueRender();
793 }
794 
795 void MotionPlanningDisplay::resetInteractiveMarkers()
796 {
797  query_start_state_->clearError();
798  query_goal_state_->clearError();
799  addBackgroundJob([this] { publishInteractiveMarkers(false); }, "publishInteractiveMarkers");
800 }
801 
803 {
804  if (robot_interaction_)
805  {
806  if (pose_update &&
807  robot_interaction_->showingMarkers(query_start_state_) == query_start_state_property_->getBool() &&
808  robot_interaction_->showingMarkers(query_goal_state_) == query_goal_state_property_->getBool())
809  {
810  if (query_start_state_property_->getBool())
811  robot_interaction_->updateInteractiveMarkers(query_start_state_);
812  if (query_goal_state_property_->getBool())
813  robot_interaction_->updateInteractiveMarkers(query_goal_state_);
814  }
815  else
816  {
817  robot_interaction_->clearInteractiveMarkers();
818  if (query_start_state_property_->getBool())
819  robot_interaction_->addInteractiveMarkers(query_start_state_, query_marker_scale_property_->getFloat());
820  if (query_goal_state_property_->getBool())
821  robot_interaction_->addInteractiveMarkers(query_goal_state_, query_marker_scale_property_->getFloat());
822  robot_interaction_->publishInteractiveMarkers();
823  }
824  if (frame_)
825  {
827  }
828  }
829 }
830 
831 void MotionPlanningDisplay::changedQueryStartColor()
832 {
833  std_msgs::msg::ColorRGBA color;
834  QColor qcolor = query_start_color_property_->getColor();
835  color.r = qcolor.redF();
836  color.g = qcolor.greenF();
837  color.b = qcolor.blueF();
838  color.a = 1.0f;
839  query_robot_start_->setDefaultAttachedObjectColor(color);
840  changedQueryStartState();
841 }
842 
843 void MotionPlanningDisplay::changedQueryStartAlpha()
844 {
845  query_robot_start_->setAlpha(query_start_alpha_property_->getFloat());
846  changedQueryStartState();
847 }
848 
849 void MotionPlanningDisplay::changedQueryMarkerScale()
850 {
852  return;
853 
854  if (isEnabled())
855  {
856  // Clear the interactive markers and re-add them:
858  }
859 }
860 
861 void MotionPlanningDisplay::changedQueryGoalColor()
862 {
863  std_msgs::msg::ColorRGBA color;
864  QColor qcolor = query_goal_color_property_->getColor();
865  color.r = qcolor.redF();
866  color.g = qcolor.greenF();
867  color.b = qcolor.blueF();
868  color.a = 1.0f;
869  query_robot_goal_->setDefaultAttachedObjectColor(color);
870  changedQueryGoalState();
871 }
872 
873 void MotionPlanningDisplay::changedQueryGoalAlpha()
874 {
875  query_robot_goal_->setAlpha(query_goal_alpha_property_->getFloat());
876  changedQueryGoalState();
877 }
878 
879 void MotionPlanningDisplay::changedQueryCollidingLinkColor()
880 {
881  changedQueryStartState();
882  changedQueryGoalState();
883 }
884 
885 void MotionPlanningDisplay::changedQueryJointViolationColor()
886 {
887  changedQueryStartState();
888  changedQueryGoalState();
889 }
890 
891 void MotionPlanningDisplay::changedAttachedBodyColor()
892 {
894  // forward color to TrajectoryVisualization
895  const QColor& color = attached_body_color_property_->getColor();
896  trajectory_visual_->setDefaultAttachedObjectColor(color);
897 }
898 
900  bool error_state_changed)
901 {
903  return;
904  addBackgroundJob([this, pose_update = !error_state_changed] { publishInteractiveMarkers(pose_update); },
905  "publishInteractiveMarkers");
907 }
908 
910  bool error_state_changed)
911 {
913  return;
914  addBackgroundJob([this, pose_update = !error_state_changed] { publishInteractiveMarkers(pose_update); },
915  "publishInteractiveMarkers");
917 }
918 
920 {
923  addMainLoopJob([this] { changedQueryStartState(); });
924  context_->queueRender();
925 }
926 
928 {
931  addMainLoopJob([this] { changedQueryGoalState(); });
932  context_->queueRender();
933 }
934 
936 {
937  *previous_state_ = *query_start_state_->getState();
938 }
939 
941 {
942  query_start_state_->setState(start);
944 }
945 
947 {
948  query_goal_state_->setState(goal);
950 }
951 
953 {
954  if (robot_interaction_)
955  {
958  robot_interaction_->getKinematicOptionsMap()->setOptions(
961  }
962 }
963 
966  const double* ik_solution) const
967 {
968  if (frame_->ui_->collision_aware_ik->isChecked() && planning_scene_monitor_)
969  {
970  state->setJointGroupPositions(group, ik_solution);
971  state->update();
972  bool res = !getPlanningSceneRO()->isStateColliding(*state, group->getName());
973  return res;
974  }
975  else
976  return true;
977 }
978 
980 {
981  unsetAllColors(&query_robot_start_->getRobot());
982  unsetAllColors(&query_robot_goal_->getRobot());
983  std::string group = planning_group_property_->getStdString();
984  if (!group.empty())
985  {
988 
989  for (std::map<std::string, LinkDisplayStatus>::const_iterator it = status_links_start_.begin();
990  it != status_links_start_.end(); ++it)
991  if (it->second == COLLISION_LINK)
992  setLinkColor(&query_robot_start_->getRobot(), it->first, query_colliding_link_color_property_->getColor());
993  else
994  setLinkColor(&query_robot_start_->getRobot(), it->first,
996 
997  for (std::map<std::string, LinkDisplayStatus>::const_iterator it = status_links_goal_.begin();
998  it != status_links_goal_.end(); ++it)
999  if (it->second == COLLISION_LINK)
1000  setLinkColor(&query_robot_goal_->getRobot(), it->first, query_colliding_link_color_property_->getColor());
1001  else
1002  setLinkColor(&query_robot_goal_->getRobot(), it->first,
1004  }
1005 }
1006 
1008 {
1009  if (!getRobotModel() || !robot_interaction_)
1010  return;
1011 
1012  if (getRobotModel()->hasJointModelGroup(group))
1013  {
1014  planning_group_property_->setStdString(group);
1015  }
1016  else
1017  RCLCPP_ERROR(LOGGER, "Group [%s] not found in the robot model.", group.c_str());
1018 }
1019 
1020 void MotionPlanningDisplay::changedPlanningGroup()
1021 {
1022  if (!getRobotModel() || !robot_interaction_)
1023  return;
1024 
1025  if (!planning_group_property_->getStdString().empty() &&
1026  !getRobotModel()->hasJointModelGroup(planning_group_property_->getStdString()))
1027  {
1028  planning_group_property_->setStdString("");
1029  return;
1030  }
1031  modified_groups_.insert(planning_group_property_->getStdString());
1032 
1033  robot_interaction_->decideActiveComponents(planning_group_property_->getStdString());
1034 
1037  updateLinkColors();
1038 
1039  if (frame_)
1041  addBackgroundJob([this] { publishInteractiveMarkers(false); }, "publishInteractiveMarkers");
1042 }
1043 
1044 void MotionPlanningDisplay::changedWorkspace()
1045 {
1047 }
1048 
1050 {
1051  return planning_group_property_->getStdString();
1052 }
1053 
1054 void MotionPlanningDisplay::setQueryStateHelper(bool use_start_state, const std::string& state_name)
1055 {
1056  moveit::core::RobotState state = use_start_state ? *getQueryStartState() : *getQueryGoalState();
1057 
1058  std::string v = "<" + state_name + ">";
1059 
1060  if (v == "<random>")
1061  {
1063  state.setToRandomPositions(jmg);
1064  }
1065  else if (v == "<current>")
1066  {
1068  if (ps)
1069  state = ps->getCurrentState();
1070  }
1071  else if (v == "<same as goal>")
1072  {
1073  state = *getQueryGoalState();
1074  }
1075  else if (v == "<same as start>")
1076  {
1077  state = *getQueryStartState();
1078  }
1079  else
1080  {
1081  // maybe it is a named state
1083  state.setToDefaultValues(jmg, state_name);
1084  }
1085 
1086  use_start_state ? setQueryStartState(state) : setQueryGoalState(state);
1087 }
1088 
1089 void MotionPlanningDisplay::populateMenuHandler(std::shared_ptr<interactive_markers::MenuHandler>& mh)
1090 {
1091  typedef interactive_markers::MenuHandler immh;
1092  std::vector<std::string> state_names;
1093  state_names.push_back("random");
1094  state_names.push_back("current");
1095  state_names.push_back("same as start");
1096  state_names.push_back("same as goal");
1097 
1098  // hacky way to distinguish between the start and goal handlers...
1099  bool is_start = (mh.get() == menu_handler_start_.get());
1100 
1101  // Commands for changing the state
1102  immh::EntryHandle menu_states =
1103  mh->insert(is_start ? "Set start state to" : "Set goal state to", immh::FeedbackCallback());
1104  for (const std::string& state_name : state_names)
1105  {
1106  // Don't add "same as start" to the start state handler, and vice versa.
1107  if ((state_name == "same as start" && is_start) || (state_name == "same as goal" && !is_start))
1108  continue;
1109  mh->insert(menu_states, state_name,
1110  [this, is_start, state_name](auto&&) { setQueryStateHelper(is_start, state_name); });
1111  }
1112 
1113  // // Group commands, which end up being the same for both interaction handlers
1114  // const std::vector<std::string>& group_names = getRobotModel()->getJointModelGroupNames();
1115  // immh::EntryHandle menu_groups = mh->insert("Planning Group", immh::FeedbackCallback());
1116  // for (int i = 0; i < group_names.size(); ++i)
1117  // mh->insert(menu_groups, group_names[i],
1118  // [this, &name = group_names[i]] { changePlanningGroup(name); });
1119 }
1120 
1122 {
1123  // Invalidate all references to the RobotModel ...
1124  if (frame_)
1126  if (trajectory_visual_)
1127  trajectory_visual_->clearRobotModel();
1128  previous_state_.reset();
1129  query_start_state_.reset();
1130  query_goal_state_.reset();
1131  kinematics_metrics_.reset();
1132  robot_interaction_.reset();
1133  dynamics_solver_.clear();
1134  // ... before calling the parent's method, which finally destroys the creating RobotModelLoader.
1136 }
1137 
1139 {
1141  trajectory_visual_->onRobotModelLoaded(getRobotModel());
1142 
1143  robot_interaction_ = std::make_shared<robot_interaction::RobotInteraction>(
1144  getRobotModel(), node_, rclcpp::names::append(getMoveGroupNS(), "rviz_moveit_motion_planning_display"));
1146  o.state_validity_callback_ = [this](moveit::core::RobotState* robot_state,
1147  const moveit::core::JointModelGroup* joint_group,
1148  const double* joint_group_variable_values) {
1149  return isIKSolutionCollisionFree(robot_state, joint_group, joint_group_variable_values);
1150  };
1151  robot_interaction_->getKinematicOptionsMap()->setOptions(
1153 
1154  int_marker_display_->subProp("Interactive Markers Namespace")
1155  ->setValue(QString::fromStdString(robot_interaction_->getServerTopic()));
1156  query_robot_start_->load(*getRobotModel()->getURDF());
1157  query_robot_goal_->load(*getRobotModel()->getURDF());
1158 
1159  // initialize previous state, start state, and goal state to current state
1160  previous_state_ = std::make_shared<moveit::core::RobotState>(getPlanningSceneRO()->getCurrentState());
1161  query_start_state_ = std::make_shared<robot_interaction::InteractionHandler>(
1162  robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient());
1163  query_goal_state_ = std::make_shared<robot_interaction::InteractionHandler>(
1164  robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient());
1165  query_start_state_->setUpdateCallback(
1166  [this](robot_interaction::InteractionHandler* handler, bool error_state_changed) {
1167  scheduleDrawQueryStartState(handler, error_state_changed);
1168  });
1169  query_goal_state_->setUpdateCallback([this](robot_interaction::InteractionHandler* handler, bool error_state_changed) {
1170  scheduleDrawQueryGoalState(handler, error_state_changed);
1171  });
1172 
1173  // Interactive marker menus
1176  query_start_state_->setMenuHandler(menu_handler_start_);
1177  query_goal_state_->setMenuHandler(menu_handler_goal_);
1178 
1179  if (!planning_group_property_->getStdString().empty())
1180  if (!getRobotModel()->hasJointModelGroup(planning_group_property_->getStdString()))
1181  planning_group_property_->setStdString("");
1182 
1183  const std::vector<std::string>& groups = getRobotModel()->getJointModelGroupNames();
1184  planning_group_property_->clearOptions();
1185  for (const std::string& group : groups)
1186  planning_group_property_->addOptionStd(group);
1187  planning_group_property_->sortOptions();
1188  if (!groups.empty() && planning_group_property_->getStdString().empty())
1189  planning_group_property_->setStdString(groups[0]);
1190 
1191  modified_groups_.clear();
1192  kinematics_metrics_ = std::make_shared<kinematics_metrics::KinematicsMetrics>(getRobotModel());
1193 
1194  geometry_msgs::msg::Vector3 gravity_vector;
1195  gravity_vector.x = 0.0;
1196  gravity_vector.y = 0.0;
1197  gravity_vector.z = 9.81;
1198 
1199  dynamics_solver_.clear();
1200  for (const std::string& group : groups)
1201  if (getRobotModel()->getJointModelGroup(group)->isChain())
1203  std::make_shared<dynamics_solver::DynamicsSolver>(getRobotModel(), group, gravity_vector);
1204 
1205  if (frame_)
1206  frame_->fillPlanningGroupOptions();
1207  changedPlanningGroup();
1208 }
1210 {
1211  frame_->onNewPlanningSceneState();
1212 }
1213 
1215  const moveit::core::RobotState& src)
1216 {
1217  moveit::core::RobotState src_copy = src;
1218  for (const std::string& modified_group : modified_groups_)
1219  {
1220  const moveit::core::JointModelGroup* jmg = dest.getJointModelGroup(modified_group);
1221  if (jmg)
1222  {
1223  std::vector<double> values_to_keep;
1224  dest.copyJointGroupPositions(jmg, values_to_keep);
1225  src_copy.setJointGroupPositions(jmg, values_to_keep);
1226  }
1227  }
1228 
1229  // overwrite the destination state
1230  dest = src_copy;
1231 }
1232 
1234 {
1235  std::string group = planning_group_property_->getStdString();
1236 
1237  if (query_start_state_ && query_start_state_property_->getBool() && !group.empty())
1238  {
1240  updateStateExceptModified(start, current_state);
1241  setQueryStartState(start);
1242  }
1243 
1244  if (query_goal_state_ && query_goal_state_property_->getBool() && !group.empty())
1245  {
1247  updateStateExceptModified(goal, current_state);
1248  setQueryGoalState(goal);
1249  }
1250 }
1251 
1254 {
1256  updateQueryStates(getPlanningSceneRO()->getCurrentState());
1257  if (frame_)
1258  frame_->sceneUpdate(update_type);
1259 }
1260 
1261 // ******************************************************************************************
1262 // Enable
1263 // ******************************************************************************************
1265 {
1267 
1268  // Planned Path Display
1269  trajectory_visual_->onEnable();
1270 
1271  text_to_display_->setVisible(false);
1272 
1273  query_robot_start_->setVisible(query_start_state_property_->getBool());
1274  query_robot_goal_->setVisible(query_goal_state_property_->getBool());
1275 
1276  int_marker_display_->setEnabled(true);
1277  int_marker_display_->setFixedFrame(fixed_frame_);
1278 
1279  frame_->enable();
1280 }
1281 
1282 // ******************************************************************************************
1283 // Disable
1284 // ******************************************************************************************
1286 {
1287  if (robot_interaction_)
1288  robot_interaction_->clear();
1289  int_marker_display_->setEnabled(false);
1290 
1291  query_robot_start_->setVisible(false);
1292  query_robot_goal_->setVisible(false);
1293  text_to_display_->setVisible(false);
1294 
1296 
1297  // Planned Path Display
1298  trajectory_visual_->onDisable();
1299 
1300  frame_->disable();
1301 }
1302 
1303 // ******************************************************************************************
1304 // Update
1305 // ******************************************************************************************
1306 void MotionPlanningDisplay::update(float wall_dt, float ros_dt)
1307 {
1308  if (int_marker_display_)
1309  int_marker_display_->update(wall_dt, ros_dt);
1310  if (frame_)
1311  frame_->updateSceneMarkers(wall_dt, ros_dt);
1312 
1313  PlanningSceneDisplay::update(wall_dt, ros_dt);
1314 }
1315 
1316 void MotionPlanningDisplay::updateInternal(float wall_dt, float ros_dt)
1317 {
1318  PlanningSceneDisplay::updateInternal(wall_dt, ros_dt);
1319 
1320  // Planned Path Display
1321  trajectory_visual_->update(wall_dt, ros_dt);
1322 
1324 }
1325 
1326 void MotionPlanningDisplay::load(const rviz_common::Config& config)
1327 {
1329  if (frame_)
1330  {
1331  float d;
1332  if (config.mapGetFloat("MoveIt_Planning_Time", &d))
1333  frame_->ui_->planning_time->setValue(d);
1334  int attempts;
1335  if (config.mapGetInt("MoveIt_Planning_Attempts", &attempts))
1336  frame_->ui_->planning_attempts->setValue(attempts);
1337  if (config.mapGetFloat("Velocity_Scaling_Factor", &d))
1338  frame_->ui_->velocity_scaling_factor->setValue(d);
1339  if (config.mapGetFloat("Acceleration_Scaling_Factor", &d))
1340  frame_->ui_->acceleration_scaling_factor->setValue(d);
1341 
1342  bool b;
1343  if (config.mapGetBool("MoveIt_Allow_Replanning", &b))
1344  frame_->ui_->allow_replanning->setChecked(b);
1345  if (config.mapGetBool("MoveIt_Allow_Sensor_Positioning", &b))
1346  frame_->ui_->allow_looking->setChecked(b);
1347  if (config.mapGetBool("MoveIt_Allow_External_Program", &b))
1348  frame_->ui_->allow_external_program->setChecked(b);
1349  if (config.mapGetBool("MoveIt_Use_Cartesian_Path", &b))
1350  frame_->ui_->use_cartesian_path->setChecked(b);
1351  if (config.mapGetBool("MoveIt_Use_Constraint_Aware_IK", &b))
1352  frame_->ui_->collision_aware_ik->setChecked(b);
1353  if (config.mapGetBool("MoveIt_Allow_Approximate_IK", &b))
1354  frame_->ui_->approximate_ik->setChecked(b);
1355 
1356  rviz_common::Config workspace = config.mapGetChild("MoveIt_Workspace");
1357  rviz_common::Config ws_center = workspace.mapGetChild("Center");
1358  float val;
1359  if (ws_center.mapGetFloat("X", &val))
1360  frame_->ui_->wcenter_x->setValue(val);
1361  if (ws_center.mapGetFloat("Y", &val))
1362  frame_->ui_->wcenter_y->setValue(val);
1363  if (ws_center.mapGetFloat("Z", &val))
1364  frame_->ui_->wcenter_z->setValue(val);
1365 
1366  rviz_common::Config ws_size = workspace.mapGetChild("Size");
1367  if (ws_size.isValid())
1368  {
1369  if (ws_size.mapGetFloat("X", &val))
1370  frame_->ui_->wsize_x->setValue(val);
1371  if (ws_size.mapGetFloat("Y", &val))
1372  frame_->ui_->wsize_y->setValue(val);
1373  if (ws_size.mapGetFloat("Z", &val))
1374  frame_->ui_->wsize_z->setValue(val);
1375  }
1376  else
1377  {
1378  double val;
1379  if (node_->get_parameter("default_workspace_bounds", val))
1380  {
1381  frame_->ui_->wsize_x->setValue(val);
1382  frame_->ui_->wsize_y->setValue(val);
1383  frame_->ui_->wsize_z->setValue(val);
1384  }
1385  }
1386  }
1387 }
1388 
1389 void MotionPlanningDisplay::save(rviz_common::Config config) const
1390 {
1392  if (frame_)
1393  {
1394  // "Options" Section
1395  config.mapSetValue("MoveIt_Planning_Time", frame_->ui_->planning_time->value());
1396  config.mapSetValue("MoveIt_Planning_Attempts", frame_->ui_->planning_attempts->value());
1397  config.mapSetValue("Velocity_Scaling_Factor", frame_->ui_->velocity_scaling_factor->value());
1398  config.mapSetValue("Acceleration_Scaling_Factor", frame_->ui_->acceleration_scaling_factor->value());
1399 
1400  config.mapSetValue("MoveIt_Allow_Replanning", frame_->ui_->allow_replanning->isChecked());
1401  config.mapSetValue("MoveIt_Allow_Sensor_Positioning", frame_->ui_->allow_looking->isChecked());
1402  config.mapSetValue("MoveIt_Allow_External_Program", frame_->ui_->allow_external_program->isChecked());
1403  config.mapSetValue("MoveIt_Use_Cartesian_Path", frame_->ui_->use_cartesian_path->isChecked());
1404  config.mapSetValue("MoveIt_Use_Constraint_Aware_IK", frame_->ui_->collision_aware_ik->isChecked());
1405  config.mapSetValue("MoveIt_Allow_Approximate_IK", frame_->ui_->approximate_ik->isChecked());
1406 
1407  rviz_common::Config workspace = config.mapMakeChild("MoveIt_Workspace");
1408  rviz_common::Config ws_center = workspace.mapMakeChild("Center");
1409  ws_center.mapSetValue("X", frame_->ui_->wcenter_x->value());
1410  ws_center.mapSetValue("Y", frame_->ui_->wcenter_y->value());
1411  ws_center.mapSetValue("Z", frame_->ui_->wcenter_z->value());
1412  rviz_common::Config ws_size = workspace.mapMakeChild("Size");
1413  ws_size.mapSetValue("X", frame_->ui_->wsize_x->value());
1414  ws_size.mapSetValue("Y", frame_->ui_->wsize_y->value());
1415  ws_size.mapSetValue("Z", frame_->ui_->wsize_z->value());
1416  }
1417 }
1418 
1420 {
1422  if (int_marker_display_)
1423  int_marker_display_->setFixedFrame(fixed_frame_);
1424  // When the fixed frame changes we need to tell RViz to update the rendered interactive marker display
1425  if (frame_ && frame_->scene_marker_)
1426  frame_->scene_marker_->requestPoseUpdate(frame_->scene_marker_->getPosition(),
1427  frame_->scene_marker_->getOrientation());
1428  changedPlanningGroup();
1429 }
1430 
1431 // Pick and place
1433 {
1434  for (std::shared_ptr<rviz_rendering::Shape>& place_location_shape : place_locations_display_)
1435  place_location_shape.reset();
1436  place_locations_display_.clear();
1437 }
1438 
1439 void MotionPlanningDisplay::visualizePlaceLocations(const std::vector<geometry_msgs::msg::PoseStamped>& place_poses)
1440 {
1442  place_locations_display_.resize(place_poses.size());
1443  for (std::size_t i = 0; i < place_poses.size(); ++i)
1444  {
1446  std::make_shared<rviz_rendering::Shape>(rviz_rendering::Shape::Sphere, context_->getSceneManager());
1447  place_locations_display_[i]->setColor(1.0f, 0.0f, 0.0f, 0.3f);
1448  Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y, place_poses[i].pose.position.z);
1449  Ogre::Vector3 extents(0.02, 0.02, 0.02);
1450  place_locations_display_[i]->setScale(extents);
1451  place_locations_display_[i]->setPosition(center);
1452  }
1453 }
1454 
1455 } // namespace moveit_rviz_plugin
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:72
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:605
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:691
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:134
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
void update(bool force=false)
Update all transforms.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void clearJobUpdateEvent()
Clear the callback to be triggered when events in JobEvent take place.
void setJobUpdateEvent(const JobUpdateCallback &event)
Set the callback to be triggered when events in JobEvent take place.
std::size_t getJobCount() const
Get the size of the queue of jobs (includes currently processed job).
void populateMenuHandler(std::shared_ptr< interactive_markers::MenuHandler > &mh)
RobotStateVisualizationPtr query_robot_start_
Handles drawing the robot at the start configuration.
void scheduleDrawQueryGoalState(robot_interaction::InteractionHandler *handler, bool error_state_changed)
kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_
rviz_common::properties::FloatProperty * metrics_text_height_property_
void updateInternal(float wall_dt, float ros_dt) override
void onNewPlanningSceneState() override
This is called upon successful retrieval of the (initial) planning scene state.
void save(rviz_common::Config config) const override
moveit::core::RobotStatePtr previous_state_
remember previous start state (updated before starting execution)
void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string &jobname)
rviz_common::properties::BoolProperty * show_joint_torques_property_
void displayTable(const std::map< std::string, double > &values, const Ogre::ColourValue &color, const Ogre::Vector3 &pos, const Ogre::Quaternion &orient)
rviz_common::properties::FloatProperty * query_marker_scale_property_
void updateStateExceptModified(moveit::core::RobotState &dest, const moveit::core::RobotState &src)
bool text_display_for_start_
indicates whether the text display is for the start state or not
robot_interaction::RobotInteractionPtr robot_interaction_
rviz_common::properties::BoolProperty * show_manipulability_property_
void setQueryGoalState(const moveit::core::RobotState &goal)
robot_interaction::InteractionHandlerPtr query_start_state_
std::map< std::string, LinkDisplayStatus > status_links_goal_
void setQueryStartState(const moveit::core::RobotState &start)
rviz_common::properties::ColorProperty * query_outside_joint_limits_link_color_property_
rviz_common::properties::FloatProperty * query_goal_alpha_property_
rviz_common::properties::ColorProperty * query_start_color_property_
void computeMetricsInternal(std::map< std::string, double > &metrics, const robot_interaction::EndEffectorInteraction &eef, const moveit::core::RobotState &state, double payload)
void computeMetrics(bool start, const std::string &group, double payload)
rviz_common::properties::Property * path_category_
rviz_common::properties::FloatProperty * metrics_set_payload_property_
void setQueryStateHelper(bool use_start_state, const std::string &v)
std::map< std::string, bool > position_only_ik_
Some groups use position only ik, calls to the metrics have to be modified appropriately.
rviz_common::properties::ColorProperty * query_goal_color_property_
rviz_common::properties::BoolProperty * compute_weight_limit_property_
std::unique_ptr< rviz_rendering::Shape > workspace_box_
std::map< std::pair< bool, std::string >, std::map< std::string, double > > computed_metrics_
std::map< std::string, LinkDisplayStatus > status_links_start_
Ogre::SceneNode * text_display_scene_node_
displays texts
void updateQueryStates(const moveit::core::RobotState &current_state)
rviz_common::properties::EditableEnumProperty * planning_group_property_
rviz_common::properties::BoolProperty * show_manipulability_index_property_
rviz_common::properties::Property * metrics_category_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_start_
void scheduleDrawQueryStartState(robot_interaction::InteractionHandler *handler, bool error_state_changed)
std::map< std::string, dynamics_solver::DynamicsSolverPtr > dynamics_solver_
moveit::core::RobotStateConstPtr getQueryStartState() const
rviz_common::properties::BoolProperty * show_workspace_property_
rviz_common::properties::BoolProperty * query_goal_state_property_
void load(const rviz_common::Config &config) override
bool isIKSolutionCollisionFree(moveit::core::RobotState *state, const moveit::core::JointModelGroup *group, const double *ik_solution) const
rviz_common::properties::FloatProperty * query_start_alpha_property_
std::vector< std::shared_ptr< rviz_rendering::Shape > > place_locations_display_
RobotStateVisualizationPtr query_robot_goal_
Handles drawing the robot at the goal configuration.
void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override
void update(float wall_dt, float ros_dt) override
rviz_common::properties::BoolProperty * query_start_state_property_
rviz_common::properties::Property * plan_category_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_goal_
rclcpp::Subscription< std_msgs::msg::String >::SharedPtr planning_group_sub_
void selectPlanningGroupCallback(const std_msgs::msg::String::ConstSharedPtr &msg)
void onRobotModelLoaded() override
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
rviz_common::properties::ColorProperty * query_colliding_link_color_property_
void visualizePlaceLocations(const std::vector< geometry_msgs::msg::PoseStamped > &place_poses)
moveit::core::RobotStateConstPtr getQueryGoalState() const
robot_interaction::InteractionHandlerPtr query_goal_state_
std::shared_ptr< rviz_default_plugins::displays::InteractiveMarker > scene_marker_
void updateSceneMarkers(float wall_dt, float ros_dt)
void sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
rviz_common::properties::ColorProperty * attached_body_color_property_
void setLinkColor(const std::string &link_name, const QColor &color)
void save(rviz_common::Config config) const override
virtual void onRobotModelLoaded()
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
void setGroupColor(rviz_default_plugins::robot::Robot *robot, const std::string &group_name, const QColor &color)
virtual void updateInternal(float wall_dt, float ros_dt)
virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
void addMainLoopJob(const std::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
const moveit::core::RobotModelConstPtr & getRobotModel() const
void addBackgroundJob(const std::function< void()> &job, const std::string &name)
void update(float wall_dt, float ros_dt) override
void unsetAllColors(rviz_default_plugins::robot::Robot *robot)
Ogre::SceneNode * planning_scene_node_
displays planning scene with everything in it
moveit::tools::BackgroundProcessing background_process_
void load(const rviz_common::Config &config) override
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
static const std::string DEFAULT
When used as key this means the default value.
static const std::string ALL
When used as key this means set ALL keys (including default)
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
p
Definition: pick.py:62
std::string append(const std::string &left, const std::string &right)
d
Definition: setup.py:4
name
Definition: setup.py:7
const rclcpp::Logger LOGGER
Definition: async_test.h:31
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
A map returning the pairs of body ids in contact, plus their contact details.
std::string parent_group
The name of the group that sustains the end-effector (usually an arm)
Definition: interaction.h:142
kinematics::KinematicsQueryOptions options_
other options
moveit::core::GroupStateValidityCallbackFn state_validity_callback_
This is called to determine if the state is valid.