From 91c0cb06f21422e9552184bd349755328ab73515 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Mon, 5 Feb 2024 18:40:50 -0800
Subject: [PATCH] critical fix for path tracking MPPI (#4099)

---
 .../include/nav2_mppi_controller/tools/utils.hpp                | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
index 4a67bc276cc..fb22b38b248 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -698,7 +698,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) {