From 1a12e4f8f98c4e1a65939df8921d75e49c88d7a4 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Thu, 23 May 2024 08:21:02 -0700
Subject: [PATCH] adding final pose in analytic expansion to check (#4353)

---
 nav2_smac_planner/src/analytic_expansion.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index f2667d3ba8..455a6e2a21 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -210,7 +210,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   node_costs.reserve(num_intervals);
 
   // Check intermediary poses (non-goal, non-start)
-  for (float i = 1; i < num_intervals; i++) {
+  for (float i = 1; i <= num_intervals; i++) {
     state_space->interpolate(from(), to(), i / num_intervals, s());
     reals = s.reals();
     // Make sure in range [0, 2PI)