From fc0df4a97e63fac1173e0ab1993fef22c26262e2 Mon Sep 17 00:00:00 2001
From: stevedan <stevedan.o.omodolor@gmail.com>
Date: Tue, 12 Mar 2024 07:38:48 +0100
Subject: [PATCH] move analytic check outside loop

---
 .../include/nav2_smac_planner/a_star.hpp      |  2 +-
 nav2_smac_planner/src/a_star.cpp              |  8 ++---
 nav2_smac_planner/src/analytic_expansion.cpp  | 31 +++++++++----------
 3 files changed, 20 insertions(+), 21 deletions(-)

diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
index 4a9273f6958..cd405330ca9 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -274,7 +274,7 @@ class AStarAlgorithm
   NodePtr _start;
   NodeVector _goals;
   NodeSet _goalsSet;
-  GoalHeadingMode _current_goal_heading_mode;
+  GoalHeadingMode _goal_heading_mode;
 
   Graph _graph;
   NodeQueue _queue;
diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
index dcced09459d..6dd37a64ce7 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -71,8 +71,8 @@ void AStarAlgorithm<NodeT>::initialize(
   _max_on_approach_iterations = max_on_approach_iterations;
   _terminal_checking_interval = terminal_checking_interval;
   _max_planning_time = max_planning_time;
-  _current_goal_heading_mode = goal_heading_mode;
-  NodeT::setGoalHeadingMode(_current_goal_heading_mode);
+  _goal_heading_mode = goal_heading_mode;
+  NodeT::setGoalHeadingMode(_goal_heading_mode);
   NodeT::precomputeDistanceHeuristic(
     lookup_table_size, _motion_model, dim_3_size, _search_info);
   _dim3_size = dim_3_size;
@@ -96,7 +96,7 @@ void AStarAlgorithm<Node2D>::initialize(
   _max_on_approach_iterations = max_on_approach_iterations;
   _terminal_checking_interval = terminal_checking_interval;
   _max_planning_time = max_planning_time;
-  _current_goal_heading_mode = goal_heading_mode;
+  _goal_heading_mode = goal_heading_mode;
 
   if (dim_3_size != 1) {
     throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization.");
@@ -215,7 +215,7 @@ void AStarAlgorithm<NodeT>::setGoal(
   CoordinateVector goals_coordinates;
   unsigned int num_bins = NodeT::motion_table.num_angle_quantization;
   unsigned int dim_3_half_bin = 0;
-  switch (_current_goal_heading_mode) {
+  switch (_goal_heading_mode) {
     case GoalHeadingMode::DEFAULT:
       _goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
       goals_coordinates.push_back(
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index 2dc5f0517b4..976412d97e1 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -65,24 +65,23 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     AnalyticExpansionNodes current_best_analytic_nodes = {};
     unsigned int current_best_goal_index = 0;
     NodePtr current_best_node = nullptr;
-    for (unsigned int g = 0; g < goals_node.size(); g++) {
-      closest_distance = std::min(
+    closest_distance = std::min(
         closest_distance,
         static_cast<int>(NodeT::getHeuristicCost(node_coords, goals_node[0]->pose)));
-      // We want to expand at a rate of d/expansion_ratio,
-      // but check to see if we are so close that we would be expanding every iteration
-      // If so, limit it to the expansion ratio (rounded up)
-      int desired_iterations = std::max(
-        static_cast<int>(closest_distance / _search_info.analytic_expansion_ratio),
-        static_cast<int>(std::ceil(_search_info.analytic_expansion_ratio)));
-
-      // If we are closer now, we should update the target number of iterations to go
-      analytic_iterations =
-        std::min(analytic_iterations, desired_iterations);
-
-      // Always run the expansion on the first run in case there is a
-      // trivial path to be found
-      if (analytic_iterations <= 0) {
+    // We want to expand at a rate of d/expansion_ratio,
+    // but check to see if we are so close that we would be expanding every iteration
+    // If so, limit it to the expansion ratio (rounded up)
+    int desired_iterations = std::max(
+      static_cast<int>(closest_distance / _search_info.analytic_expansion_ratio),
+      static_cast<int>(std::ceil(_search_info.analytic_expansion_ratio)));
+    // If we are closer now, we should update the target number of iterations to go
+    analytic_iterations =
+      std::min(analytic_iterations, desired_iterations);
+    
+    // Always run the expansion on the first run in case there is a
+    // trivial path to be found
+    if (analytic_iterations <= 0) {
+      for (unsigned int g = 0; g < goals_node.size(); g++) {
         // Reset the counter and try the analytic path expansion
         analytic_iterations = desired_iterations;
         AnalyticExpansionNodes analytic_nodes =