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);