diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
index 0eecb78185e..94e9ddfbb5f 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
@@ -119,7 +119,7 @@ class AnalyticExpansion
    * @return The score of the refined path
    */
   float refineAnalyticPath(
-    const NodePtr & current_node,
+    NodePtr & node,
     const NodePtr & goal_node,
     const NodeGetter & getter,
     AnalyticExpansionNodes & analytic_nodes);
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index da46f781975..774454c216a 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -66,6 +66,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
 
     AnalyticExpansionNodes current_best_analytic_nodes = {};
     NodePtr current_best_goal = nullptr;
+    NodePtr current_best_node = nullptr;
     float current_best_score = std::numeric_limits<float>::max();
 
     closest_distance = std::min(
@@ -97,13 +98,15 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
           current_node->motion_table.state_space);
         if (!analytic_nodes.empty()) {
           found_valid_expansion = true;
-          bool score = refineAnalyticPath(
-            current_node, current_goal_node, getter, analytic_nodes);
+          NodePtr node = current_node;
+          float score = refineAnalyticPath(
+            node, current_goal_node, getter, analytic_nodes);
           // Update the best score if we found a better path
           if (score < current_best_score) {
             current_best_analytic_nodes = analytic_nodes;
             current_best_goal = current_goal_node;
             current_best_score = score;
+            current_best_node = node;
           }
         }
       }
@@ -116,13 +119,15 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
             current_node, current_goal_node, getter,
             current_node->motion_table.state_space);
           if (!analytic_nodes.empty()) {
-            bool score = refineAnalyticPath(
-              current_node, current_goal_node, getter, analytic_nodes);
+            NodePtr node = current_node;
+            float score = refineAnalyticPath(
+              node, current_goal_node, getter, analytic_nodes);
             // Update the best score if we found a better path
             if (score < current_best_score) {
               current_best_analytic_nodes = analytic_nodes;
               current_best_goal = current_goal_node;
               current_best_score = score;
+              current_best_node = node;
             }
           }
         }
@@ -131,7 +136,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
 
     if (!current_best_analytic_nodes.empty()) {
       return setAnalyticPath(
-        current_node, current_best_goal,
+        current_best_node, current_best_goal,
         current_best_analytic_nodes);
     }
     analytic_iterations--;
@@ -273,12 +278,11 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
 
 template<typename NodeT>
 float AnalyticExpansion<NodeT>::refineAnalyticPath(
-  const NodePtr & current_node,
+  NodePtr & node,
   const NodePtr & goal_node,
   const NodeGetter & getter,
   AnalyticExpansionNodes & analytic_nodes)
 {
-  NodePtr node = current_node;
   NodePtr test_node = node;
   AnalyticExpansionNodes refined_analytic_nodes;
   for (int i = 0; i < 8; i++) {
@@ -407,7 +411,7 @@ getAnalyticPath(
 
 template<>
 float AnalyticExpansion<Node2D>::refineAnalyticPath(
-  const NodePtr &,
+  NodePtr &,
   const NodePtr &,
   const NodeGetter &,
   AnalyticExpansionNodes &)
diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
index 7338f4e43c0..16fb3882849 100644
--- a/nav2_smac_planner/test/test_a_star.cpp
+++ b/nav2_smac_planner/test/test_a_star.cpp
@@ -141,7 +141,9 @@ TEST(AStarTest, test_a_star_2d)
   int dummy_int2 = 0;
   EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {},
   nullptr, dummy_int1, dummy_int2), nullptr);
-  EXPECT_EQ(expander.refineAnalyticPath(nullptr, nullptr, nullptr,
+
+  nav2_smac_planner::Node2D* start = nullptr;
+  EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr,
     analytic_expansion_nodes), std::numeric_limits<float>::max());
   nav2_smac_planner::AnalyticExpansion<nav2_smac_planner::Node2D>::AnalyticExpansionNodes
     expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr);