Multi-controller trajectories are not canceled when one controller aborts.

Hugal31 commented 2 years ago

Description

When executing multi-controller trajectories, if one controller aborts (e.g. due to a path constraint violation), the other controllers are not stopped.

Example of use case: I have a UR5 mounted on an elevator. Sometimes, the elevator fails to remain within the trajectory error tolerance, and the elevator controller (a joint_trajectory_controller) stops with the error PATH_TOLERANCE_VIOLATED. However, the arm continue its trajectory.

Your environment

ROS Distro: Noetic
OS Version: Ubuntu 20.04
Binary build 1.1.9-1focal.20220519.091055

Steps to reproduce

Plan and execute a trajectory needing more than one controller.
Make one controller fails

Expected behaviour

When one controller enter in error, such as PATH_TOLERANCE_VIOLATED for the joint_trajectory_controller, the other controllers should be stopped.

Actual behaviour

The other controllers continue to execute their trajectory. This can led to collision, among other things.

Backtrace or Console output

Notice the time between the elevator and the arm controllers result (the two trajectories were supposed to finish at the same time).

[ WARN] [1660639215.768143761]: Controller 'elevator_trajectory_controller' failed with error PATH_TOLERANCE_VIOLATED:
[ INFO] [1660639220.168192807]: Controller 'arm_trajectory_controller' successfully finished
[ WARN] [1660639220.230077031]: Controller handle elevator_trajectory_controller reports status ABORTED
[ INFO] [1660639220.230108729]: Completed trajectory execution with status ABORTED ...

rhaschke commented 2 years ago

@cambel can you comment on this? Looks like you became the expert of the execution manager :wink:

v4hn commented 2 years ago

Again one of these things that make me respond like "what???" after years of working with the code. This is not implemented at all. If sending fails (which is something that cannot happen with the standard MoveIt configuration) it aborts other trajectories, but afterwards it only (optionally) waits for all controllers in order. There is no event handling at all, not to talk about error conditions. So yes, this is a critical bug, that can only be fixed with quite a few logic modifications in TEM.

As @cambel rewrites the class anyway in their GSoC project #3156 (ending in November) he just promised me that the new version will not have that problem anymore. Unless someone independent wants to spent time on this before (and can show it works well) I would propose to leave this issue open until then.

moveit / moveit