From fea1f5ae70a831f6ff7acc3570c86cc105627a57 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 14 Jul 2016 22:31:08 +0200 Subject: [PATCH 1/6] cleanup structure of updateQueryStateHelper --- .../src/motion_planning_frame_planning.cpp | 97 ++++++++++--------- 1 file changed, 50 insertions(+), 47 deletions(-) diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 4054d01950..2992486f67 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -162,63 +162,66 @@ void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState &state, configureWorkspace(); if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) state.setToRandomPositions(jmg); + return; } - else - if (v == "") - { - configureWorkspace(); - if (const robot_model::JointModelGroup *jmg = + if (v == "") + { + configureWorkspace(); + + if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) + { + // Loop until a collision free state is found + static const int MAX_ATTEMPTS = 100; + int attempt_count = 0; // prevent loop for going forever + while (attempt_count < MAX_ATTEMPTS) { - // Loop until a collision free state is found - static const int MAX_ATTEMPTS = 100; - int attempt_count = 0; // prevent loop for going forever - while (attempt_count < MAX_ATTEMPTS) - { - // Generate random state - state.setToRandomPositions(jmg); + // Generate random state + state.setToRandomPositions(jmg); - state.update(); // prevent dirty transforms + state.update(); // prevent dirty transforms - // Test for collision - if (planning_display_->getPlanningSceneRO()->isStateValid(state, "", false)) - break; + // Test for collision + if (planning_display_->getPlanningSceneRO()->isStateValid(state, "", false)) + break; - attempt_count ++; - } - // Explain if no valid rand state found - if (attempt_count >= MAX_ATTEMPTS) - ROS_WARN("Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS); - } - else - { - ROS_WARN_STREAM("Unable to get joint model group " << planning_display_->getCurrentPlanningGroup()); + attempt_count ++; } + // Explain if no valid rand state found + if (attempt_count >= MAX_ATTEMPTS) + ROS_WARN("Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS); } else - if (v == "") - { - const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO(); - if (ps) - state = ps->getCurrentState(); - } - else - if (v == "") - { - state = *planning_display_->getQueryGoalState(); - } - else - if (v == "") - { - state = *planning_display_->getQueryStartState(); - } - else - { - // maybe it is a named state - if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) - state.setToDefaultValues(jmg, v); - } + { + ROS_WARN_STREAM("Unable to get joint model group " << planning_display_->getCurrentPlanningGroup()); + } + return; + } + + if (v == "") + { + const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO(); + if (ps) + state = ps->getCurrentState(); + return; + } + + if (v == "") + { + state = *planning_display_->getQueryGoalState(); + return; + } + + if (v == "") + { + state = *planning_display_->getQueryStartState(); + return; + } + + // maybe it is a named state + if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup())) + state.setToDefaultValues(jmg, v); } void MotionPlanningFrame::populatePlannersList(const moveit_msgs::PlannerInterfaceDescription &desc) From d0fab1bb00a057b8a90cd4ed1b9854282da0bc9f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 14 Jul 2016 13:46:09 +0200 Subject: [PATCH 2/6] updateStartStateToCurrent after execution --- .../motion_planning_frame.h | 1 + .../src/motion_planning_frame_planning.cpp | 12 ++++++++++++ 2 files changed, 13 insertions(+) diff --git a/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index a0ca95beb7..bbc139323d 100644 --- a/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -210,6 +210,7 @@ private Q_SLOTS: void computeExecuteButtonClicked(); void computePlanAndExecuteButtonClicked(); void computePlanAndExecuteButtonClickedDisplayHelper(); + void updateStartStateToCurrent(); void populateConstraintsList(); void populateConstraintsList(const std::vector &constr); void configureForPlanning(); diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 2992486f67..cad05a1d50 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -130,6 +130,8 @@ void MotionPlanningFrame::computeExecuteButtonClicked() { if (move_group_ && current_plan_) move_group_->execute(*current_plan_); + + updateStartStateToCurrent(); } void MotionPlanningFrame::computePlanAndExecuteButtonClicked() @@ -139,6 +141,16 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked() configureForPlanning(); move_group_->move(); ui_->plan_and_execute_button->setEnabled(true); + + updateStartStateToCurrent(); +} + +void MotionPlanningFrame::updateStartStateToCurrent() +{ + if (ui_->start_state_selection->currentText() == "") + { + useStartStateButtonClicked(); + } } void MotionPlanningFrame::useStartStateButtonClicked() From f9808b868f87ea5a625a392f86e0f2a330005774 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 14 Jul 2016 11:50:12 +0200 Subject: [PATCH 3/6] visualization: stop execution button --- .../motion_planning_frame.h | 4 +- .../src/motion_planning_frame.cpp | 1 + .../src/motion_planning_frame_planning.cpp | 37 +++++++++++++++---- .../ui/motion_planning_rviz_plugin_frame.ui | 11 ++++++ 4 files changed, 44 insertions(+), 9 deletions(-) diff --git a/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index bbc139323d..cdea56cf1e 100644 --- a/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -143,6 +143,7 @@ private Q_SLOTS: void planButtonClicked(); void executeButtonClicked(); void planAndExecuteButtonClicked(); + void stopButtonClicked(); void allowReplanningToggled(bool checked); void allowLookingToggled(bool checked); void allowExternalProgramCommunication(bool enable); @@ -210,7 +211,8 @@ private Q_SLOTS: void computeExecuteButtonClicked(); void computePlanAndExecuteButtonClicked(); void computePlanAndExecuteButtonClickedDisplayHelper(); - void updateStartStateToCurrent(); + void computeStopButtonClicked(); + void onFinishedExecution(bool success); void populateConstraintsList(); void populateConstraintsList(const std::vector &constr); void configureForPlanning(); diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index b9a85608df..a5b7b29e63 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -70,6 +70,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay *pdisplay, rviz:: connect( ui_->plan_button, SIGNAL( clicked() ), this, SLOT( planButtonClicked() )); connect( ui_->execute_button, SIGNAL( clicked() ), this, SLOT( executeButtonClicked() )); connect( ui_->plan_and_execute_button, SIGNAL( clicked() ), this, SLOT( planAndExecuteButtonClicked() )); + connect( ui_->stop_button, SIGNAL( clicked() ), this, SLOT( stopButtonClicked() )); connect( ui_->use_start_state_button, SIGNAL( clicked() ), this, SLOT( useStartStateButtonClicked() )); connect( ui_->use_goal_state_button, SIGNAL( clicked() ), this, SLOT( useGoalStateButtonClicked() )); connect( ui_->database_connect_button, SIGNAL( clicked() ), this, SLOT( databaseConnectButtonClicked() )); diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index cad05a1d50..8f687b91b0 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -65,6 +65,12 @@ void MotionPlanningFrame::planAndExecuteButtonClicked() planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this), "plan and execute"); } +void MotionPlanningFrame::stopButtonClicked() +{ + ui_->stop_button->setEnabled(false); // avoid clicking again + planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeStopButtonClicked, this), "stop"); +} + void MotionPlanningFrame::allowReplanningToggled(bool checked) { if (move_group_) @@ -129,9 +135,11 @@ void MotionPlanningFrame::computePlanButtonClicked() void MotionPlanningFrame::computeExecuteButtonClicked() { if (move_group_ && current_plan_) - move_group_->execute(*current_plan_); - - updateStartStateToCurrent(); + { + ui_->stop_button->setEnabled(true); // enable stopping + bool success = move_group_->execute(*current_plan_); + onFinishedExecution(success); + } } void MotionPlanningFrame::computePlanAndExecuteButtonClicked() @@ -139,18 +147,31 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked() if (!move_group_) return; configureForPlanning(); - move_group_->move(); + ui_->stop_button->setEnabled(true); + bool success = move_group_->move(); + onFinishedExecution(success); ui_->plan_and_execute_button->setEnabled(true); +} - updateStartStateToCurrent(); +void MotionPlanningFrame::computeStopButtonClicked() +{ + if (move_group_) + move_group_->stop(); } -void MotionPlanningFrame::updateStartStateToCurrent() +void MotionPlanningFrame::onFinishedExecution(bool success) { + // visualize result of execution + if (success) + ui_->result_label->setText("Executed"); + else + ui_->result_label->setText(!ui_->stop_button->isEnabled() ? "Stopped" : "Failed"); + // disable stop button + ui_->stop_button->setEnabled(false); + + // update query start state to current if neccessary if (ui_->start_state_selection->currentText() == "") - { useStartStateButtonClicked(); - } } void MotionPlanningFrame::useStartStateButtonClicked() diff --git a/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 10a291056e..807a461a02 100644 --- a/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -237,6 +237,16 @@ + + + + false + + + &Stop + + + @@ -1601,6 +1611,7 @@ plan_button execute_button plan_and_execute_button + stop_button start_state_selection use_start_state_button goal_state_selection From 96664f7d5b9290fbc10900253e768d0830b44282 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 14 Jul 2016 22:36:34 +0200 Subject: [PATCH 4/6] rviz plugin: execute motion in separate thread executing the motion in the (single) background thread blocks visual updates of the scene robot --- .../src/motion_planning_frame_planning.cpp | 6 ++++-- .../planning_scene_display.h | 12 +++++++++--- .../src/planning_scene_display.cpp | 5 +++++ 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 8f687b91b0..cb735d09bd 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -55,14 +55,16 @@ void MotionPlanningFrame::planButtonClicked() void MotionPlanningFrame::executeButtonClicked() { ui_->execute_button->setEnabled(false); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this), "execute"); + // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution + planning_display_->spawnBackgroundJob(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this)); } void MotionPlanningFrame::planAndExecuteButtonClicked() { ui_->plan_and_execute_button->setEnabled(false); ui_->execute_button->setEnabled(false); - planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this), "plan and execute"); + // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution + planning_display_->spawnBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this)); } void MotionPlanningFrame::stopButtonClicked() diff --git a/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 620b0768cd..2b459087e8 100644 --- a/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -87,15 +87,21 @@ class PlanningSceneDisplay : public rviz::Display void queueRenderSceneGeometry(); - // pass the execution of this function call to a separate thread that runs in the background + /** Queue this function call for execution within the background thread + All jobs are queued and processed in order by a single background thread. */ void addBackgroundJob(const boost::function &job, const std::string &name); - // queue the execution of this function for the next time the main update() loop gets called + /** Directly spawn a (detached) background thread for execution of this function call + Should be used, when order of processing is not relevant / job can run in parallel. + Must be used, when job will be blocking. Using addBackgroundJob() in this case will block other queued jobs as well */ + void spawnBackgroundJob(const boost::function &job); + + /// queue the execution of this function for the next time the main update() loop gets called void addMainLoopJob(const boost::function &job); void waitForAllMainLoopJobs(); - // remove all queued jobs + /// remove all queued jobs void clearJobs(); const std::string getMoveGroupNS() const; diff --git a/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 34c268795c..5cf86b962e 100644 --- a/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -232,6 +232,11 @@ void PlanningSceneDisplay::addBackgroundJob(const boost::function &job, background_process_.addJob(job, name); } +void PlanningSceneDisplay::spawnBackgroundJob(const boost::function &job) +{ + boost::thread t(job); +} + void PlanningSceneDisplay::addMainLoopJob(const boost::function &job) { boost::unique_lock ulock(main_loop_jobs_lock_); From 298a67de52285eb98f0bf154e28b6ce83f424f17 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 15 Jul 2016 23:15:06 +0200 Subject: [PATCH 5/6] suppress warning "Execution of motions should always start at the robot's current state." when planning+executing --- .../src/motion_planning_frame_planning.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index cb735d09bd..5d6fb4ea1d 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -149,6 +149,9 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked() if (!move_group_) return; configureForPlanning(); + // move_group::move() on the server side, will always start from the current state + // to suppress a warning, we pass an empty state (which encodes "start from current state") + move_group_->setStartStateToCurrentState(); ui_->stop_button->setEnabled(true); bool success = move_group_->move(); onFinishedExecution(success); From 181e180667689c2ba382edd6f7674a495377d0f4 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 11 Jul 2016 00:00:22 +0200 Subject: [PATCH 6/6] rviz display: disable Query Start State by default This property is "only" used by people playing around with planning internals, e.g. the people maintaining the code.. The "average" user does not expect to be able to manipulate the start state of the motion-request te makes from rviz - it doesn't make sense if you want to actually move the robot. Therefore, disable the start-state by default to reduce the initial complexity the average user is confronted with. --- .../motion_planning_rviz_plugin/src/motion_planning_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index b90e087a20..ae283aabb4 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -131,7 +131,7 @@ MotionPlanningDisplay::MotionPlanningDisplay() : show_workspace_property_ = new rviz::BoolProperty("Show Workspace", false, "Shows the axis-aligned bounding box for the workspace allowed for planning", plan_category_, SLOT(changedWorkspace()), this); - query_start_state_property_ = new rviz::BoolProperty("Query Start State", true, "Shows the start state for the motion planning query", + query_start_state_property_ = new rviz::BoolProperty("Query Start State", false, "Set a custom start state for the motion planning query", plan_category_, SLOT(changedQueryStartState()), this); query_goal_state_property_ = new rviz::BoolProperty("Query Goal State", true, "Shows the goal state for the motion planning query",