Tracking critics dwb and mppi

[silanus23]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5037
Primary OS tested on	Ubuntu
Robotic platform tested on	nav2_bringup
Does this PR contain AI generated software?	util tests
Was this PR description generated by AI software?	nope definitely not

---

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Critic additons and their parameter additions

Description of how this change was tested

Added util tests. I have succeded to create cases where turtlebot has to create a new path cause it's so out of bounds(done this on dwb).

---

Future work that may be required in bullet points

These can be added to default yaml

###Note:
I had to create a new branch cause the original branch was sub branch of my first PR. I had to carry it via copy paste couldn't find a better way.

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.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
• Should this be backported to current distributions? If so, tag with backport-*.

[SteveMacenski]

Have you tested these critics in some way - in particular for MPPI? The TB4 world with some strategically placed obstacles in the way might be a good test case. The MPPI critic looks pretty complex. I would have intuitively started with something that simply:

• Evaluates the distance from the path segments (using the utility function already merged) to a given trajectory sample. If outside, then penalize highly.
• As an add on feature, add the option to also have a penalty that scales with the distance away from the path (with some margin before application so we don't overscore based on small adjustments).

I suspect (?) that it may be possible to instead strategically place this logic into the Path Align Critic and/or share some of its computations to make that critic more simple and concise. Is it possible to surgically place this into the Path Align logic and/or some of the computation is shared? Either we can share some functions and/or data, or put the Path Hug scoring inside of that critic as additional weight penalties (if we already have path-trajectory distances computed for its own scoring)

[silanus23]

@SteveMacenski I didn't try to get full results. I tried to solve the problem of the Planner can not keep up with the HZ (this part needed optimizations making this PR a lot complex). PathAlign integration is actually sounds a lot better than my solution.In addition PathFollow can be considered too. But I have to look it to be sure. There are plenty of path mppi critic that uses sampling strides etc. I don't know if it's too daring but maybe unification of path critics and keeping both versions.
BTW realized some of my additions to dwb after testing (ones I shared as visuals in previous PR) causing some problems took them back and will try to get visual results in this version too.
Regardless of the solution my initial proposal is deleting everything about MPPI. Making this PR only about DWB and handle MPPI in another sperate PR. What do you think about this approach?

[SteveMacenski]

I think that makes sense - though I think MPPI is much higher priority than DWB as the default controller. DWB is more for legacy users.