61 #include <OgreSceneManager.h>
62 #include <OgreSceneNode.h>
68 #include <boost/format.hpp>
69 #include <boost/algorithm/string/replace.hpp>
70 #include <boost/algorithm/string/trim.hpp>
71 #include <boost/lexical_cast.hpp>
75 #include "ui_motion_planning_rviz_plugin_frame.h"
83 : PlanningSceneDisplay()
84 , text_to_display_(nullptr)
85 , private_handle_(
"~")
87 , frame_dock_(nullptr)
90 , int_marker_display_(nullptr)
93 plan_category_ =
new rviz::Property(
"Planning Request", QVariant(),
"",
this);
94 metrics_category_ =
new rviz::Property(
"Planning Metrics", QVariant(),
"",
this);
95 path_category_ =
new rviz::Property(
"Planned Path", QVariant(),
"",
this);
98 compute_weight_limit_property_ =
100 "Shows the weight limit at a particular pose for an end-effector", metrics_category_,
101 SLOT(changedShowWeightLimit()),
this);
103 show_manipulability_index_property_ =
104 new rviz::BoolProperty(
"Show Manipulability Index",
false,
"Shows the manipulability index for an end-effector",
105 metrics_category_, SLOT(changedShowManipulabilityIndex()),
this);
107 show_manipulability_property_ =
108 new rviz::BoolProperty(
"Show Manipulability",
false,
"Shows the manipulability for an end-effector",
109 metrics_category_, SLOT(changedShowManipulability()),
this);
112 "Shows the joint torques for a given configuration and payload",
113 metrics_category_, SLOT(changedShowJointTorques()),
this);
115 metrics_set_payload_property_ =
116 new rviz::FloatProperty(
"Payload", 1.0f,
"Specify the payload at the end effector (kg)", metrics_category_,
117 SLOT(changedMetricsSetPayload()),
this);
118 metrics_set_payload_property_->setMin(0.0);
120 metrics_text_height_property_ =
new rviz::FloatProperty(
"TextHeight", 0.08f,
"Text height", metrics_category_,
121 SLOT(changedMetricsTextHeight()),
this);
122 metrics_text_height_property_->setMin(0.001);
126 planning_group_property_ =
128 "The name of the group of links to plan for (from the ones defined in the SRDF)",
129 plan_category_, SLOT(changedPlanningGroup()),
this);
131 "Shows the axis-aligned bounding box for "
132 "the workspace allowed for planning",
133 plan_category_, SLOT(changedWorkspace()),
this);
134 query_start_state_property_ =
135 new rviz::BoolProperty(
"Query Start State",
false,
"Set a custom start state for the motion planning query",
136 plan_category_, SLOT(changedQueryStartState()),
this);
137 query_goal_state_property_ =
138 new rviz::BoolProperty(
"Query Goal State",
true,
"Shows the goal state for the motion planning query",
139 plan_category_, SLOT(changedQueryGoalState()),
this);
140 query_marker_scale_property_ =
142 "Specifies scale of the interactive marker overlayed on the robot. 0 is auto scale.",
143 plan_category_, SLOT(changedQueryMarkerScale()),
this);
144 query_marker_scale_property_->setMin(0.0f);
146 query_start_color_property_ =
147 new rviz::ColorProperty(
"Start State Color", QColor(0, 255, 0),
"The highlight color for the start state",
148 plan_category_, SLOT(changedQueryStartColor()),
this);
149 query_start_alpha_property_ =
150 new rviz::FloatProperty(
"Start State Alpha", 1.0f,
"Specifies the alpha for the robot links", plan_category_,
151 SLOT(changedQueryStartAlpha()),
this);
152 query_start_alpha_property_->setMin(0.0);
153 query_start_alpha_property_->setMax(1.0);
155 query_goal_color_property_ =
156 new rviz::ColorProperty(
"Goal State Color", QColor(250, 128, 0),
"The highlight color for the goal state",
157 plan_category_, SLOT(changedQueryGoalColor()),
this);
159 query_goal_alpha_property_ =
160 new rviz::FloatProperty(
"Goal State Alpha", 1.0f,
"Specifies the alpha for the robot links", plan_category_,
161 SLOT(changedQueryGoalAlpha()),
this);
162 query_goal_alpha_property_->setMin(0.0);
163 query_goal_alpha_property_->setMax(1.0);
165 query_colliding_link_color_property_ =
166 new rviz::ColorProperty(
"Colliding Link Color", QColor(255, 0, 0),
"The highlight color for colliding links",
167 plan_category_, SLOT(changedQueryCollidingLinkColor()),
this);
169 query_outside_joint_limits_link_color_property_ =
171 "The highlight color for child links of joints that are outside bounds", plan_category_,
172 SLOT(changedQueryJointViolationColor()),
this);
175 trajectory_visual_ = std::make_shared<TrajectoryVisualization>(path_category_,
this);
179 const std::string& name) { backgroundJobUpdate(event, name); });
185 MotionPlanningDisplay::~MotionPlanningDisplay()
187 background_process_.clearJobUpdateEvent();
190 query_robot_start_.reset();
191 query_robot_goal_.reset();
193 delete text_to_display_;
194 delete int_marker_display_;
198 void MotionPlanningDisplay::onInitialize()
200 PlanningSceneDisplay::onInitialize();
203 trajectory_visual_->onInitialize(planning_scene_node_, context_, update_nh_);
204 QColor qcolor = attached_body_color_property_->getColor();
205 trajectory_visual_->setDefaultAttachedObjectColor(qcolor);
208 std::make_shared<RobotStateVisualization>(planning_scene_node_, context_,
"Planning Request Start",
nullptr);
209 query_robot_start_->setCollisionVisible(
false);
210 query_robot_start_->setVisualVisible(
true);
211 query_robot_start_->setVisible(query_start_state_property_->getBool());
212 std_msgs::ColorRGBA color;
213 qcolor = query_start_color_property_->getColor();
214 color.r = qcolor.redF();
215 color.g = qcolor.greenF();
216 color.b = qcolor.blueF();
218 query_robot_start_->setDefaultAttachedObjectColor(color);
221 std::make_shared<RobotStateVisualization>(planning_scene_node_, context_,
"Planning Request Goal",
nullptr);
222 query_robot_goal_->setCollisionVisible(
false);
223 query_robot_goal_->setVisualVisible(
true);
224 query_robot_goal_->setVisible(query_goal_state_property_->getBool());
225 qcolor = query_goal_color_property_->getColor();
226 color.r = qcolor.redF();
227 color.g = qcolor.greenF();
228 color.b = qcolor.blueF();
229 query_robot_goal_->setDefaultAttachedObjectColor(color);
234 connect(frame_, SIGNAL(configChanged()), this->getModel(), SIGNAL(configChanged()));
235 resetStatusTextColor();
236 addStatusText(
"Initialized.");
239 connect(frame_, SIGNAL(planningFinished()), trajectory_visual_.get(), SLOT(interruptCurrentDisplay()));
244 connect(frame_dock_, SIGNAL(visibilityChanged(
bool)),
this, SLOT(motionPanelVisibilityChange(
bool)));
245 frame_dock_->setIcon(getIcon());
248 int_marker_display_ = context_->getDisplayFactory()->make(
"rviz/InteractiveMarkers");
249 int_marker_display_->initialize(context_);
251 text_display_scene_node_ = planning_scene_node_->createChildSceneNode();
254 text_to_display_->setCharacterHeight(metrics_text_height_property_->getFloat());
255 text_to_display_->showOnTop();
256 text_to_display_->setVisible(
false);
257 text_display_for_start_ =
false;
258 text_display_scene_node_->attachObject(text_to_display_);
260 if (context_ && context_->getWindowManager() && context_->getWindowManager()->getParentWindow())
262 QShortcut* im_reset_shortcut =
263 new QShortcut(QKeySequence(
"Ctrl+I"), context_->getWindowManager()->getParentWindow());
264 connect(im_reset_shortcut, SIGNAL(activated()),
this, SLOT(resetInteractiveMarkers()));
268 void MotionPlanningDisplay::motionPanelVisibilityChange(
bool enable)
274 void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(
bool enable)
278 planning_group_sub_ = node_handle_.subscribe(
"/rviz/moveit/select_planning_group", 1,
279 &MotionPlanningDisplay::selectPlanningGroupCallback,
this);
283 planning_group_sub_.shutdown();
287 void MotionPlanningDisplay::selectPlanningGroupCallback(
const std_msgs::StringConstPtr& msg)
290 addMainLoopJob([
this, group = msg->data] { changePlanningGroup(group); });
293 void MotionPlanningDisplay::reset()
295 text_to_display_->setVisible(
false);
297 query_robot_start_->clear();
298 query_robot_goal_->clear();
300 PlanningSceneDisplay::reset();
303 trajectory_visual_->reset();
305 bool enabled = this->isEnabled();
310 query_robot_start_->setVisible(query_start_state_property_->getBool());
311 query_robot_goal_->setVisible(query_goal_state_property_->getBool());
315 void MotionPlanningDisplay::setName(
const QString& name)
317 BoolProperty::setName(name);
320 frame_dock_->setWindowTitle(
name);
321 frame_dock_->setObjectName(
name);
323 trajectory_visual_->setName(name);
329 addMainLoopJob([
this] { updateBackgroundJobProgressBar(); });
332 void MotionPlanningDisplay::updateBackgroundJobProgressBar()
336 QProgressBar* p = frame_->ui_->background_job_progress;
337 int n = background_process_.getJobCount();
347 if (p->maximum() < n)
354 p->setValue(p->maximum() - n);
359 void MotionPlanningDisplay::changedShowWeightLimit()
361 if (text_display_for_start_)
363 if (query_start_state_property_->getBool())
364 displayMetrics(
true);
368 if (query_goal_state_property_->getBool())
369 displayMetrics(
false);
373 void MotionPlanningDisplay::changedShowManipulabilityIndex()
375 if (text_display_for_start_)
377 if (query_start_state_property_->getBool())
378 displayMetrics(
true);
382 if (query_goal_state_property_->getBool())
383 displayMetrics(
false);
387 void MotionPlanningDisplay::changedShowManipulability()
389 if (text_display_for_start_)
391 if (query_start_state_property_->getBool())
392 displayMetrics(
true);
396 if (query_goal_state_property_->getBool())
397 displayMetrics(
false);
401 void MotionPlanningDisplay::changedShowJointTorques()
403 if (text_display_for_start_)
405 if (query_start_state_property_->getBool())
406 displayMetrics(
true);
410 if (query_goal_state_property_->getBool())
411 displayMetrics(
false);
415 void MotionPlanningDisplay::changedMetricsSetPayload()
417 if (text_display_for_start_)
419 if (query_start_state_property_->getBool())
420 displayMetrics(
true);
424 if (query_goal_state_property_->getBool())
425 displayMetrics(
false);
429 void MotionPlanningDisplay::changedMetricsTextHeight()
431 text_to_display_->setCharacterHeight(metrics_text_height_property_->getFloat());
434 void MotionPlanningDisplay::displayTable(
const std::map<std::string, double>& values,
const Ogre::ColourValue& color,
435 const Ogre::Vector3& pos,
const Ogre::Quaternion& orient)
438 std::stringstream ss;
439 for (
const std::pair<const std::string, double>&
value :
values)
440 ss << boost::format(
"%-10s %-4.2f") %
value.first %
value.second << std::endl;
442 if (ss.str().empty())
444 text_to_display_->setVisible(
false);
448 text_to_display_->setCaption(ss.str());
449 text_to_display_->setColor(color);
450 text_display_scene_node_->setPosition(pos);
451 text_display_scene_node_->setOrientation(orient);
454 text_to_display_->setVisible(
true);
457 void MotionPlanningDisplay::renderWorkspaceBox()
459 if (!frame_ || !show_workspace_property_->getBool())
462 workspace_box_.reset();
469 std::make_unique<rviz::Shape>(
rviz::Shape::Cube, context_->getSceneManager(), planning_scene_node_);
470 workspace_box_->setColor(0.0
f, 0.0
f, 0.6
f, 0.3
f);
473 Ogre::Vector3 center(frame_->ui_->wcenter_x->value(), frame_->ui_->wcenter_y->value(),
474 frame_->ui_->wcenter_z->value());
475 Ogre::Vector3
extents(frame_->ui_->wsize_x->value(), frame_->ui_->wsize_y->value(), frame_->ui_->wsize_z->value());
476 workspace_box_->setScale(extents);
477 workspace_box_->setPosition(center);
480 void MotionPlanningDisplay::computeMetrics(
bool start,
const std::string& group,
double payload)
482 if (!robot_interaction_)
484 const std::vector<robot_interaction::EndEffectorInteraction>& eef = robot_interaction_->getActiveEndEffectors();
487 boost::mutex::scoped_lock slock(update_metrics_lock_);
489 moveit::core::RobotStateConstPtr state =
start ? getQueryStartState() : getQueryGoalState();
491 if (ee.parent_group == group)
492 computeMetricsInternal(computed_metrics_[std::make_pair(start, group)], ee, *state, payload);
495 void MotionPlanningDisplay::computeMetricsInternal(std::map<std::string, double>& metrics,
500 dynamics_solver::DynamicsSolverPtr ds;
501 std::map<std::string, dynamics_solver::DynamicsSolverPtr>::const_iterator it = dynamics_solver_.find(ee.
parent_group);
502 if (it != dynamics_solver_.end())
509 unsigned int saturated_joint;
510 std::vector<double> joint_values;
512 if (ds->getMaxPayload(joint_values, max_payload, saturated_joint))
514 metrics[
"max_payload"] = max_payload;
515 metrics[
"saturated_joint"] = saturated_joint;
517 std::vector<double> joint_torques;
518 joint_torques.resize(joint_values.size());
519 if (ds->getPayloadTorques(joint_values, payload, joint_torques))
521 for (std::size_t i = 0; i < joint_torques.size(); ++i)
523 std::stringstream stream;
524 stream <<
"torque[" << i <<
"]";
525 metrics[stream.str()] = joint_torques[i];
530 if (kinematics_metrics_)
532 if (position_only_ik_.find(ee.
parent_group) == position_only_ik_.end())
535 double manipulability_index, manipulability;
537 if (kinematics_metrics_->getManipulabilityIndex(state, ee.
parent_group, manipulability_index, position_ik))
538 metrics[
"manipulability_index"] = manipulability_index;
539 if (kinematics_metrics_->getManipulability(state, ee.
parent_group, manipulability))
540 metrics[
"manipulability"] = manipulability;
546 inline void copyItemIfExists(
const std::map<std::string, double>& source, std::map<std::string, double>& dest,
547 const std::string& key)
549 std::map<std::string, double>::const_iterator it = source.find(key);
550 if (it != source.end())
551 dest[key] = it->second;
555 void MotionPlanningDisplay::displayMetrics(
bool start)
557 if (!robot_interaction_ || !planning_scene_monitor_)
560 static const Ogre::Quaternion
ORIENTATION(1.0, 0.0, 0.0, 0.0);
561 const std::vector<robot_interaction::EndEffectorInteraction>& eef = robot_interaction_->getActiveEndEffectors();
565 moveit::core::RobotStateConstPtr state =
start ? getQueryStartState() : getQueryGoalState();
569 Ogre::Vector3 position(0.0, 0.0, 0.0);
570 std::map<std::string, double> text_table;
571 const std::map<std::string, double>& metrics_table = computed_metrics_[std::make_pair(start, ee.
parent_group)];
572 if (compute_weight_limit_property_->getBool())
574 copyItemIfExists(metrics_table, text_table,
"max_payload");
575 copyItemIfExists(metrics_table, text_table,
"saturated_joint");
577 if (show_manipulability_index_property_->getBool())
578 copyItemIfExists(metrics_table, text_table,
"manipulability_index");
579 if (show_manipulability_property_->getBool())
580 copyItemIfExists(metrics_table, text_table,
"manipulability");
581 if (show_joint_torques_property_->getBool())
583 std::size_t nj = getRobotModel()->getJointModelGroup(ee.
parent_group)->getJointModelNames().size();
584 for (
size_t j = 0; j < nj; ++j)
586 std::stringstream stream;
587 stream <<
"torque[" << j <<
"]";
588 copyItemIfExists(metrics_table, text_table, stream.str());
599 const Eigen::Vector3d&
t = state->getGlobalLinkTransform(lm).translation();
602 position[2] =
t.z() + 0.2;
605 displayTable(text_table, query_start_color_property_->getOgreColor(), position, ORIENTATION);
607 displayTable(text_table, query_goal_color_property_->getOgreColor(), position, ORIENTATION);
608 text_display_for_start_ =
start;
612 void MotionPlanningDisplay::drawQueryStartState()
614 if (!planning_scene_monitor_)
617 if (query_start_state_property_->getBool())
621 moveit::core::RobotStateConstPtr state = getQueryStartState();
624 query_robot_start_->update(state);
625 query_robot_start_->setVisible(
true);
628 std::vector<std::string> collision_links;
629 getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
630 status_links_start_.clear();
631 for (
const std::string& collision_link : collision_links)
632 status_links_start_[collision_link] = COLLISION_LINK;
633 if (!collision_links.empty())
636 getPlanningSceneRO()->getCollidingPairs(pairs, *state);
637 setStatusTextColor(query_start_color_property_->getColor());
638 addStatusText(
"Start state colliding links:");
639 for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin(); it != pairs.end();
641 addStatusText(it->first.first +
" - " + it->first.second);
644 if (!getCurrentPlanningGroup().empty())
649 std::vector<std::string> outside_bounds;
652 if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2))
654 outside_bounds.push_back(jmodel->getChildLinkModel()->getName());
655 status_links_start_[outside_bounds.back()] = OUTSIDE_BOUNDS_LINK;
657 if (!outside_bounds.empty())
659 setStatusTextColor(query_start_color_property_->getColor());
660 addStatusText(
"Links descending from joints that are outside bounds in start state:");
661 addStatusText(outside_bounds);
667 displayMetrics(
true);
671 query_robot_start_->setVisible(
false);
672 context_->queueRender();
675 void MotionPlanningDisplay::resetStatusTextColor()
677 setStatusTextColor(Qt::darkGray);
680 void MotionPlanningDisplay::setStatusTextColor(
const QColor& color)
683 frame_->ui_->status_text->setTextColor(color);
686 void MotionPlanningDisplay::addStatusText(
const std::string& text)
689 frame_->ui_->status_text->append(QString::fromStdString(text));
692 void MotionPlanningDisplay::addStatusText(
const std::vector<std::string>& text)
694 for (
const std::string& it : text)
698 void MotionPlanningDisplay::recomputeQueryStartStateMetrics()
700 std::string
group = planning_group_property_->getStdString();
702 computeMetrics(
true, group, metrics_set_payload_property_->getFloat());
705 void MotionPlanningDisplay::recomputeQueryGoalStateMetrics()
707 std::string
group = planning_group_property_->getStdString();
709 computeMetrics(
false, group, metrics_set_payload_property_->getFloat());
712 void MotionPlanningDisplay::changedQueryStartState()
714 if (!planning_scene_monitor_)
716 setStatusTextColor(query_start_color_property_->getColor());
717 addStatusText(
"Changed start state");
718 drawQueryStartState();
719 addBackgroundJob([
this] { publishInteractiveMarkers(
true); },
"publishInteractiveMarkers");
722 void MotionPlanningDisplay::changedQueryGoalState()
724 if (!planning_scene_monitor_)
726 setStatusTextColor(query_goal_color_property_->getColor());
727 addStatusText(
"Changed goal state");
728 drawQueryGoalState();
729 addBackgroundJob([
this] { publishInteractiveMarkers(
true); },
"publishInteractiveMarkers");
732 void MotionPlanningDisplay::drawQueryGoalState()
734 if (!planning_scene_monitor_)
736 if (query_goal_state_property_->getBool())
740 moveit::core::RobotStateConstPtr state = getQueryGoalState();
743 query_robot_goal_->update(state);
744 query_robot_goal_->setVisible(
true);
747 std::vector<std::string> collision_links;
748 getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
749 status_links_goal_.clear();
750 for (
const std::string& collision_link : collision_links)
751 status_links_goal_[collision_link] = COLLISION_LINK;
752 if (!collision_links.empty())
755 getPlanningSceneRO()->getCollidingPairs(pairs, *state);
756 setStatusTextColor(query_goal_color_property_->getColor());
757 addStatusText(
"Goal state colliding links:");
758 for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin(); it != pairs.end();
760 addStatusText(it->first.first +
" - " + it->first.second);
764 if (!getCurrentPlanningGroup().empty())
770 std::vector<std::string> outside_bounds;
772 if (!state->satisfiesBounds(jmodel, jmodel->getMaximumExtent() * 1e-2))
774 outside_bounds.push_back(jmodel->getChildLinkModel()->getName());
775 status_links_goal_[outside_bounds.back()] = OUTSIDE_BOUNDS_LINK;
778 if (!outside_bounds.empty())
780 setStatusTextColor(query_goal_color_property_->getColor());
781 addStatusText(
"Links descending from joints that are outside bounds in goal state:");
782 addStatusText(outside_bounds);
789 displayMetrics(
false);
793 query_robot_goal_->setVisible(
false);
794 context_->queueRender();
797 void MotionPlanningDisplay::resetInteractiveMarkers()
799 query_start_state_->clearError();
800 query_goal_state_->clearError();
801 addBackgroundJob([
this] { publishInteractiveMarkers(
false); },
"publishInteractiveMarkers");
804 void MotionPlanningDisplay::publishInteractiveMarkers(
bool pose_update)
806 if (robot_interaction_)
809 robot_interaction_->showingMarkers(query_start_state_) == query_start_state_property_->getBool() &&
810 robot_interaction_->showingMarkers(query_goal_state_) == query_goal_state_property_->getBool())
812 if (query_start_state_property_->getBool())
813 robot_interaction_->updateInteractiveMarkers(query_start_state_);
814 if (query_goal_state_property_->getBool())
815 robot_interaction_->updateInteractiveMarkers(query_goal_state_);
819 robot_interaction_->clearInteractiveMarkers();
820 if (query_start_state_property_->getBool())
821 robot_interaction_->addInteractiveMarkers(query_start_state_, query_marker_scale_property_->getFloat());
822 if (query_goal_state_property_->getBool())
823 robot_interaction_->addInteractiveMarkers(query_goal_state_, query_marker_scale_property_->getFloat());
824 robot_interaction_->publishInteractiveMarkers();
828 frame_->updateExternalCommunication();
833 void MotionPlanningDisplay::changedQueryStartColor()
835 std_msgs::ColorRGBA color;
836 QColor qcolor = query_start_color_property_->getColor();
837 color.r = qcolor.redF();
838 color.g = qcolor.greenF();
839 color.b = qcolor.blueF();
841 query_robot_start_->setDefaultAttachedObjectColor(color);
842 changedQueryStartState();
845 void MotionPlanningDisplay::changedQueryStartAlpha()
847 query_robot_start_->setAlpha(query_start_alpha_property_->getFloat());
848 changedQueryStartState();
851 void MotionPlanningDisplay::changedQueryMarkerScale()
853 if (!planning_scene_monitor_)
859 publishInteractiveMarkers(
false);
863 void MotionPlanningDisplay::changedQueryGoalColor()
865 std_msgs::ColorRGBA color;
866 QColor qcolor = query_goal_color_property_->getColor();
867 color.r = qcolor.redF();
868 color.g = qcolor.greenF();
869 color.b = qcolor.blueF();
871 query_robot_goal_->setDefaultAttachedObjectColor(color);
872 changedQueryGoalState();
875 void MotionPlanningDisplay::changedQueryGoalAlpha()
877 query_robot_goal_->setAlpha(query_goal_alpha_property_->getFloat());
878 changedQueryGoalState();
881 void MotionPlanningDisplay::changedQueryCollidingLinkColor()
883 changedQueryStartState();
884 changedQueryGoalState();
887 void MotionPlanningDisplay::changedQueryJointViolationColor()
889 changedQueryStartState();
890 changedQueryGoalState();
893 void MotionPlanningDisplay::changedAttachedBodyColor()
895 PlanningSceneDisplay::changedAttachedBodyColor();
897 const QColor& color = attached_body_color_property_->getColor();
898 trajectory_visual_->setDefaultAttachedObjectColor(color);
902 bool error_state_changed)
904 if (!planning_scene_monitor_)
906 addBackgroundJob([
this, pose_update = !error_state_changed] { publishInteractiveMarkers(pose_update); },
907 "publishInteractiveMarkers");
908 updateQueryStartState();
912 bool error_state_changed)
914 if (!planning_scene_monitor_)
916 addBackgroundJob([
this, pose_update = !error_state_changed] { publishInteractiveMarkers(pose_update); },
917 "publishInteractiveMarkers");
918 updateQueryGoalState();
921 void MotionPlanningDisplay::updateQueryStartState()
923 queryStartStateChanged();
924 recomputeQueryStartStateMetrics();
925 addMainLoopJob([
this] { changedQueryStartState(); });
926 context_->queueRender();
929 void MotionPlanningDisplay::updateQueryGoalState()
931 queryGoalStateChanged();
932 recomputeQueryGoalStateMetrics();
933 addMainLoopJob([
this] { changedQueryGoalState(); });
934 context_->queueRender();
937 void MotionPlanningDisplay::rememberPreviousStartState()
939 *previous_state_ = *query_start_state_->getState();
944 query_start_state_->setState(
start);
945 updateQueryStartState();
950 query_goal_state_->setState(goal);
951 updateQueryGoalState();
954 void MotionPlanningDisplay::useApproximateIK(
bool flag)
956 if (robot_interaction_)
960 robot_interaction_->getKinematicOptionsMap()->setOptions(
968 const double* ik_solution)
const
970 if (frame_->ui_->collision_aware_ik->isChecked() && planning_scene_monitor_)
974 bool res = !getPlanningSceneRO()->isStateColliding(*state,
group->getName());
981 void MotionPlanningDisplay::updateLinkColors()
983 unsetAllColors(&query_robot_start_->getRobot());
984 unsetAllColors(&query_robot_goal_->getRobot());
985 std::string
group = planning_group_property_->getStdString();
988 setGroupColor(&query_robot_start_->getRobot(),
group, query_start_color_property_->getColor());
989 setGroupColor(&query_robot_goal_->getRobot(),
group, query_goal_color_property_->getColor());
991 for (std::map<std::string, LinkDisplayStatus>::const_iterator it = status_links_start_.begin();
992 it != status_links_start_.end(); ++it)
993 if (it->second == COLLISION_LINK)
994 setLinkColor(&query_robot_start_->getRobot(), it->first, query_colliding_link_color_property_->getColor());
996 setLinkColor(&query_robot_start_->getRobot(), it->first,
997 query_outside_joint_limits_link_color_property_->getColor());
999 for (std::map<std::string, LinkDisplayStatus>::const_iterator it = status_links_goal_.begin();
1000 it != status_links_goal_.end(); ++it)
1001 if (it->second == COLLISION_LINK)
1002 setLinkColor(&query_robot_goal_->getRobot(), it->first, query_colliding_link_color_property_->getColor());
1004 setLinkColor(&query_robot_goal_->getRobot(), it->first,
1005 query_outside_joint_limits_link_color_property_->getColor());
1009 void MotionPlanningDisplay::changePlanningGroup(
const std::string& group)
1011 if (!getRobotModel() || !robot_interaction_)
1014 if (getRobotModel()->hasJointModelGroup(group))
1016 planning_group_property_->setStdString(
group);
1019 ROS_ERROR(
"Group [%s] not found in the robot model.",
group.c_str());
1022 void MotionPlanningDisplay::changedPlanningGroup()
1024 if (!getRobotModel() || !robot_interaction_)
1027 if (!planning_group_property_->getStdString().empty() &&
1028 !getRobotModel()->hasJointModelGroup(planning_group_property_->getStdString()))
1030 planning_group_property_->setStdString(
"");
1033 modified_groups_.insert(planning_group_property_->getStdString());
1035 robot_interaction_->decideActiveComponents(planning_group_property_->getStdString());
1037 updateQueryStartState();
1038 updateQueryGoalState();
1042 frame_->changePlanningGroup();
1043 addBackgroundJob([
this] { publishInteractiveMarkers(
false); },
"publishInteractiveMarkers");
1046 void MotionPlanningDisplay::changedWorkspace()
1048 renderWorkspaceBox();
1051 std::string MotionPlanningDisplay::getCurrentPlanningGroup()
const
1053 return planning_group_property_->getStdString();
1056 void MotionPlanningDisplay::setQueryStateHelper(
bool use_start_state,
const std::string& state_name)
1060 std::string
v =
"<" + state_name +
">";
1062 if (
v ==
"<random>")
1067 else if (v ==
"<current>")
1071 state = ps->getCurrentState();
1073 else if (v ==
"<same as goal>")
1075 state = *getQueryGoalState();
1077 else if (v ==
"<same as start>")
1079 state = *getQueryStartState();
1088 use_start_state ? setQueryStartState(state) : setQueryGoalState(state);
1091 void MotionPlanningDisplay::populateMenuHandler(std::shared_ptr<interactive_markers::MenuHandler>& mh)
1094 std::vector<std::string> state_names;
1095 state_names.push_back(
"random");
1096 state_names.push_back(
"current");
1097 state_names.push_back(
"same as start");
1098 state_names.push_back(
"same as goal");
1101 bool is_start = (mh.get() == menu_handler_start_.get());
1104 immh::EntryHandle menu_states =
1105 mh->insert(is_start ?
"Set start state to" :
"Set goal state to", immh::FeedbackCallback());
1106 for (
const std::string& state_name : state_names)
1109 if ((state_name ==
"same as start" && is_start) || (state_name ==
"same as goal" && !is_start))
1111 mh->insert(menu_states, state_name,
1112 [
this, is_start, state_name](
auto&& ) { setQueryStateHelper(is_start, state_name); });
1123 void MotionPlanningDisplay::clearRobotModel()
1127 frame_->clearRobotModel();
1128 if (trajectory_visual_)
1129 trajectory_visual_->clearRobotModel();
1130 previous_state_.reset();
1131 query_start_state_.reset();
1132 query_goal_state_.reset();
1133 kinematics_metrics_.reset();
1134 robot_interaction_.reset();
1135 dynamics_solver_.clear();
1137 PlanningSceneDisplay::clearRobotModel();
1140 void MotionPlanningDisplay::onRobotModelLoaded()
1142 PlanningSceneDisplay::onRobotModelLoaded();
1143 trajectory_visual_->onRobotModelLoaded(getRobotModel());
1145 std::string ns =
"rviz_moveit_motion_planning_display";
1147 if (!robot_desc_ns.empty())
1149 robot_interaction_ = std::make_shared<robot_interaction::RobotInteraction>(getRobotModel(), ns);
1153 const double* joint_group_variable_values) {
1154 return isIKSolutionCollisionFree(robot_state, joint_group, joint_group_variable_values);
1156 robot_interaction_->getKinematicOptionsMap()->setOptions(
1159 int_marker_display_->subProp(
"Update Topic")
1160 ->setValue(QString::fromStdString(robot_interaction_->getServerTopic() +
"/update"));
1161 query_robot_start_->load(*getRobotModel()->getURDF());
1162 query_robot_goal_->load(*getRobotModel()->getURDF());
1165 previous_state_ = std::make_shared<moveit::core::RobotState>(getPlanningSceneRO()->getCurrentState());
1166 query_start_state_ = std::make_shared<robot_interaction::InteractionHandler>(
1167 robot_interaction_,
"start", *previous_state_, planning_scene_monitor_->getTFClient());
1168 query_goal_state_ = std::make_shared<robot_interaction::InteractionHandler>(
1169 robot_interaction_,
"goal", *previous_state_, planning_scene_monitor_->getTFClient());
1170 query_start_state_->setUpdateCallback(
1172 scheduleDrawQueryStartState(handler, error_state_changed);
1175 scheduleDrawQueryGoalState(handler, error_state_changed);
1179 populateMenuHandler(menu_handler_start_);
1180 populateMenuHandler(menu_handler_goal_);
1181 query_start_state_->setMenuHandler(menu_handler_start_);
1182 query_goal_state_->setMenuHandler(menu_handler_goal_);
1184 if (!planning_group_property_->getStdString().empty())
1185 if (!getRobotModel()->hasJointModelGroup(planning_group_property_->getStdString()))
1186 planning_group_property_->setStdString(
"");
1188 const std::vector<std::string>& groups = getRobotModel()->getJointModelGroupNames();
1189 planning_group_property_->clearOptions();
1190 for (
const std::string& group : groups)
1191 planning_group_property_->addOptionStd(group);
1192 planning_group_property_->sortOptions();
1193 if (!groups.empty() && planning_group_property_->getStdString().empty())
1194 planning_group_property_->setStdString(groups[0]);
1196 modified_groups_.clear();
1197 kinematics_metrics_ = std::make_shared<kinematics_metrics::KinematicsMetrics>(getRobotModel());
1199 geometry_msgs::Vector3 gravity_vector;
1200 gravity_vector.x = 0.0;
1201 gravity_vector.y = 0.0;
1202 gravity_vector.z = 9.81;
1204 dynamics_solver_.clear();
1205 for (
const std::string& group : groups)
1206 if (getRobotModel()->getJointModelGroup(group)->isChain())
1207 dynamics_solver_[
group] =
1208 std::make_shared<dynamics_solver::DynamicsSolver>(getRobotModel(), group, gravity_vector);
1211 frame_->fillPlanningGroupOptions();
1212 changedPlanningGroup();
1214 void MotionPlanningDisplay::onNewPlanningSceneState()
1216 frame_->onNewPlanningSceneState();
1223 for (
const std::string& modified_group : modified_groups_)
1228 std::vector<double> values_to_keep;
1240 std::string
group = planning_group_property_->getStdString();
1244 if (query_start_state_)
1247 updateStateExceptModified(start, current_state);
1248 if (query_start_state_property_->getBool())
1249 setQueryStartState(start);
1252 if (query_goal_state_)
1255 updateStateExceptModified(goal, current_state);
1256 if (query_goal_state_property_->getBool())
1257 setQueryGoalState(goal);
1261 void MotionPlanningDisplay::onSceneMonitorReceivedUpdate(
1264 PlanningSceneDisplay::onSceneMonitorReceivedUpdate(update_type);
1265 updateQueryStates(getPlanningSceneRO()->getCurrentState());
1267 frame_->sceneUpdate(update_type);
1273 void MotionPlanningDisplay::onEnable()
1275 PlanningSceneDisplay::onEnable();
1278 trajectory_visual_->onEnable();
1280 text_to_display_->setVisible(
false);
1282 query_robot_start_->setVisible(query_start_state_property_->getBool());
1283 query_robot_goal_->setVisible(query_goal_state_property_->getBool());
1285 int_marker_display_->setEnabled(
true);
1286 int_marker_display_->setFixedFrame(fixed_frame_);
1294 void MotionPlanningDisplay::onDisable()
1296 if (robot_interaction_)
1297 robot_interaction_->clear();
1298 int_marker_display_->setEnabled(
false);
1300 query_robot_start_->setVisible(
false);
1301 query_robot_goal_->setVisible(
false);
1302 text_to_display_->setVisible(
false);
1304 PlanningSceneDisplay::onDisable();
1307 trajectory_visual_->onDisable();
1315 void MotionPlanningDisplay::update(
float wall_dt,
float ros_dt)
1317 if (int_marker_display_)
1318 int_marker_display_->update(wall_dt, ros_dt);
1320 frame_->updateSceneMarkers(wall_dt, ros_dt);
1322 PlanningSceneDisplay::update(wall_dt, ros_dt);
1325 void MotionPlanningDisplay::updateInternal(
float wall_dt,
float ros_dt)
1327 PlanningSceneDisplay::updateInternal(wall_dt, ros_dt);
1330 trajectory_visual_->update(wall_dt, ros_dt);
1332 renderWorkspaceBox();
1335 void MotionPlanningDisplay::load(
const rviz::Config& config)
1337 PlanningSceneDisplay::load(config);
1341 if (
config.mapGetFloat(
"MoveIt_Planning_Time", &
d))
1342 frame_->ui_->planning_time->setValue(
d);
1344 if (
config.mapGetInt(
"MoveIt_Planning_Attempts", &attempts))
1345 frame_->ui_->planning_attempts->setValue(attempts);
1346 if (
config.mapGetFloat(
"Velocity_Scaling_Factor", &
d))
1347 frame_->ui_->velocity_scaling_factor->setValue(
d);
1348 if (
config.mapGetFloat(
"Acceleration_Scaling_Factor", &
d))
1349 frame_->ui_->acceleration_scaling_factor->setValue(
d);
1352 if (
config.mapGetBool(
"MoveIt_Allow_Replanning", &b))
1353 frame_->ui_->allow_replanning->setChecked(b);
1354 if (
config.mapGetBool(
"MoveIt_Allow_Sensor_Positioning", &b))
1355 frame_->ui_->allow_looking->setChecked(b);
1356 if (
config.mapGetBool(
"MoveIt_Allow_External_Program", &b))
1357 frame_->ui_->allow_external_program->setChecked(b);
1358 if (
config.mapGetBool(
"MoveIt_Use_Cartesian_Path", &b))
1359 frame_->ui_->use_cartesian_path->setChecked(b);
1360 if (
config.mapGetBool(
"MoveIt_Use_Constraint_Aware_IK", &b))
1361 frame_->ui_->collision_aware_ik->setChecked(b);
1362 if (
config.mapGetBool(
"MoveIt_Allow_Approximate_IK", &b))
1363 frame_->ui_->approximate_ik->setChecked(b);
1364 if (
config.mapGetBool(
"JointsTab_Use_Radians", &b))
1365 frame_->joints_tab_->setUseRadians(
true);
1371 frame_->ui_->wcenter_x->setValue(val);
1373 frame_->ui_->wcenter_y->setValue(val);
1375 frame_->ui_->wcenter_z->setValue(val);
1383 frame_->ui_->wsize_y->setValue(
val);
1385 frame_->ui_->wsize_z->setValue(
val);
1391 if (nh.getParam(
"default_workspace_bounds", val))
1393 frame_->ui_->wsize_x->setValue(val);
1394 frame_->ui_->wsize_y->setValue(val);
1395 frame_->ui_->wsize_z->setValue(val);
1401 void MotionPlanningDisplay::save(
rviz::Config config)
const
1403 PlanningSceneDisplay::save(config);
1407 config.mapSetValue(
"MoveIt_Planning_Time", frame_->ui_->planning_time->value());
1408 config.mapSetValue(
"MoveIt_Planning_Attempts", frame_->ui_->planning_attempts->value());
1409 config.mapSetValue(
"Velocity_Scaling_Factor", frame_->ui_->velocity_scaling_factor->value());
1410 config.mapSetValue(
"Acceleration_Scaling_Factor", frame_->ui_->acceleration_scaling_factor->value());
1412 config.mapSetValue(
"MoveIt_Allow_Replanning", frame_->ui_->allow_replanning->isChecked());
1413 config.mapSetValue(
"MoveIt_Allow_Sensor_Positioning", frame_->ui_->allow_looking->isChecked());
1414 config.mapSetValue(
"MoveIt_Allow_External_Program", frame_->ui_->allow_external_program->isChecked());
1415 config.mapSetValue(
"MoveIt_Use_Cartesian_Path", frame_->ui_->use_cartesian_path->isChecked());
1416 config.mapSetValue(
"MoveIt_Use_Constraint_Aware_IK", frame_->ui_->collision_aware_ik->isChecked());
1417 config.mapSetValue(
"MoveIt_Allow_Approximate_IK", frame_->ui_->approximate_ik->isChecked());
1418 config.mapSetValue(
"JointsTab_Use_Radians", frame_->joints_tab_->useRadians());
1422 ws_center.
mapSetValue(
"X", frame_->ui_->wcenter_x->value());
1423 ws_center.
mapSetValue(
"Y", frame_->ui_->wcenter_y->value());
1424 ws_center.
mapSetValue(
"Z", frame_->ui_->wcenter_z->value());
1426 ws_size.
mapSetValue(
"X", frame_->ui_->wsize_x->value());
1427 ws_size.
mapSetValue(
"Y", frame_->ui_->wsize_y->value());
1428 ws_size.
mapSetValue(
"Z", frame_->ui_->wsize_z->value());
1432 void MotionPlanningDisplay::fixedFrameChanged()
1434 PlanningSceneDisplay::fixedFrameChanged();
1435 if (int_marker_display_)
1436 int_marker_display_->setFixedFrame(fixed_frame_);
1437 changedPlanningGroup();
1441 void MotionPlanningDisplay::clearPlaceLocationsDisplay()
1443 for (std::shared_ptr<rviz::Shape>& place_location_shape : place_locations_display_)
1444 place_location_shape.reset();
1445 place_locations_display_.clear();
1448 void MotionPlanningDisplay::visualizePlaceLocations(
const std::vector<geometry_msgs::PoseStamped>& place_poses)
1450 clearPlaceLocationsDisplay();
1451 place_locations_display_.resize(place_poses.size());
1452 for (std::size_t i = 0; i < place_poses.size(); ++i)
1454 place_locations_display_[i] = std::make_shared<rviz::Shape>(
rviz::Shape::Sphere, context_->getSceneManager());
1455 place_locations_display_[i]->setColor(1.0f, 0.0f, 0.0f, 0.3f);
1456 Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y, place_poses[i].pose.position.z);
1457 Ogre::Vector3
extents(0.02, 0.02, 0.02);
1458 place_locations_display_[i]->setScale(extents);
1459 place_locations_display_[i]->setPosition(center);