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

quasi-robotics · Mar 22, 2024 · 91c0cb0 · 91c0cb0
1 parent 76ec075
commit 91c0cb0
Showing 1 changed file with 1 addition and 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
@@ -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) {