Stop execution on motion planning rviz plugin #709

wkentaro · 2016-07-10T08:05:24Z

davetcoleman · 2016-07-10T17:57:26Z

Sounds cool - can you provide a screenshot of the GUI change?

wkentaro · 2016-07-10T19:19:36Z

I added a GIF.

davetcoleman · 2016-07-10T20:03:56Z

visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

@@ -129,15 +135,31 @@ void MotionPlanningFrame::computePlanButtonClicked()
 void MotionPlanningFrame::computeExecuteButtonClicked()
 {
  if (move_group_ && current_plan_)
-    move_group_->execute(*current_plan_);
+    ui_->stop_button->setEnabled(true);
+    move_group_->asyncExecute(*current_plan_);


you need brackets around these two lines

wkentaro · 2016-07-10T21:31:49Z

Oops, I sent the wip code. Fixed.

davetcoleman · 2016-07-10T21:47:55Z

+1, looks great!

rhaschke · 2016-07-11T06:28:57Z

visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

 }

 void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
 {
  if (!move_group_)
    return;
  configureForPlanning();
-  move_group_->move();
+  ui_->stop_button->setEnabled(true);
+  move_group_->asyncMove();


Changing from synchronous to asynchronous command here, immediately (re)enables the "plan+execute" button.
This changes behaviour. Before, the button was enabled only when move() finished.

Maybe reenabling the button after the execution is necessary?

The button should be only re-enabled after execution. Because you switch to asynchronous execution now, the button gets re-enabled immediately.

rhaschke · 2016-07-11T06:33:38Z

Nice feature. Have a look at the inline comments.

rhaschke · 2016-07-13T11:00:53Z

Hm. While your recent code will fix the enabling of the stop button, I think the code is rather complicated.
Why do you need to use asynchronous execution at all? The execute() and move() calls were already executed within a separate thread of rviz. Hence, there is no problem to synchronously wait for their end.
Did you tried that?

wkentaro · 2016-07-13T20:37:29Z

Did you tried that?

Yeah, I have tried move() calls and tried to pushing the stop button, but the joint trajectory action could not be canceled.

rhaschke · 2016-07-14T21:34:05Z

visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp

@@ -129,15 +137,28 @@ void MotionPlanningFrame::computePlanButtonClicked()
 void MotionPlanningFrame::computeExecuteButtonClicked()
 {
  if (move_group_ && current_plan_)
-    move_group_->execute(*current_plan_);


Replacing move_group_->execute() with move_group_->asyncMove() is not possible, because behaviour is changed: While the former executes a previously planned, manually validated trajectory, the latter will actually plan a new trajectory and directly execute it - with no chance for inspection/validation.

I cannot accept this PR in this form.

Looking deeper into the reasons, I found the following: There are essentially to methods to plan/execute:

move_group::plan() and move_group::move() are calls to an action, that can be interrupted. The flag plan_only allows to switch off/on execution.

However, move_group::execute() is a call to a service (MoveGroupExecuteService), which cannot be interrupted.

In order to allow stopping of a trajectory, the service should be turned into an action as well. As far as I can see, that changed could be hidden transparently behind the existing MoveGroupInterface. However, I'm not sure, whether some (3rd-party) code is directly calling the service. Within the MoveIt code base, I couldn't find any other usage.

@davetcoleman Do you think, this change is reasonable?

Alternatively, we do not allow to stop an executed-only trajectory.

In order to allow stopping of a trajectory, the service should be turned into an action as well.

Precisely speaking, for stopping of a trajectory, the service call with wait_for_execution:=false and stop() method call is enough. The reason I needed to change the execute() method to move() is to get the trajectory execution server status to handle GUI button status correctly in 5503fa9.

Replacing move_group_->execute() with move_group_->asyncMove() is not possible, because behaviour is changed: While the former executes a previously planned, manually validated trajectory, the latter will actually plan a new trajectory and directly execute it - with no chance for inspection/validation.

I see.

the service should be turned into an action as well.

I agree with the change.

Precisely speaking, for stopping of a trajectory, the service call
with |wait_for_execution:=false| and |stop()| method call is enough.
The reason I needed to change the |execute()| method to |move()| is to
get the trajectory execution server status to handle GUI button status
correctly in 5503fa9
The service call doesn't have a stop() method, does it? I guess you are
intermixing with the move action?

@wkentaro: Could you propose a PR (w.r.t. #713) for turning the execute service into an action?
Dave agreed on that approach, but I don't have time for it right now.
I think, having that done too, we can merge this branch.

The service call doesn't have a stop() method, does it? I guess you are
intermixing with the move action?

I don't use stop() method of the move action, but use stop() method in move_group with trajectory_event_manager.
https://github.com/ros-planning/moveit_ros/blob/kinetic-devel/planning_interface/move_group_interface/src/move_group.cpp#L763
https://github.com/ros-planning/moveit_ros/blob/kinetic-devel/planning_interface/move_group_interface/src/move_group.cpp#L130

I think the execution can be interrupted if wait_for_execution=false for execution service, with stop() calling in move_group.

Indeed. That looks promising. Could you prepare a PR for this?

Indeed. That looks promising. Could you prepare a PR for this?

This PR without last commit is that approach. The trajectory execution with execute_button on rviz plugin was able to be interrupted with asyncExecute() and move_group_->stop().
However, it is unable to get the status of execution of service call, so I could not handle the GUI button status, enable/disable, correctly.
That's why I needed to change the asyncExecute() to asyncMove() in the last commit, in order to handle the enable/disable status of GUI precisely.

@wkentaro Doesn't work like you described. Of course, using asynchronous execution (wait_for_execution=false) will allow the stop call to get through and stop motion.
However, asynchronous execution through a service doesn't allow to get feedback when execution finished. We need to turn the service into an action.

Yeah. You're right. I meant it does not work.

rhaschke · 2016-07-15T08:27:02Z

If you agree, please close this PR in favor of #713.

rhaschke · 2016-07-17T10:02:17Z

Closed in favor of #713.

Stop execution on motion planning rviz plugin

c7f56d7

davetcoleman reviewed Jul 10, 2016
View reviewed changes

Fix style and simplify to stop execution on rviz

fbdf7e9

rhaschke reviewed Jul 11, 2016
View reviewed changes

Handle enable/disable GUI button state correctly with goal state

5503fa9

rhaschke reviewed Jul 14, 2016
View reviewed changes

rhaschke mentioned this pull request Jul 15, 2016

rviz plugin: stop execution + update of start state to current #713

Merged

rhaschke closed this Jul 17, 2016

wkentaro mentioned this pull request Jul 18, 2016

Add capability of execute known trajectory with a ROS action #719

Closed

wkentaro deleted the stop-execution branch August 17, 2016 13:11

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Stop execution on motion planning rviz plugin #709

Stop execution on motion planning rviz plugin #709

wkentaro commented Jul 10, 2016 •

edited

Loading

davetcoleman commented Jul 10, 2016

wkentaro commented Jul 10, 2016

davetcoleman Jul 10, 2016

wkentaro commented Jul 10, 2016

davetcoleman commented Jul 10, 2016

rhaschke Jul 11, 2016

wkentaro Jul 11, 2016

rhaschke Jul 11, 2016

wkentaro Jul 13, 2016

rhaschke commented Jul 11, 2016

rhaschke commented Jul 13, 2016

wkentaro commented Jul 13, 2016

rhaschke Jul 14, 2016

wkentaro Jul 15, 2016

wkentaro Jul 15, 2016

rhaschke Jul 15, 2016 •

edited

Loading

rhaschke Jul 15, 2016

wkentaro Jul 15, 2016

rhaschke Jul 15, 2016

wkentaro Jul 15, 2016 •

edited

Loading

rhaschke Jul 15, 2016

wkentaro Jul 16, 2016

rhaschke commented Jul 15, 2016

rhaschke commented Jul 17, 2016

Stop execution on motion planning rviz plugin #709

Stop execution on motion planning rviz plugin #709

Conversation

wkentaro commented Jul 10, 2016 • edited Loading

davetcoleman commented Jul 10, 2016

wkentaro commented Jul 10, 2016

Choose a reason for hiding this comment

wkentaro commented Jul 10, 2016

davetcoleman commented Jul 10, 2016

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke commented Jul 11, 2016

rhaschke commented Jul 13, 2016

wkentaro commented Jul 13, 2016

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke Jul 15, 2016 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

wkentaro Jul 15, 2016 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke commented Jul 15, 2016

rhaschke commented Jul 17, 2016

wkentaro commented Jul 10, 2016 •

edited

Loading

rhaschke Jul 15, 2016 •

edited

Loading

wkentaro Jul 15, 2016 •

edited

Loading