From 104a914a55ece165909687bce592c644d2b7c5e6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 20 Nov 2025 15:59:42 +0100 Subject: [PATCH] findPathFurthestReachedPoint Signed-off-by: Tony Najjar --- .../include/nav2_mppi_controller/tools/utils.hpp | 7 ++++++- 1 file changed, 6 insertions(+), 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 4a3cc6b7c06..ada40729968 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -304,7 +304,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) for (size_t i = 0; i != n_rows; i++) { min_id_by_path = 0; min_distance_by_path = std::numeric_limits::max(); - for (size_t j = max_id_by_trajectories; j != n_cols; j++) { + for (size_t j = 0; j != n_cols; j++) { const float cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { min_distance_by_path = cur_dist; @@ -312,6 +312,11 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) } } max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path); + + // Early exit if we've already reached the end of the path + if (max_id_by_trajectories == static_cast(n_cols) - 1) { + break; + } } return max_id_by_trajectories; }