Merge pull request #718 from ros-planning/wip-stop-execution

cherry-picked #713 from indigo-devel

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,6 +211,8 @@ private Q_SLOTS:
   void computeExecuteButtonClicked();
   void computePlanAndExecuteButtonClicked();
   void computePlanAndExecuteButtonClickedDisplayHelper();
+  void computeStopButtonClicked();
+  void onFinishedExecution(bool success);
   void populateConstraintsList();
   void populateConstraintsList(const std::vector<std::string> &constr);
   void configureForPlanning();

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",

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() ));

visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

@@ -55,14 +55,22 @@ 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()
+{
+  ui_->stop_button->setEnabled(false); // avoid clicking again
+  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeStopButtonClicked, this), "stop");
 }
 
 void MotionPlanningFrame::allowReplanningToggled(bool checked)
@@ -129,18 +137,48 @@ void MotionPlanningFrame::computePlanButtonClicked()
 void MotionPlanningFrame::computeExecuteButtonClicked()
 {
   if (move_group_ && current_plan_)
-    move_group_->execute(*current_plan_);
+  {
+    ui_->stop_button->setEnabled(true); // enable stopping
+    bool success = move_group_->execute(*current_plan_);
+    onFinishedExecution(success);
+  }
 }
 
 void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
 {
   if (!move_group_)
     return;
   configureForPlanning();
-  move_group_->move();
+  // 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);
   ui_->plan_and_execute_button->setEnabled(true);
 }
 
+void MotionPlanningFrame::computeStopButtonClicked()
+{
+  if (move_group_)
+    move_group_->stop();
+}
+
+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() == "<current>")
+    useStartStateButtonClicked();
+}
+
 void MotionPlanningFrame::useStartStateButtonClicked()
 {
   robot_state::RobotState start = *planning_display_->getQueryStartState();
@@ -162,63 +200,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 == "<random valid>")
-    {
-      configureWorkspace();
 
-      if (const robot_model::JointModelGroup *jmg =
+  if (v == "<random valid>")
+  {
+    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 == "<current>")
-      {
-        const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
-        if (ps)
-          state = ps->getCurrentState();
-      }
-      else
-        if (v == "<same as goal>")
-        {
-          state = *planning_display_->getQueryGoalState();
-        }
-        else
-          if (v == "<same as start>")
-          {
-            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 == "<current>")
+  {
+    const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
+    if (ps)
+      state = ps->getCurrentState();
+    return;
+  }
+
+  if (v == "<same as goal>")
+  {
+    state = *planning_display_->getQueryGoalState();
+    return;
+  }
+
+  if (v == "<same as start>")
+  {
+    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)

visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui

@@ -266,6 +266,16 @@
                 </property>
                </widget>
               </item>
+              <item>
+               <widget class="QPushButton" name="stop_button">
+                <property name="enabled">
+                 <bool>false</bool>
+                </property>
+                <property name="text">
+                 <string>&amp;Stop</string>
+                </property>
+               </widget>
+              </item>
               <item>
                <widget class="QLabel" name="result_label">
                 <property name="layoutDirection">
@@ -1649,6 +1659,7 @@
   <tabstop>plan_button</tabstop>
   <tabstop>execute_button</tabstop>
   <tabstop>plan_and_execute_button</tabstop>
+  <tabstop>stop_button</tabstop>
   <tabstop>start_state_selection</tabstop>
   <tabstop>use_start_state_button</tabstop>
   <tabstop>goal_state_selection</tabstop>

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<void()> &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<void()> &job);
+
+  /// queue the execution of this function for the next time the main update() loop gets called
   void addMainLoopJob(const boost::function<void()> &job);
 
   void waitForAllMainLoopJobs();
 
-  // remove all queued jobs
+  /// remove all queued jobs
   void clearJobs();
 
   const std::string getMoveGroupNS() const;

visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp

@@ -232,6 +232,11 @@ void PlanningSceneDisplay::addBackgroundJob(const boost::function<void()> &job,
   background_process_.addJob(job, name);
 }
 
+void PlanningSceneDisplay::spawnBackgroundJob(const boost::function<void ()> &job)
+{
+  boost::thread t(job);
+}
+
 void PlanningSceneDisplay::addMainLoopJob(const boost::function<void()> &job)
 {
   boost::unique_lock<boost::mutex> ulock(main_loop_jobs_lock_);