Adding initial prototype for a MPPI trajectory validator

[SteveMacenski]

This creates a new plugin interface to MPPI which takes in the control request, optimal trajectory, and outputs if this is valid.

This is performed after the optimization process is completed to receive a final trajectory for smoothing - so we should already be applying kinematic and dynamic constraints to the output to make sure its valid in that regard. It should also be known collision free since that would trigger a soft and then hard reset if no trajectories are valid.

This can be used to validate the trajectory based on application specific constraints, constraints on control due to standards or multi-robot coordination, via point achievement, and so forth.

TODO

• Add docs for this new plugin + migration guide
• Provide a non-trivial implementation for users

On implementation, we could add in:

• Parameter for max deviation from a path
• Additional non-collision margin from obstacles
• That this trajectory makes some non-trivial progress and doesn't sit idle due to a local minimum

[SteveMacenski]

@leander-dsouza we have more mypy errors :(

[SteveMacenski]

@MarcoMatteoBassa @nourazzii take a look

[leander-dsouza]

@leander-dsouza we have more mypy errors :(

I have resolved them in #5322 :)

[nourazzii]

Just a few comments but overall looks good to me.

[codecov]

Codecov Report

Attention: Patch coverage is 79.62963% with 11 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...2_mppi_controller/optimal_trajectory_validator.hpp	75.86%	7 Missing ⚠️
nav2_mppi_controller/src/optimizer.cpp	80.95%	4 Missing ⚠️

Files with missing lines	Coverage Δ
...troller/include/nav2_mppi_controller/optimizer.hpp	100.00% <ø> (ø)
nav2_mppi_controller/src/controller.cpp	93.87% <100.00%> (+3.31%)	⬆️
...2_mppi_controller/src/critics/obstacles_critic.cpp	81.81% <100.00%> (ø)
...ectory_validators/optimal_trajectory_validator.cpp	100.00% <100.00%> (ø)
nav2_mppi_controller/src/optimizer.cpp	96.91% <80.95%> (-1.26%)	⬇️
...2_mppi_controller/optimal_trajectory_validator.hpp	75.86% <75.86%> (ø)

... and 6 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

@nourazzii I added in some implementation so this does something out of the box by default. We now can check if the optimal trajectory is in collision (which it technically could be after I thought about it for some time, since the optimal trajectory is a combination of the best scored trajectories. If you're really close to an obstacle, the averaging out of the best few could make you out of dynamic feasibility. Once that is quashed down to the valid range, that could deform the trajectory by just that slight amount that makes it invalid -- so we can check for that immediately within the local time horizon).