critical fix for path tracking MPPI (ros-navigation#4099) (ros-naviga…

…tion#4101)

(cherry picked from commit 8f7b6ce)

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -673,7 +673,7 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
 inline size_t findClosestPathPt(const std::vector<float> & vec, float dist, size_t init = 0)
 {
   auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist);
-  if (iter == vec.begin()) {
+  if (iter == vec.begin() + init) {
     return 0;
   }
   if (dist - *(iter - 1) < *iter - dist) {