Preempt trajectory execution if one controller aborts

[Martin-Oehler]

Description

In the current trajectory execution handling, if one goal aborts, the other goals are executed regardless. An aborted goal is likely due to a malfunction of the respective controller/hardware. A collision-free execution can no longer be guaranteed and a further execution of the trajectory is very unsafe. Therefore, I suggest to monitor the state of all controllers and preempt all goals if one goal is aborted.

This issue is related to aborted goal handling of the joint_trajectory_controller ros-controls/ros_controllers#395

I implemented the necessary changes here. I am not sure if further changes are required, it works in my setup. If there is interest to merge this, I will open a PR.

[welcome]

Thanks for reporting an issue. We will have a look asap. If you can think of a fix, please consider providing it as a pull request.

[rhaschke]

Thanks for reporting this issue. I agree, that trajectory execution shouldn't continue when one controller aborts. Unfortunately, MoveIt doesn't provide an API in ControllerHandles to report status changes via callbacks. For this reason you had to implement an ugly busy loop.
But, yes, please file this as a PR.

[Martin-Oehler]

Unfortunately, MoveIt doesn't provide an API in ControllerHandles to report status changes via callbacks.

I also looked into this, but adding callbacks would require a lot of changes. Maybe it could be refactored at some point.

For this reason you had to implement an ugly busy loop.

It is ugly, I agree. But since it has been a busy loop before, not much changed in that regard.

But, yes, please file this as a PR.

Ok, I will do that. I am leaving this open for now for further discussion.

[rhaschke]

What about launching threads for all controller handles to perform the actual waitForExecution() call. These threads can then asynchronously notify the main function about state changes.

Btw, before it wasn't necessarily a busy loop. All implementations of waitForExecution() in MoveIt were not busy looping.

[Martin-Oehler]

What about launching threads for all controller handles to perform the actual waitForExecution() call.

That would be a possibility. However, we could also directly poll the states of the controllers using getLastExecutionStatus() and not use waitForExecution() at all. I think that might be even better as you can directly get the state of controllers without the need for additional threads.

[rhaschke]

Yes, I think you are right.