critical fix for path tracking MPPI (#4099)

SteveMacenski · web-flow · commit 8f7b6ceef943 · 2024-02-05T18:40:50.000-08:00
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) {

Original file line number	Diff line number	Diff line change
`@@ -698,7 +698,7 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)`
`698`	`698`	`inline size_t findClosestPathPt(const std::vector<float> & vec, float dist, size_t init = 0)`
`699`	`699`	`{`
`700`	`700`	`auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist);`
`701`		`- if (iter == vec.begin()) {`
	`701`	`+ if (iter == vec.begin() + init) {`
`702`	`702`	`return 0;`
`703`	`703`	`}`
`704`	`704`	`if (dist - (iter - 1) < iter - dist) {`