diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp
index 10420094faf..3222042a6f4 100644
--- a/nav2_util/include/nav2_util/geometry_utils.hpp
+++ b/nav2_util/include/nav2_util/geometry_utils.hpp
@@ -117,7 +117,7 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal)
  */
 inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t start_index = 0)
 {
-  if (start_index >= path.poses.size() - 1) {
+  if (start_index + 1 >= path.poses.size()) {
     return 0.0;
   }
   double path_length = 0.0;