[MPPI] empty path_follow_critic proper fix (#3599)

* [MPPI] empty path_follow_critic proper fix * fix linting issue --------- Co-authored-by: Guillaume Doisy <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
ros-navigation · Jun 9, 2023 · 8edb367 · 8edb367
1 parent 8b72277
commit 8edb367
Showing 1 changed file with 2 additions and 3 deletions.
diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp
@@ -34,16 +34,15 @@ 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;
   }
 
   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);