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 
41 #include <rviz/robot/robot.h>
42 #include <rviz/robot/robot_link.h>
43 
51 #include <rviz/display_context.h>
52 #include <rviz/frame_manager.h>
53 #include <rviz/panel_dock_widget.h>
55 #include <rviz/display_factory.h>
57 
58 #include <OgreSceneManager.h>
59 #include <OgreSceneNode.h>
61 
62 #include <tf/transform_listener.h>
63 
66 
67 #include <boost/format.hpp>
68 #include <boost/algorithm/string/replace.hpp>
69 #include <boost/algorithm/string/trim.hpp>
70 #include <boost/lexical_cast.hpp>
71 
72 #include <QShortcut>
73 
74 #include "ui_motion_planning_rviz_plugin_frame.h"
75 
76 namespace moveit_rviz_plugin
77 {
78 // ******************************************************************************************
79 // Base class contructor
80 // ******************************************************************************************
83  , text_to_display_(NULL)
84  , private_handle_("~")
85  , frame_(NULL)
86  , frame_dock_(NULL)
87  , menu_handler_start_(new interactive_markers::MenuHandler)
88  , menu_handler_goal_(new interactive_markers::MenuHandler)
89  , int_marker_display_(NULL)
90 {
91  // Category Groups
92  plan_category_ = new rviz::Property("Planning Request", QVariant(), "", this);
93  metrics_category_ = new rviz::Property("Planning Metrics", QVariant(), "", this);
94  path_category_ = new rviz::Property("Planned Path", QVariant(), "", this);
95 
96  // Metrics category -----------------------------------------------------------------------------------------
98  "Show Weight Limit", false, "Shows the weight limit at a particular pose for an end-effector", metrics_category_,
99  SLOT(changedShowWeightLimit()), this);
100 
102  new rviz::BoolProperty("Show Manipulability Index", false, "Shows the manipulability index for an end-effector",
104 
106  new rviz::BoolProperty("Show Manipulability", false, "Shows the manipulability for an end-effector",
108 
109  show_joint_torques_property_ = new rviz::BoolProperty("Show Joint Torques", false,
110  "Shows the joint torques for a given configuration and payload",
112 
114  new rviz::FloatProperty("Payload", 1.0f, "Specify the payload at the end effector (kg)", metrics_category_,
115  SLOT(changedMetricsSetPayload()), this);
117 
118  metrics_text_height_property_ = new rviz::FloatProperty("TextHeight", 0.08f, "Text height", metrics_category_,
119  SLOT(changedMetricsTextHeight()), this);
121 
122  // Planning request category -----------------------------------------------------------------------------------------
123 
125  "Planning Group", "", "The name of the group of links to plan for (from the ones defined in the SRDF)",
126  plan_category_, SLOT(changedPlanningGroup()), this);
127  show_workspace_property_ = new rviz::BoolProperty("Show Workspace", false, "Shows the axis-aligned bounding box for "
128  "the workspace allowed for planning",
129  plan_category_, SLOT(changedWorkspace()), this);
131  new rviz::BoolProperty("Query Start State", false, "Set a custom start state for the motion planning query",
132  plan_category_, SLOT(changedQueryStartState()), this);
134  new rviz::BoolProperty("Query Goal State", true, "Shows the goal state for the motion planning query",
135  plan_category_, SLOT(changedQueryGoalState()), this);
137  new rviz::FloatProperty("Interactive Marker Size", 0.0f,
138  "Specifies scale of the interactive marker overlayed on the robot. 0 is auto scale.",
139  plan_category_, SLOT(changedQueryMarkerScale()), this);
141 
143  new rviz::ColorProperty("Start State Color", QColor(0, 255, 0), "The highlight color for the start state",
144  plan_category_, SLOT(changedQueryStartColor()), this);
146  new rviz::FloatProperty("Start State Alpha", 1.0f, "Specifies the alpha for the robot links", plan_category_,
147  SLOT(changedQueryStartAlpha()), this);
150 
152  new rviz::ColorProperty("Goal State Color", QColor(250, 128, 0), "The highlight color for the goal state",
153  plan_category_, SLOT(changedQueryGoalColor()), this);
154 
156  new rviz::FloatProperty("Goal State Alpha", 1.0f, "Specifies the alpha for the robot links", plan_category_,
157  SLOT(changedQueryGoalAlpha()), this);
160 
162  new rviz::ColorProperty("Colliding Link Color", QColor(255, 0, 0), "The highlight color for colliding links",
164 
166  new rviz::ColorProperty("Joint Violation Color", QColor(255, 0, 255),
167  "The highlight color for child links of joints that are outside bounds", plan_category_,
168  SLOT(changedQueryJointViolationColor()), this);
169 
170  // Trajectory playback / planned path category ---------------------------------------------
172 
173  // Start background jobs
175 }
176 
177 // ******************************************************************************************
178 // Deconstructor
179 // ******************************************************************************************
181 {
183  clearJobs();
184 
185  query_robot_start_.reset();
186  query_robot_goal_.reset();
187 
188  delete text_to_display_;
189  delete int_marker_display_;
190  if (frame_dock_)
191  delete frame_dock_;
192 }
193 
195 {
197 
198  // Planned Path Display
200  QColor qcolor = attached_body_color_property_->getColor();
201  trajectory_visual_->setDefaultAttachedObjectColor(qcolor);
202 
203  query_robot_start_.reset(new RobotStateVisualization(planning_scene_node_, context_, "Planning Request Start", NULL));
204  query_robot_start_->setCollisionVisible(false);
205  query_robot_start_->setVisualVisible(true);
207  std_msgs::ColorRGBA color;
209  color.r = qcolor.redF();
210  color.g = qcolor.greenF();
211  color.b = qcolor.blueF();
212  color.a = 1.0f;
213  query_robot_start_->setDefaultAttachedObjectColor(color);
214 
215  query_robot_goal_.reset(new RobotStateVisualization(planning_scene_node_, context_, "Planning Request Goal", NULL));
216  query_robot_goal_->setCollisionVisible(false);
217  query_robot_goal_->setVisualVisible(true);
220  color.r = qcolor.redF();
221  color.g = qcolor.greenF();
222  color.b = qcolor.blueF();
223  query_robot_goal_->setDefaultAttachedObjectColor(color);
224 
226  frame_ = new MotionPlanningFrame(this, context_, window_context ? window_context->getParentWindow() : NULL);
228  addStatusText("Initialized.");
229 
230  // immediately switch to next trajectory display after planning
231  connect(frame_, SIGNAL(planningFinished()), trajectory_visual_.get(), SLOT(interruptCurrentDisplay()));
232 
233  if (window_context)
234  {
235  frame_dock_ = window_context->addPane(getName(), frame_);
236  connect(frame_dock_, SIGNAL(visibilityChanged(bool)), this, SLOT(motionPanelVisibilityChange(bool)));
238  }
239 
240  int_marker_display_ = context_->getDisplayFactory()->make("rviz/InteractiveMarkers");
242 
243  text_display_scene_node_ = planning_scene_node_->createChildSceneNode();
244  text_to_display_ = new rviz::MovableText("EMPTY");
248  text_to_display_->setVisible(false);
249  text_display_for_start_ = false;
251 
253  {
254  QShortcut* im_reset_shortcut =
255  new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_R), context_->getWindowManager()->getParentWindow());
256  connect(im_reset_shortcut, SIGNAL(activated()), this, SLOT(resetInteractiveMarkers()));
257  }
258 }
259 
261 {
262  if (enable)
263  setEnabled(true);
264 }
265 
267 {
268  if (enable)
269  {
270  planning_group_sub_ = node_handle_.subscribe("/rviz/moveit/select_planning_group", 1,
272  }
273  else
274  {
276  }
277 }
278 
279 void MotionPlanningDisplay::selectPlanningGroupCallback(const std_msgs::StringConstPtr& msg)
280 {
282  return;
283  if (getRobotModel()->hasJointModelGroup(msg->data))
284  {
287  }
288  else
289  {
290  ROS_ERROR("Group [%s] not found in the robot model.", msg->data.c_str());
291  }
292 }
294 {
295  text_to_display_->setVisible(false);
296 
297  query_robot_start_->clear();
298  query_robot_goal_->clear();
299 
301 
302  // Planned Path Display
303  trajectory_visual_->reset();
304 
305  frame_->disable();
306  frame_->enable();
307 
310 }
311 
312 void MotionPlanningDisplay::setName(const QString& name)
313 {
314  BoolProperty::setName(name);
315  if (frame_dock_)
316  {
318  frame_dock_->setObjectName(name);
319  }
320  trajectory_visual_->setName(name);
321 }
322 
324 {
326 }
327 
329 {
330  if (!frame_)
331  return;
332  QProgressBar* p = frame_->ui_->background_job_progress;
334 
335  if (n == 0)
336  {
337  p->setValue(p->maximum());
338  p->update();
339  p->hide();
340  p->setMaximum(0);
341  }
342  else
343  {
344  if (n == 1)
345  {
346  if (p->maximum() == 0)
347  p->setValue(0);
348  else
349  p->setValue(p->maximum() - 1);
350  }
351  else
352  {
353  if (p->maximum() < n)
354  p->setMaximum(n);
355  else
356  p->setValue(p->maximum() - n);
357  }
358  p->show();
359  p->update();
360  }
361 }
362 
364 {
366  {
368  displayMetrics(true);
369  }
370  else
371  {
373  displayMetrics(false);
374  }
375 }
376 
378 {
380  {
382  displayMetrics(true);
383  }
384  else
385  {
387  displayMetrics(false);
388  }
389 }
390 
392 {
394  {
396  displayMetrics(true);
397  }
398  else
399  {
401  displayMetrics(false);
402  }
403 }
404 
406 {
408  {
410  displayMetrics(true);
411  }
412  else
413  {
415  displayMetrics(false);
416  }
417 }
418 
420 {
422  {
424  displayMetrics(true);
425  }
426  else
427  {
429  displayMetrics(false);
430  }
431 }
432 
434 {
436 }
437 
438 void MotionPlanningDisplay::displayTable(const std::map<std::string, double>& values, const Ogre::ColourValue& color,
439  const Ogre::Vector3& pos, const Ogre::Quaternion& orient)
440 {
441  // the line we want to render
442  std::stringstream ss;
443  for (std::map<std::string, double>::const_iterator it = values.begin(); it != values.end(); ++it)
444  ss << boost::format("%-10s %-4.2f") % it->first % it->second << std::endl;
445 
446  if (ss.str().empty())
447  {
448  text_to_display_->setVisible(false);
449  return;
450  }
451 
452  text_to_display_->setCaption(ss.str());
453  text_to_display_->setColor(color);
454  text_display_scene_node_->setPosition(pos);
455  text_display_scene_node_->setOrientation(orient);
456 
457  // make sure the node is visible
458  text_to_display_->setVisible(true);
459 }
460 
462 {
464  {
465  if (workspace_box_)
466  workspace_box_.reset();
467  return;
468  }
469 
470  if (!workspace_box_)
471  {
473  workspace_box_->setColor(0.0f, 0.0f, 0.6f, 0.3f);
474  }
475 
476  Ogre::Vector3 center(frame_->ui_->wcenter_x->value(), frame_->ui_->wcenter_y->value(),
477  frame_->ui_->wcenter_z->value());
478  Ogre::Vector3 extents(frame_->ui_->wsize_x->value(), frame_->ui_->wsize_y->value(), frame_->ui_->wsize_z->value());
479  workspace_box_->setScale(extents);
480  workspace_box_->setPosition(center);
481 }
482 
483 void MotionPlanningDisplay::computeMetrics(bool start, const std::string& group, double payload)
484 {
485  if (!robot_interaction_)
486  return;
487  const std::vector<robot_interaction::RobotInteraction::EndEffector>& eef =
488  robot_interaction_->getActiveEndEffectors();
489  if (eef.empty())
490  return;
491  boost::mutex::scoped_lock slock(update_metrics_lock_);
492 
493  robot_state::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState();
494  for (std::size_t i = 0; i < eef.size(); ++i)
495  if (eef[i].parent_group == group)
496  computeMetricsInternal(computed_metrics_[std::make_pair(start, group)], eef[i], *state, payload);
497 }
498 
499 void MotionPlanningDisplay::computeMetricsInternal(std::map<std::string, double>& metrics,
501  const robot_state::RobotState& state, double payload)
502 {
503  metrics.clear();
504  dynamics_solver::DynamicsSolverPtr ds;
505  std::map<std::string, dynamics_solver::DynamicsSolverPtr>::const_iterator it = dynamics_solver_.find(ee.parent_group);
506  if (it != dynamics_solver_.end())
507  ds = it->second;
508 
509  // Max payload
510  if (ds)
511  {
512  double max_payload;
513  unsigned int saturated_joint;
514  std::vector<double> joint_values;
515  state.copyJointGroupPositions(ee.parent_group, joint_values);
516  if (ds->getMaxPayload(joint_values, max_payload, saturated_joint))
517  {
518  metrics["max_payload"] = max_payload;
519  metrics["saturated_joint"] = saturated_joint;
520  }
521  std::vector<double> joint_torques;
522  joint_torques.resize(joint_values.size());
523  if (ds->getPayloadTorques(joint_values, payload, joint_torques))
524  {
525  for (std::size_t i = 0; i < joint_torques.size(); ++i)
526  {
527  std::stringstream stream;
528  stream << "torque[" << i << "]";
529  metrics[stream.str()] = joint_torques[i];
530  }
531  }
532  }
533 
535  {
536  if (position_only_ik_.find(ee.parent_group) == position_only_ik_.end())
537  private_handle_.param(ee.parent_group + "/position_only_ik", position_only_ik_[ee.parent_group], false);
538 
539  double manipulability_index, manipulability;
540  bool position_ik = position_only_ik_[ee.parent_group];
541  if (kinematics_metrics_->getManipulabilityIndex(state, ee.parent_group, manipulability_index, position_ik))
542  metrics["manipulability_index"] = manipulability_index;
543  if (kinematics_metrics_->getManipulability(state, ee.parent_group, manipulability))
544  metrics["manipulability"] = manipulability;
545  }
546 }
547 
548 namespace
549 {
550 inline void copyItemIfExists(const std::map<std::string, double>& source, std::map<std::string, double>& dest,
551  const std::string& key)
552 {
553  std::map<std::string, double>::const_iterator it = source.find(key);
554  if (it != source.end())
555  dest[key] = it->second;
556 }
557 }
558 
560 {
562  return;
563 
564  static const Ogre::Quaternion orientation(1.0, 0.0, 0.0, 0.0);
565  const std::vector<robot_interaction::RobotInteraction::EndEffector>& eef =
566  robot_interaction_->getActiveEndEffectors();
567  if (eef.empty())
568  return;
569 
570  robot_state::RobotStateConstPtr state = start ? getQueryStartState() : getQueryGoalState();
571 
572  for (std::size_t i = 0; i < eef.size(); ++i)
573  {
574  Ogre::Vector3 position(0.0, 0.0, 0.0);
575  std::map<std::string, double> text_table;
576  const std::map<std::string, double>& metrics_table = computed_metrics_[std::make_pair(start, eef[i].parent_group)];
578  {
579  copyItemIfExists(metrics_table, text_table, "max_payload");
580  copyItemIfExists(metrics_table, text_table, "saturated_joint");
581  }
583  copyItemIfExists(metrics_table, text_table, "manipulability_index");
585  copyItemIfExists(metrics_table, text_table, "manipulability");
587  {
588  std::size_t nj = getRobotModel()->getJointModelGroup(eef[i].parent_group)->getJointModelNames().size();
589  for (size_t j = 0; j < nj; ++j)
590  {
591  std::stringstream stream;
592  stream << "torque[" << j << "]";
593  copyItemIfExists(metrics_table, text_table, stream.str());
594  }
595  }
596 
597  const robot_state::LinkModel* lm = NULL;
598  const robot_model::JointModelGroup* jmg = getRobotModel()->getJointModelGroup(eef[i].parent_group);
599  if (jmg)
600  if (!jmg->getLinkModelNames().empty())
601  lm = state->getLinkModel(jmg->getLinkModelNames().back());
602  if (lm)
603  {
604  const Eigen::Vector3d& t = state->getGlobalLinkTransform(lm).translation();
605  position[0] = t.x();
606  position[1] = t.y();
607  position[2] = t.z() + 0.2; // \todo this should be a param
608  }
609  if (start)
610  displayTable(text_table, query_start_color_property_->getOgreColor(), position, orientation);
611  else
612  displayTable(text_table, query_goal_color_property_->getOgreColor(), position, orientation);
613  text_display_for_start_ = start;
614  }
615 }
616 
618 {
620  return;
621 
623  {
624  if (isEnabled())
625  {
626  robot_state::RobotStateConstPtr state = getQueryStartState();
627 
628  // update link poses
629  query_robot_start_->update(state);
630  query_robot_start_->setVisible(true);
631 
632  // update link colors
633  std::vector<std::string> collision_links;
634  getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
635  status_links_start_.clear();
636  for (std::size_t i = 0; i < collision_links.size(); ++i)
637  status_links_start_[collision_links[i]] = COLLISION_LINK;
638  if (!collision_links.empty())
639  {
641  getPlanningSceneRO()->getCollidingPairs(pairs, *state);
643  addStatusText("Start state colliding links:");
644  for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin(); it != pairs.end();
645  ++it)
646  addStatusText(it->first.first + " - " + it->first.second);
647  addStatusText(".");
648  }
649  if (!getCurrentPlanningGroup().empty())
650  {
651  const robot_model::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup());
652  if (jmg)
653  {
654  std::vector<std::string> outside_bounds;
655  const std::vector<const robot_model::JointModel*>& jmodels = jmg->getActiveJointModels();
656  for (std::size_t i = 0; i < jmodels.size(); ++i)
657  if (!state->satisfiesBounds(jmodels[i], jmodels[i]->getMaximumExtent() * 1e-2))
658  {
659  outside_bounds.push_back(jmodels[i]->getChildLinkModel()->getName());
660  status_links_start_[outside_bounds.back()] = OUTSIDE_BOUNDS_LINK;
661  }
662  if (!outside_bounds.empty())
663  {
665  addStatusText("Links descending from joints that are outside bounds in start state:");
666  addStatusText(outside_bounds);
667  }
668  }
669  }
671  // update metrics text
672  displayMetrics(true);
673  }
674  }
675  else
676  query_robot_start_->setVisible(false);
678 }
679 
681 {
682  setStatusTextColor(Qt::darkGray);
683 }
684 
686 {
687  if (frame_)
688  frame_->ui_->status_text->setTextColor(color);
689 }
690 
691 void MotionPlanningDisplay::addStatusText(const std::string& text)
692 {
693  if (frame_)
694  frame_->ui_->status_text->append(QString::fromStdString(text));
695 }
696 
697 void MotionPlanningDisplay::addStatusText(const std::vector<std::string>& text)
698 {
699  for (std::size_t i = 0; i < text.size(); ++i)
700  addStatusText(text[i]);
701 }
702 
704 {
706  if (!group.empty())
708 }
709 
711 {
713  if (!group.empty())
715 }
716 
718 {
720  return;
722  addStatusText("Changed start state");
725  "publishInteractiveMarkers");
726 }
727 
729 {
731  return;
733  addStatusText("Changed goal state");
736  "publishInteractiveMarkers");
737 }
738 
740 {
742  return;
744  {
745  if (isEnabled())
746  {
747  robot_state::RobotStateConstPtr state = getQueryGoalState();
748 
749  // update link poses
750  query_robot_goal_->update(state);
751  query_robot_goal_->setVisible(true);
752 
753  // update link colors
754  std::vector<std::string> collision_links;
755  getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
756  status_links_goal_.clear();
757  for (std::size_t i = 0; i < collision_links.size(); ++i)
758  status_links_goal_[collision_links[i]] = COLLISION_LINK;
759  if (!collision_links.empty())
760  {
762  getPlanningSceneRO()->getCollidingPairs(pairs, *state);
764  addStatusText("Goal state colliding links:");
765  for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin(); it != pairs.end();
766  ++it)
767  addStatusText(it->first.first + " - " + it->first.second);
768  addStatusText(".");
769  }
770 
771  if (!getCurrentPlanningGroup().empty())
772  {
773  const robot_model::JointModelGroup* jmg = state->getJointModelGroup(getCurrentPlanningGroup());
774  if (jmg)
775  {
776  const std::vector<const robot_state::JointModel*>& jmodels = jmg->getActiveJointModels();
777  std::vector<std::string> outside_bounds;
778  for (std::size_t i = 0; i < jmodels.size(); ++i)
779  if (!state->satisfiesBounds(jmodels[i], jmodels[i]->getMaximumExtent() * 1e-2))
780  {
781  outside_bounds.push_back(jmodels[i]->getChildLinkModel()->getName());
782  status_links_goal_[outside_bounds.back()] = OUTSIDE_BOUNDS_LINK;
783  }
784 
785  if (!outside_bounds.empty())
786  {
788  addStatusText("Links descending from joints that are outside bounds in goal state:");
789  addStatusText(outside_bounds);
790  }
791  }
792  }
794 
795  // update metrics text
796  displayMetrics(false);
797  }
798  }
799  else
800  query_robot_goal_->setVisible(false);
802 }
803 
805 {
806  query_start_state_->clearError();
807  query_goal_state_->clearError();
809  "publishInteractiveMarkers");
810 }
811 
813 {
814  if (robot_interaction_)
815  {
816  if (pose_update &&
819  {
821  robot_interaction_->updateInteractiveMarkers(query_start_state_);
823  robot_interaction_->updateInteractiveMarkers(query_goal_state_);
824  }
825  else
826  {
827  robot_interaction_->clearInteractiveMarkers();
832  robot_interaction_->publishInteractiveMarkers();
833  }
834  if (frame_)
835  {
837  }
838  }
839 }
840 
842 {
843  std_msgs::ColorRGBA color;
844  QColor qcolor = query_start_color_property_->getColor();
845  color.r = qcolor.redF();
846  color.g = qcolor.greenF();
847  color.b = qcolor.blueF();
848  color.a = 1.0f;
849  query_robot_start_->setDefaultAttachedObjectColor(color);
851 }
852 
854 {
857 }
858 
860 {
862  return;
863 
864  if (isEnabled())
865  {
866  // Clear the interactive markers and re-add them:
868  }
869 }
870 
872 {
873  std_msgs::ColorRGBA color;
874  QColor qcolor = query_goal_color_property_->getColor();
875  color.r = qcolor.redF();
876  color.g = qcolor.greenF();
877  color.b = qcolor.blueF();
878  color.a = 1.0f;
879  query_robot_goal_->setDefaultAttachedObjectColor(color);
881 }
882 
884 {
887 }
888 
890 {
893 }
894 
896 {
899 }
900 
902 {
904  // forward color to TrajectoryVisualization
905  const QColor& color = attached_body_color_property_->getColor();
906  trajectory_visual_->setDefaultAttachedObjectColor(color);
907 }
908 
910  bool error_state_changed)
911 {
913  return;
914  addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed),
915  "publishInteractiveMarkers");
919 }
920 
922  bool error_state_changed)
923 {
925  return;
926  addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed),
927  "publishInteractiveMarkers");
931 }
932 
934 {
938 }
939 
941 {
945 }
946 
947 void MotionPlanningDisplay::setQueryStartState(const robot_state::RobotState& start)
948 {
949  query_start_state_->setState(start);
951 }
952 
953 void MotionPlanningDisplay::setQueryGoalState(const robot_state::RobotState& goal)
954 {
955  query_goal_state_->setState(goal);
957 }
958 
960 {
961  if (query_start_state_)
962  {
963  kinematics::KinematicsQueryOptions o = query_start_state_->getKinematicsQueryOptions();
965  query_start_state_->setKinematicsQueryOptions(o);
966  }
967  if (query_goal_state_)
968  {
969  kinematics::KinematicsQueryOptions o = query_goal_state_->getKinematicsQueryOptions();
971  query_goal_state_->setKinematicsQueryOptions(o);
972  }
973 }
974 
975 bool MotionPlanningDisplay::isIKSolutionCollisionFree(robot_state::RobotState* state,
976  const robot_model::JointModelGroup* group,
977  const double* ik_solution) const
978 {
979  if (frame_->ui_->collision_aware_ik->isChecked() && planning_scene_monitor_)
980  {
981  state->setJointGroupPositions(group, ik_solution);
982  state->update();
983  bool res = !getPlanningSceneRO()->isStateColliding(*state, group->getName());
984  return res;
985  }
986  else
987  return true;
988 }
989 
991 {
992  unsetAllColors(&query_robot_start_->getRobot());
993  unsetAllColors(&query_robot_goal_->getRobot());
995  if (!group.empty())
996  {
999 
1000  for (std::map<std::string, LinkDisplayStatus>::const_iterator it = status_links_start_.begin();
1001  it != status_links_start_.end(); ++it)
1002  if (it->second == COLLISION_LINK)
1004  else
1005  setLinkColor(&query_robot_start_->getRobot(), it->first,
1007 
1008  for (std::map<std::string, LinkDisplayStatus>::const_iterator it = status_links_goal_.begin();
1009  it != status_links_goal_.end(); ++it)
1010  if (it->second == COLLISION_LINK)
1012  else
1013  setLinkColor(&query_robot_goal_->getRobot(), it->first,
1015  }
1016 }
1017 
1018 void MotionPlanningDisplay::changePlanningGroup(const std::string& group)
1019 {
1020  if (!getRobotModel() || !robot_interaction_)
1021  return;
1022 
1023  if (getRobotModel()->hasJointModelGroup(group))
1024  {
1027  }
1028  else
1029  ROS_ERROR("Group [%s] not found in the robot model.", group.c_str());
1030 }
1031 
1033 {
1034  if (!getRobotModel() || !robot_interaction_)
1035  return;
1036 
1037  if (!planning_group_property_->getStdString().empty())
1038  if (!getRobotModel()->hasJointModelGroup(planning_group_property_->getStdString()))
1039  {
1041  return;
1042  }
1044 
1045  if (robot_interaction_)
1046  robot_interaction_->decideActiveComponents(planning_group_property_->getStdString());
1047 
1050  updateLinkColors();
1051 
1052  if (frame_)
1055  "publishInteractiveMarkers");
1056 }
1057 
1059 {
1061 }
1062 
1064 {
1066 }
1067 
1068 void MotionPlanningDisplay::setQueryStateHelper(bool use_start_state, const std::string& state_name)
1069 {
1070  robot_state::RobotState state = use_start_state ? *getQueryStartState() : *getQueryGoalState();
1071 
1072  std::string v = "<" + state_name + ">";
1073 
1074  if (v == "<random>")
1075  {
1076  if (const robot_state::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup()))
1077  state.setToRandomPositions(jmg);
1078  }
1079  else if (v == "<current>")
1080  {
1082  if (ps)
1083  state = ps->getCurrentState();
1084  }
1085  else if (v == "<same as goal>")
1086  {
1087  state = *getQueryGoalState();
1088  }
1089  else if (v == "<same as start>")
1090  {
1091  state = *getQueryStartState();
1092  }
1093  else
1094  {
1095  // maybe it is a named state
1096  if (const robot_model::JointModelGroup* jmg = state.getJointModelGroup(getCurrentPlanningGroup()))
1097  state.setToDefaultValues(jmg, state_name);
1098  }
1099 
1100  use_start_state ? setQueryStartState(state) : setQueryGoalState(state);
1101 }
1102 
1103 void MotionPlanningDisplay::populateMenuHandler(std::shared_ptr<interactive_markers::MenuHandler>& mh)
1104 {
1105  typedef interactive_markers::MenuHandler immh;
1106  std::vector<std::string> state_names;
1107  state_names.push_back("random");
1108  state_names.push_back("current");
1109  state_names.push_back("same as start");
1110  state_names.push_back("same as goal");
1111 
1112  // hacky way to distinguish between the start and goal handlers...
1113  bool is_start = (mh.get() == menu_handler_start_.get());
1114 
1115  // Commands for changing the state
1116  immh::EntryHandle menu_states =
1117  mh->insert(is_start ? "Set start state to" : "Set goal state to", immh::FeedbackCallback());
1118  for (std::size_t i = 0; i < state_names.size(); ++i)
1119  {
1120  // Don't add "same as start" to the start state handler, and vice versa.
1121  if ((state_names[i] == "same as start" && is_start) || (state_names[i] == "same as goal" && !is_start))
1122  continue;
1123  mh->insert(menu_states, state_names[i],
1124  boost::bind(&MotionPlanningDisplay::setQueryStateHelper, this, is_start, state_names[i]));
1125  }
1126 
1127  // // Group commands, which end up being the same for both interaction handlers
1128  // const std::vector<std::string>& group_names = getRobotModel()->getJointModelGroupNames();
1129  // immh::EntryHandle menu_groups = mh->insert("Planning Group", immh::FeedbackCallback());
1130  // for (int i = 0; i < group_names.size(); ++i)
1131  // mh->insert(menu_groups, group_names[i],
1132  // boost::bind(&MotionPlanningDisplay::changePlanningGroup, this, group_names[i]));
1133 }
1134 
1136 {
1138  trajectory_visual_->onRobotModelLoaded(getRobotModel());
1139 
1140  robot_interaction_.reset(
1141  new robot_interaction::RobotInteraction(getRobotModel(), "rviz_moveit_motion_planning_display"));
1142  int_marker_display_->subProp("Update Topic")
1143  ->setValue(QString::fromStdString(robot_interaction_->getServerTopic() + "/update"));
1144  query_robot_start_->load(*getRobotModel()->getURDF());
1145  query_robot_goal_->load(*getRobotModel()->getURDF());
1146 
1147  robot_state::RobotStatePtr ks(new robot_state::RobotState(getPlanningSceneRO()->getCurrentState()));
1149  "start", *ks, planning_scene_monitor_->getTFClient()));
1151  "goal", *getQueryStartState(), planning_scene_monitor_->getTFClient()));
1152  query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, _1, _2));
1153  query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, _1, _2));
1154  query_start_state_->setGroupStateValidityCallback(
1155  boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, _1, _2, _3));
1156  query_goal_state_->setGroupStateValidityCallback(
1157  boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, _1, _2, _3));
1158 
1159  // Interactive marker menus
1162  query_start_state_->setMenuHandler(menu_handler_start_);
1163  query_goal_state_->setMenuHandler(menu_handler_goal_);
1164 
1165  if (!planning_group_property_->getStdString().empty())
1166  if (!getRobotModel()->hasJointModelGroup(planning_group_property_->getStdString()))
1168 
1169  const std::vector<std::string>& groups = getRobotModel()->getJointModelGroupNames();
1171  for (std::size_t i = 0; i < groups.size(); ++i)
1174  if (!groups.empty() && planning_group_property_->getStdString().empty())
1176 
1177  modified_groups_.clear();
1179 
1180  geometry_msgs::Vector3 gravity_vector;
1181  gravity_vector.x = 0.0;
1182  gravity_vector.y = 0.0;
1183  gravity_vector.z = 9.81;
1184 
1185  dynamics_solver_.clear();
1186  for (std::size_t i = 0; i < groups.size(); ++i)
1187  if (getRobotModel()->getJointModelGroup(groups[i])->isChain())
1188  dynamics_solver_[groups[i]].reset(
1189  new dynamics_solver::DynamicsSolver(getRobotModel(), groups[i], gravity_vector));
1191 }
1192 
1193 void MotionPlanningDisplay::updateStateExceptModified(robot_state::RobotState& dest, const robot_state::RobotState& src)
1194 {
1195  robot_state::RobotState src_copy = src;
1196  for (std::set<std::string>::const_iterator it = modified_groups_.begin(); it != modified_groups_.end(); ++it)
1197  {
1198  const robot_model::JointModelGroup* jmg = dest.getJointModelGroup(*it);
1199  if (jmg)
1200  {
1201  std::vector<double> values_to_keep;
1202  dest.copyJointGroupPositions(jmg, values_to_keep);
1203  src_copy.setJointGroupPositions(jmg, values_to_keep);
1204  }
1205  }
1206 
1207  // overwrite the destination state
1208  dest = src_copy;
1209 }
1210 
1213 {
1215  robot_state::RobotState current_state = getPlanningSceneRO()->getCurrentState();
1216  std::string group = planning_group_property_->getStdString();
1217 
1218  if (query_start_state_property_->getBool() && !group.empty())
1219  {
1220  robot_state::RobotState start = *getQueryStartState();
1221  updateStateExceptModified(start, current_state);
1222  setQueryStartState(start);
1223  }
1224 
1225  if (query_goal_state_property_->getBool() && !group.empty())
1226  {
1227  robot_state::RobotState goal = *getQueryGoalState();
1228  updateStateExceptModified(goal, current_state);
1229  setQueryGoalState(goal);
1230  }
1231 
1232  if (frame_)
1233  frame_->sceneUpdate(update_type);
1234 }
1235 
1236 // ******************************************************************************************
1237 // Enable
1238 // ******************************************************************************************
1240 {
1242 
1243  // Planned Path Display
1244  trajectory_visual_->onEnable();
1245 
1246  text_to_display_->setVisible(false);
1247 
1250 
1253 
1254  if (frame_ && frame_->parentWidget())
1255  frame_->parentWidget()->show();
1256 }
1257 
1258 // ******************************************************************************************
1259 // Disable
1260 // ******************************************************************************************
1262 {
1263  if (robot_interaction_)
1264  robot_interaction_->clear();
1266 
1267  query_robot_start_->setVisible(false);
1268  query_robot_goal_->setVisible(false);
1269  text_to_display_->setVisible(false);
1270 
1272 
1273  // Planned Path Display
1274  trajectory_visual_->onDisable();
1275 
1276  if (frame_ && frame_->parentWidget())
1277  frame_->parentWidget()->hide();
1278 }
1279 
1280 // ******************************************************************************************
1281 // Update
1282 // ******************************************************************************************
1283 void MotionPlanningDisplay::update(float wall_dt, float ros_dt)
1284 {
1285  if (int_marker_display_)
1286  int_marker_display_->update(wall_dt, ros_dt);
1287  if (frame_)
1288  frame_->updateSceneMarkers(wall_dt, ros_dt);
1289 
1290  PlanningSceneDisplay::update(wall_dt, ros_dt);
1291 }
1292 
1293 void MotionPlanningDisplay::updateInternal(float wall_dt, float ros_dt)
1294 {
1295  PlanningSceneDisplay::updateInternal(wall_dt, ros_dt);
1296 
1297  // Planned Path Display
1298  trajectory_visual_->update(wall_dt, ros_dt);
1299 
1301 }
1302 
1304 {
1306  if (frame_)
1307  {
1308  QString host;
1309  ros::NodeHandle nh;
1310  std::string host_param;
1311  if (config.mapGetString("MoveIt_Warehouse_Host", &host))
1312  frame_->ui_->database_host->setText(host);
1313  else if (nh.getParam("warehouse_host", host_param))
1314  {
1315  host = QString::fromStdString(host_param);
1316  frame_->ui_->database_host->setText(host);
1317  }
1318  int port;
1319  if (config.mapGetInt("MoveIt_Warehouse_Port", &port) || nh.getParam("warehouse_port", port))
1320  frame_->ui_->database_port->setValue(port);
1321  float d;
1322  if (config.mapGetFloat("MoveIt_Planning_Time", &d))
1323  frame_->ui_->planning_time->setValue(d);
1324  int attempts;
1325  if (config.mapGetInt("MoveIt_Planning_Attempts", &attempts))
1326  frame_->ui_->planning_attempts->setValue(attempts);
1327  if (config.mapGetFloat("MoveIt_Goal_Tolerance", &d))
1328  frame_->ui_->goal_tolerance->setValue(d);
1329  bool b;
1330  if (config.mapGetBool("MoveIt_Use_Constraint_Aware_IK", &b))
1331  frame_->ui_->collision_aware_ik->setChecked(b);
1332  if (config.mapGetBool("MoveIt_Allow_Approximate_IK", &b))
1333  frame_->ui_->approximate_ik->setChecked(b);
1334  if (config.mapGetBool("MoveIt_Allow_External_Program", &b))
1335  frame_->ui_->allow_external_program->setChecked(b);
1336  if (config.mapGetBool("MoveIt_Allow_Replanning", &b))
1337  frame_->ui_->allow_replanning->setChecked(b);
1338  if (config.mapGetBool("MoveIt_Allow_Sensor_Positioning", &b))
1339  frame_->ui_->allow_looking->setChecked(b);
1340 
1341  float v;
1342  if (config.mapGetFloat("Velocity_Scaling_Factor", &v))
1343  frame_->ui_->velocity_scaling_factor->setValue(v);
1344  if (config.mapGetFloat("Acceleration_Scaling_Factor", &v))
1345  frame_->ui_->acceleration_scaling_factor->setValue(v);
1346 
1347  rviz::Config workspace = config.mapGetChild("MoveIt_Workspace");
1348  rviz::Config ws_center = workspace.mapGetChild("Center");
1349  float val;
1350  if (ws_center.mapGetFloat("X", &val))
1351  frame_->ui_->wcenter_x->setValue(val);
1352  if (ws_center.mapGetFloat("Y", &val))
1353  frame_->ui_->wcenter_y->setValue(val);
1354  if (ws_center.mapGetFloat("Z", &val))
1355  frame_->ui_->wcenter_z->setValue(val);
1356 
1357  rviz::Config ws_size = workspace.mapGetChild("Size");
1358  if (ws_size.isValid())
1359  {
1360  if (ws_size.mapGetFloat("X", &val))
1361  frame_->ui_->wsize_x->setValue(val);
1362  if (ws_size.mapGetFloat("Y", &val))
1363  frame_->ui_->wsize_y->setValue(val);
1364  if (ws_size.mapGetFloat("Z", &val))
1365  frame_->ui_->wsize_z->setValue(val);
1366  }
1367  else
1368  {
1369  std::string node_name = ros::names::append(getMoveGroupNS(), "move_group");
1370  ros::NodeHandle nh_(node_name);
1371  double val;
1372  if (nh_.getParam("default_workspace_bounds", val))
1373  {
1374  frame_->ui_->wsize_x->setValue(val);
1375  frame_->ui_->wsize_y->setValue(val);
1376  frame_->ui_->wsize_z->setValue(val);
1377  }
1378  }
1379  }
1380 }
1381 
1383 {
1385  if (frame_)
1386  {
1387  config.mapSetValue("MoveIt_Warehouse_Host", frame_->ui_->database_host->text());
1388  config.mapSetValue("MoveIt_Warehouse_Port", frame_->ui_->database_port->value());
1389 
1390  config.mapSetValue("MoveIt_Goal_Tolerance", frame_->ui_->goal_tolerance->value());
1391 
1392  // "Options" Section
1393  config.mapSetValue("MoveIt_Planning_Time", frame_->ui_->planning_time->value());
1394  config.mapSetValue("MoveIt_Planning_Attempts", frame_->ui_->planning_attempts->value());
1395  config.mapSetValue("Velocity_Scaling_Factor", frame_->ui_->velocity_scaling_factor->value());
1396  config.mapSetValue("Acceleration_Scaling_Factor", frame_->ui_->acceleration_scaling_factor->value());
1397 
1398  config.mapSetValue("MoveIt_Allow_Replanning", frame_->ui_->allow_replanning->isChecked());
1399  config.mapSetValue("MoveIt_Allow_Sensor_Positioning", frame_->ui_->allow_looking->isChecked());
1400  config.mapSetValue("MoveIt_Allow_External_Program", frame_->ui_->allow_external_program->isChecked());
1401  config.mapSetValue("MoveIt_Use_Constraint_Aware_IK", frame_->ui_->collision_aware_ik->isChecked());
1402  config.mapSetValue("MoveIt_Allow_Approximate_IK", frame_->ui_->approximate_ik->isChecked());
1403 
1404  rviz::Config workspace = config.mapMakeChild("MoveIt_Workspace");
1405  rviz::Config ws_center = workspace.mapMakeChild("Center");
1406  ws_center.mapSetValue("X", frame_->ui_->wcenter_x->value());
1407  ws_center.mapSetValue("Y", frame_->ui_->wcenter_y->value());
1408  ws_center.mapSetValue("Z", frame_->ui_->wcenter_z->value());
1409  rviz::Config ws_size = workspace.mapMakeChild("Size");
1410  ws_size.mapSetValue("X", frame_->ui_->wsize_x->value());
1411  ws_size.mapSetValue("Y", frame_->ui_->wsize_y->value());
1412  ws_size.mapSetValue("Z", frame_->ui_->wsize_z->value());
1413  }
1414 }
1415 
1417 {
1419  if (int_marker_display_)
1422 }
1423 
1424 // Pick and place
1426 {
1427  for (std::size_t i = 0; i < place_locations_display_.size(); ++i)
1429  place_locations_display_.clear();
1430 }
1431 
1432 void MotionPlanningDisplay::visualizePlaceLocations(const std::vector<geometry_msgs::PoseStamped>& place_poses)
1433 {
1435  place_locations_display_.resize(place_poses.size());
1436  for (std::size_t i = 0; i < place_poses.size(); ++i)
1437  {
1439  place_locations_display_[i]->setColor(1.0f, 0.0f, 0.0f, 0.3f);
1440  Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y,
1441  place_poses[i].pose.position.z);
1442  Ogre::Vector3 extents(0.02, 0.02, 0.02);
1443  place_locations_display_[i]->setScale(extents);
1444  place_locations_display_[i]->setPosition(center);
1445  }
1446 }
1447 
1448 } // namespace moveit_rviz_plugin
void scheduleDrawQueryStartState(robot_interaction::RobotInteraction::InteractionHandler *handler, bool error_state_changed)
virtual QColor getColor() const
d
Definition: setup.py:4
#define NULL
void setMin(float min)
void setIcon(QIcon icon)
void showOnTop(bool show=true)
void setMax(float max)
void setGroupColor(rviz::Robot *robot, const std::string &group_name, const QColor &color)
Ogre::ColourValue getOgreColor() const
void updateStateExceptModified(robot_state::RobotState &dest, const robot_state::RobotState &src)
virtual void onRobotModelLoaded()
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_start_
void selectPlanningGroupCallback(const std_msgs::StringConstPtr &msg)
RobotStateVisualizationPtr query_robot_start_
Handles drawing the robot at the start configuration.
Update the links of an rviz::Robot using a robot_state::RobotState.
virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
DisplayContext * context_
void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string &jobname)
virtual void clearOptions()
virtual void load(const rviz::Config &config)
virtual bool setValue(const QVariant &new_value)
virtual QIcon getIcon() const
ros::NodeHandle update_nh_
f
virtual float getFloat() const
bool mapGetFloat(const QString &key, float *value_out) const
virtual QWidget * getParentWindow()=0
void computeMetricsInternal(std::map< std::string, double > &metrics, const robot_interaction::RobotInteraction::EndEffector &eef, const robot_state::RobotState &state, double payload)
robot_state::RobotStateConstPtr getQueryStartState() const
v
std::map< std::string, LinkDisplayStatus > status_links_start_
void setCharacterHeight(Ogre::Real height)
void setQueryStartState(const robot_state::RobotState &start)
void addOptionStd(const std::string &option)
geometry_msgs::TransformStamped t
void setWindowTitle(QString title)
bool isValid() const
virtual void onRobotModelLoaded()
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
virtual void updateInternal(float wall_dt, float ros_dt)
void sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_goal_
bool mapGetString(const QString &key, QString *value_out) const
void setQueryGoalState(const robot_state::RobotState &goal)
std::vector< std::shared_ptr< rviz::Shape > > place_locations_display_
std::string getStdString()
void mapSetValue(const QString &key, QVariant value)
bool isEnabled() const
std::map< std::string, bool > position_only_ik_
Some groups use position only ik, calls to the metrics have to be modified appropriately.
robot_interaction::RobotInteractionPtr robot_interaction_
virtual bool getBool() const
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
QString fixed_frame_
virtual void update(float wall_dt, float ros_dt)
virtual Property * subProp(const QString &sub_name)
group
std::size_t getJobCount() const
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
bool text_display_for_start_
indicates whether the text display is for the start state or not
void displayTable(const std::map< std::string, double > &values, const Ogre::ColourValue &color, const Ogre::Vector3 &pos, const Ogre::Quaternion &orient)
virtual void update(float wall_dt, float ros_dt)
virtual PanelDockWidget * addPane(const QString &name, QWidget *pane, Qt::DockWidgetArea area=Qt::LeftDockWidgetArea, bool floating=true)=0
robot_interaction::RobotInteraction::InteractionHandlerPtr query_start_state_
void visualizePlaceLocations(const std::vector< geometry_msgs::PoseStamped > &place_poses)
virtual void save(rviz::Config config) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
rviz::EditableEnumProperty * planning_group_property_
std::map< std::string, dynamics_solver::DynamicsSolverPtr > dynamics_solver_
void setCaption(const Ogre::String &caption)
Config mapMakeChild(const QString &key)
bool mapGetBool(const QString &key, bool *value_out) const
bool isIKSolutionCollisionFree(robot_state::RobotState *state, const robot_state::JointModelGroup *group, const double *ik_solution) const
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
Ogre::SceneNode * planning_scene_node_
displays planning scene with everything in it
void setTextAlignment(const HorizontalAlignment &horizontalAlignment, const VerticalAlignment &verticalAlignment)
void setFixedFrame(const QString &fixed_frame)
robot_state::RobotStateConstPtr getQueryGoalState() const
bool mapGetInt(const QString &key, int *value_out) const
virtual void load(const rviz::Config &config)
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called ...
virtual Ogre::SceneManager * getSceneManager() const =0
void populateMenuHandler(std::shared_ptr< interactive_markers::MenuHandler > &mh)
void setQueryStateHelper(bool use_start_state, const std::string &v)
virtual void queueRender()=0
const robot_model::RobotModelConstPtr & getRobotModel() const
res
RobotStateVisualizationPtr query_robot_goal_
Handles drawing the robot at the goal configuration.
void setEnabled(bool enabled)
void setColor(const Ogre::ColourValue &color)
bool getParam(const std::string &key, std::string &s) const
virtual DisplayFactory * getDisplayFactory() const =0
moveit::tools::BackgroundProcessing background_process_
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
void initialize(DisplayContext *context)
virtual void update(float wall_dt, float ros_dt)
groups
virtual void updateInternal(float wall_dt, float ros_dt)
virtual Display * make(const QString &class_id, QString *error_return=NULL)
Config mapGetChild(const QString &key) const
robot_interaction::RobotInteraction::InteractionHandlerPtr query_goal_state_
virtual void save(rviz::Config config) const
void scheduleDrawQueryGoalState(robot_interaction::RobotInteraction::InteractionHandler *handler, bool error_state_changed)
void updateSceneMarkers(float wall_dt, float ros_dt)
void setLinkColor(const std::string &link_name, const QColor &color)
void computeMetrics(bool start, const std::string &group, double payload)
bool setStdString(const std::string &std_str)
void setJobUpdateEvent(const JobUpdateCallback &event)
#define ROS_ERROR(...)
virtual QString getName() const
std::map< std::pair< bool, std::string >, std::map< std::string, double > > computed_metrics_
Ogre::SceneNode * text_display_scene_node_
displays texts
virtual WindowManagerInterface * getWindowManager() const =0
std::map< std::string, LinkDisplayStatus > status_links_goal_
rviz::ColorProperty * query_outside_joint_limits_link_color_property_
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene


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