From b749f79df801bf6dfa5eda5159e7c0b217576543 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 5 Feb 2024 18:40:50 -0800 Subject: [PATCH] critical fix for path tracking MPPI (#4099) (cherry picked from commit 8f7b6ceef943c99497c39da341219a67a4796162) --- .../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 ab860428113..a31ce891058 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/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 & 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) {