45% performance improvement in MPPI

[SteveMacenski]

From 12.7ms on average to 6.80ms on average for 10000 cycles - with full footprint checking enabled

---

Validate final run-time and system performance of changes to make sure still high functioning with changes

• Cost - OK avoiding obstacles and dynamic obstacles (@BriceRenaudeau ?)
• Path Align - OK aligning effectively with the path and alongside other critics
• Path Angle - OK checking only end in a broad set of situations?
• High level Behavior - OK (retune?)

---

• Increase default trajectory_point_step to 4 for path align
• Document + default for trajectory_point_step for cost critic
• Move out legacy Path Align
• docs, lint updating docs for mppi_opt docs.nav2.org#533
• Tests update

[jjd9]

Hello, I did not find any glaring issues, but I found some small things I thought were worth pointing out. Thanks!

[aosmw]

Hi Steve,

Do you know about this "pitfall"? https://xtensor.readthedocs.io/en/latest/pitfall.html#random-numbers-not-consistent
Using a random number function from xtensor actually returns a lazy generator. That means, accessing the same element of a random number generator does not give the same random number if called twice.

Is there a speed advantage if the xt::random::randn operations in NoiseGenerator::generateNoisedControls are wrapped in xt::eval?

regards
Mikew

[SteveMacenski]

That's a good question. I can look into that.

Edit: That is not relevent since we set to noises_vx_ (and analog) which is an xtensor object. That evaluates on construction, but just to verify I added eval and it made no difference. It actually made me wonder if this would be faster if we did use lazy evaluation, so I moved some of the sampling into the main function so that we could take advantage of lazy evaluation and it made statistically no meaningful improvement (but seems slightly worse, but within error bounds. It mostly added more jitter, which I think is why we originally did it this way if my foggy memory serves).

However that attribute actually calls something interesting into question. Do we need all 3 noise distributions if we lazy evaluate them? if calling multiple times isn't the same value, we could use a lazy expression to create 1 noise sample and then use it for all 3 axes. If we hard evaluate it, then we can't do that since the same noises would be used, but if we used the lazy evaulation, that could make an impact.

That doesn't actually speed things up for us unfortunately since we're still re-evaluating 3x56x2000 samples no matter how we cut it in memory (and with the added problem of no longer having control over the std per axis).

Overall, I still don't see a way to optimize this further unforunately - which is of annoyance to me as well. I feel like this shouldn't be as heavy as it is.

[mergify]

@SteveMacenski, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@SteveMacenski, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).