diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
index 60e10398265..ce823210aa2 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -297,8 +297,8 @@ bool AStarAlgorithm<NodeT>::areInputsValid()
   // Check if ending point is valid
   if (getToleranceHeuristic() < 0.001) {
     // if any goal is valid, then all goals are valid
-    auto first_goal = *_goalsSet.begin(); // Get the first goal from _goalsSet
-    if (!first_goal->isNodeValid(_traverse_unknown, _collision_checker)) {
+    auto goal = *_goalsSet.begin(); 
+    if (!goal->isNodeValid(_traverse_unknown, _collision_checker)) {
       throw nav2_core::GoalOccupied("Goal was in lethal cost");
     }
   }
@@ -386,6 +386,7 @@ bool AStarAlgorithm<NodeT>::createPath(
     current_node->visited();
 
     // 2.1) Use an analytic expansion (if available) to generate a path
+    expansion_result = nullptr;
     expansion_result = _expander->tryAnalyticExpansion(
       current_node, getGoals(),
       getInitialGoalCoordinate(), neighborGetter, analytic_iterations, closest_distance);
@@ -477,7 +478,7 @@ float AStarAlgorithm<NodeT>::getHeuristicCost(const NodePtr & node)
     NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3());
   // Heurittic cost for more than one goal is already cotemplated in the
   // precomputation stage
-  float heuristic = NodeT::getHeuristicCost(node_coords, _goals_coordinates[0]);
+  float heuristic = NodeT::getHeuristicCost(node_coords, getInitialGoalCoordinate());
   if (heuristic < _best_heuristic_node.first) {
     _best_heuristic_node = {heuristic, node->getIndex()};
   }