[WIP] Move Relative with Pilz does not reject trajectory with collision

[captain-yoshi]

It is my understanding that the MoveRelative stage min/max distance cannot be used with Pilz Industrial Motion Planner in it's default behaviour:

The min/max distance option relies on an early stop on collisions to work. Unlike MoveIt Cartesian Interpolator, the Pilz does not check for collision by default as pointed out by ros-planning/moveit/issues/2942. This results in the trajectory still containing waypoints that are in collision.

This could lead to potential collisions. Should the default behaviour of Pilz align more with the "early stop on collisions" ? If not, we should at least document this behavior in MTC. I don't think there is fullproof solution for this usecase...

I added 2 tests which demonstrates the issue and valid behaviour:

• cartesianCollisionMinMaxDistance: Set's a collision at the end of the trajectory. The plan will succeed because the minimum distance is cleared. Early stop on collisions. This test should be added in this package.
• cartesianPilzCollisionMinMaxDistance: Plan should have NOT succeeded because waypoints are in collision. No early stop on collisions (Default). Just for demonstration.

---

I will probably submit a PR to be able to modify the default behaviour for Pilz in MoveIt. Would these 3 modes make sense ?

1. Don't stop on collisions (default current behaviour)
2. Early stop on self-collisions
3. Early stop on collisions