Enable reaction to planner failure in the planner logic

[sjahr]

Description

I've addressed the inability to react in the logic to a failure of the global/local planner action by adding additional Hybrid Planning events. This way it is possible to define the reaction to those events in the planner logic plugin and make it easy to customize this behavior.
Additionally, I fixed the issue @JensVanhooydonck mentioned in moveit#763 that it is not possible to send multiple consecutive hybrid planning requests by simply adding a bool flag.

@AndyZe and @JensVanhooydonck can you verify that this works?

Checklist

• Required by CI: Code is auto formatted using clang-format

[JensVanhooydonck]

Multiple consecutive hybrid planning requests seem to work.
However, when the Collision ahead, hold current possition is printed, and the global planner does not find a solution anymore, there is an infinity loop.

The is no maximum amount of retries or timeout anymore when using the hybrid planner.

[AndyZe]

However, when the Collision ahead, hold current possition is printed, and the global planner does not find a solution anymore, there is an infinity loop.

The is no maximum amount of retries or timeout anymore when using the hybrid planner.

@JensVanhooydonck I think it's important to note, this is a demo. It's not really meant to be perfectly polished and capable of handling all edge cases.

With that said, I think the way to handle repeated global planning failures is similar to this commit. Basically it added a failure count for the local planner and aborts if STUCK_ITERATIONS_THRESHOLD is exceeded. I think you could do something really similar in the global planner plugin (moveit_planning_pipeline.cpp).

Is that the way you would suggest to handle repeated global failures, @sjahr?

[sjahr]

possition

@JensVanhooydonck thanks a lot for verifying that this works!
I cannot reproduce the error you mentioned in my setup. But for me, the global planner also needs to rerun multiple times till a feasible global solution is found.
Just to clarify this: Does the global planner fail to find a solution or does the new global trajectory also drive the robot into the collision object (which would cause the local planner to stop and retrigger the global planner again)? In the first case, the whole hybrid planning should fail with these demo logic plugins because if the global planner fails to find a solution, the action is not successful and GLOBAL_PLANNING_ACTION_ABORTED is triggered, which should cause the whole hybrid planning action to fail.
The latter case is desired behavior because this setup reacts to an invalidated global trajectory by "brute-force" retriggering the global planner till a valid trajectory is found. This works in some use cases and with a non-deterministic global planner but there is no guarantee that a valid solution is found, especially in more complex planning environments.

[AndyZe]

Thanks @sjahr! I'll merge it. If @JensVanhooydonck still has an issue we can fix it in the main PR or do a followup PR later.