Bug report

In MPPI utils.hpp, function findPathFurthestReachedPoint()

https://github.com/ros-planning/navigation2/blob/654e7e0d39c515612713c7426a30b2a219ae477a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp#L315-L326

I tried to understand what it is going on here: in the inner for loop, it find an index jon the path, which has smallest distance to the end point of the current trajectory i, meanwhile, the outter for loop iterate over all the trajectories, and find a path point id that is largest over all the trajectories.

It appears to me that in the outter loop, after min_id_by_path = 0, the min_distance_by_path should also be reset to max; otherwise each time in the inner loop, the min_distance_by_path will depend on the previous iterated trajectories. And the final result will then depend on the order of trajectories in the sampled set, which doesn't sound quite right.