From 8edb3673c39a7b15e5a9895bf47ea4c1189e897a Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 6 Jun 2023 19:24:33 +0100 Subject: [PATCH] [MPPI] empty path_follow_critic proper fix (#3599) * [MPPI] empty path_follow_critic proper fix * fix linting issue --------- Co-authored-by: Guillaume Doisy Co-authored-by: Steve Macenski --- nav2_mppi_controller/src/critics/path_follow_critic.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 40b8c4aa9f..e440bd0b3e 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -34,9 +34,7 @@ void PathFollowCritic::initialize() void PathFollowCritic::score(CriticData & data) { - const size_t path_size = data.path.x.shape(0) - 1; - - if (!enabled_ || path_size == 0 || + if (!enabled_ || data.path.x.shape(0) < 2 || utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { return; @@ -44,6 +42,7 @@ void PathFollowCritic::score(CriticData & data) utils::setPathFurthestPointIfNotSet(data); utils::setPathCostsIfNotSet(data, costmap_ros_); + const size_t path_size = data.path.x.shape(0) - 1; auto offseted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, path_size);