[MPPI] Enforce inversion MOD (NO TEST)

[doisyg]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses
Primary OS tested on	Ubuntu
Robotic platform tested on	Dexory robot

---

Description of contribution in a few bullet points

THIS IS A DRAFT
TESTS ARE DISABLED
Tested and used in production for a couple of months but doesn't necessarily work with all use cases.
Needs #3454 to work properly.

This PR adds a mechanism to MPPI to enforce global path inversions: the part of the global path taken into account by the controller will be cropped to the closest orientation inversion found on the path until the robot is within the tolerances of the inversion pose.
This new behavior of the controller is controlled by the following new parameters:

      enforce_inversion (bool)
      inversion_xy_tolerance (double)
      inversion_yaw_tolerance (double)

Default behavior:

no_enforce.mp4

With path inversion:

enforce.mp4

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[SteveMacenski]

Note to self for tomorrow; consider if we can handle this better in the critics themselves rather than in the path handler so we can process more context all at once rather than only dishing out a little at a time (which causes slow-at-goal problem to appear mid-use). This works and is probably the easiest way to go, but if we can instead have the path-related critics look at the cusp points / orientations for inverting, that would be great. That would allow us to deviate from them a little bit where optimal, but generally still do the directional changes where explicitly requested.

Its worth at least trying to see if that can be made to work with a couple days work before reverting back to this general idea, which is shown to work but provides the controller with less context to operate with

[SteveMacenski]

Commit to supersede in your inbox shortly for testing.