adding final pose in analytic expansion to check (ros-navigation#4353)

quasi-robotics · Oct 22, 2024 · 62cf956 · 62cf956
1 parent a0b1a49
commit 62cf956
Showing 1 changed file with 1 addition and 1 deletion.
diff --git 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)