Tony Najjar

> do you see this with Nav2 on main without your custom changes I tried on the latest main now with no changes in MPPI package and still reproduces. It...

> I only had a quick look as I am traveling. I think keeping the optimal trajectory visualized and distinguished is important, as it's the most useful part. Ignore this...

Thanks for the review, will polish it tomorrow

@SteveMacenski I addressed many of your comments but additionally made many more changes. Can you give it a general look? There are probably a bunch of nitpicks left. For now...

> For now I dropped visualizing in-collision trajectories with another color because I think that would require adding an additional vector in CriticsData that tracks which trajectories are in-collision. Would...

I'm also passively working on this internally since a couple of days but I'm not sure yet if the solution will be cleanly-architectured enough to be contributed back. I'll know...

``` [bt_navigator] [WARN] Node timed out while executing service call to local_costmap/clear_entirely_local_costmap ``` The server callback needs to acquire the costmap lock before clearing it so this could be because...

> Cost Critic doesn't penalize when rear part of footprint is getting closer to the obstacle during presented rotation. This is the expected behavior currently. From the the lines you...

@Jakubach can you test out with https://github.com/ros-navigation/navigation2/pull/5672? You might need to tune `near_collision_cost` (loosely, the smaller the number, the bigger the distance to the obstacle) and `critical_cost` (the bigger the...

You might benefit from this PR that adds critic-based visualizations so you can see what your CostCritic is doing, and projected optimal footprints to see if MPPI's optimal path is...