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 a3ae1f6c7b2..83b9aa59673 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -16,12 +16,15 @@
 #ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_
 #define NAV2_SMAC_PLANNER__A_STAR_HPP_
 
+#include <vector>
+#include <iostream>
+#include <unordered_map>
+#include <unordered_set>
 #include <functional>
 #include <memory>
 #include <queue>
 #include <tuple>
 #include <utility>
-#include <vector>
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_core/planner_exceptions.hpp"
@@ -49,6 +52,7 @@ class AStarAlgorithm
   typedef NodeT * NodePtr;
   typedef robin_hood::unordered_node_map<uint64_t, NodeT> Graph;
   typedef std::vector<NodePtr> NodeVector;
+  typedef std::unordered_set<NodePtr> NodeSet;
   typedef std::pair<float, NodeBasic<NodeT>> NodeElement;
   typedef typename NodeT::Coordinates Coordinates;
   typedef typename NodeT::CoordinateVector CoordinateVector;
@@ -90,6 +94,8 @@ class AStarAlgorithm
    * or planning time exceeded
    * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns
    * false after this timeout
+   * @param lookup_table_size Size of the lookup table to store heuristic values
+   * @param dim_3_size Number of quantization bins
    */
   void initialize(
     const bool & allow_unknown,
@@ -129,7 +135,8 @@ class AStarAlgorithm
   void setGoal(
     const float & mx,
     const float & my,
-    const unsigned int & dim_3);
+    const unsigned int & dim_3,
+    const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT);
 
   /**
    * @brief Set the starting pose for planning, as a node index
@@ -155,10 +162,16 @@ class AStarAlgorithm
   NodePtr & getStart();
 
   /**
-   * @brief Get pointer reference to goal node
-   * @return Node pointer reference to goal node
+   * @brief Get pointer reference to goals node
+   * @return unordered_set of node pointers reference to the goals nodes
+   */
+  NodeSet & getGoals();
+
+  /**
+   * @brief Get pointer reference to goals coordinates
+   * @return vector of goals coordinates reference to the goals coordinates
    */
-  NodePtr & getGoal();
+  CoordinateVector & getGoalsCoordinates();
 
   /**
    * @brief Get maximum number of on-approach iterations after within threshold
@@ -260,9 +273,9 @@ class AStarAlgorithm
   unsigned int _dim3_size;
   SearchInfo _search_info;
 
-  Coordinates _goal_coordinates;
+  CoordinateVector _goals_coordinates;
   NodePtr _start;
-  NodePtr _goal;
+  NodeSet _goalsSet;
 
   Graph _graph;
   NodeQueue _queue;
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 c4d3deccda7..a572da6ad0c 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
@@ -18,6 +18,7 @@
 #include <functional>
 #include <list>
 #include <memory>
+#include <unordered_set>
 #include <string>
 #include <vector>
 
@@ -35,7 +36,9 @@ class AnalyticExpansion
 {
 public:
   typedef NodeT * NodePtr;
+  typedef std::unordered_set<NodePtr> NodeSet;
   typedef typename NodeT::Coordinates Coordinates;
+  typedef typename NodeT::CoordinateVector CoordinateVector;
   typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
 
   /**
@@ -79,16 +82,18 @@ class AnalyticExpansion
   /**
    * @brief Attempt an analytic path completion
    * @param node The node to start the analytic path from
-   * @param goal The goal node to plan to
+   * @param goals_node set of goals node to plan to
+   * @param goals_coords vector of goal coordinates to plan to
    * @param getter Gets a node at a set of coordinates
    * @param iterations Iterations to run over
    * @param best_cost Best heuristic cost to propertionally expand more closer to the goal
-   * @return Node pointer reference to goal node if successful, else
-   * return nullptr
+   * @return Node pointer reference to goal node with the best score out of the goals node if
+   * successful, else return nullptr
    */
   NodePtr tryAnalyticExpansion(
     const NodePtr & current_node,
-    const NodePtr & goal_node,
+    const NodeSet & goals_node,
+    const CoordinateVector & goals_coords,
     const NodeGetter & getter, int & iterations, int & best_cost);
 
   /**
diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp
index af44ce53659..6344c86fb89 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp
@@ -28,6 +28,14 @@ enum class MotionModel
   STATE_LATTICE = 4,
 };
 
+enum class GoalHeadingMode
+{
+  UNKNOWN = 0,
+  DEFAULT = 1,
+  BIDIRECTIONAL = 2,
+  ALL_DIRECTION = 3,
+};
+
 inline std::string toString(const MotionModel & n)
 {
   switch (n) {
@@ -59,6 +67,33 @@ inline MotionModel fromString(const std::string & n)
   }
 }
 
+inline std::string toString(const GoalHeadingMode & n)
+{
+  switch (n) {
+    case GoalHeadingMode::DEFAULT:
+      return "DEFAULT";
+    case GoalHeadingMode::BIDIRECTIONAL:
+      return "BIDIRECTIONAL";
+    case GoalHeadingMode::ALL_DIRECTION:
+      return "ALL_DIRECTION";
+    default:
+      return "Unknown";
+  }
+}
+
+inline GoalHeadingMode fromStringToGH(const std::string & n)
+{
+  if (n == "DEFAULT") {
+    return GoalHeadingMode::DEFAULT;
+  } else if (n == "BIDIRECTIONAL") {
+    return GoalHeadingMode::BIDIRECTIONAL;
+  } else if (n == "ALL_DIRECTION") {
+    return GoalHeadingMode::ALL_DIRECTION;
+  } else {
+    return GoalHeadingMode::UNKNOWN;
+  }
+}
+
 const float UNKNOWN_COST = 255.0;
 const float OCCUPIED_COST = 254.0;
 const float INSCRIBED_COST = 253.0;
diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
index 2ef090fe498..968d673ba10 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
@@ -50,6 +50,16 @@ class Node2D
     : x(x_in), y(y_in)
     {}
 
+    inline bool operator==(const Coordinates & rhs) const
+    {
+      return this->x == rhs.x && this->y == rhs.y;
+    }
+
+    inline bool operator!=(const Coordinates & rhs) const
+    {
+      return !(*this == rhs);
+    }
+
     float x, y;
   };
   typedef std::vector<Coordinates> CoordinateVector;
@@ -75,6 +85,15 @@ class Node2D
     return this->_index == rhs._index;
   }
 
+  /**
+   * @brief setting continuous coordinate search poses (in partial-cells)
+   * @param Pose pose
+   */
+  inline void setPose(const Coordinates & pose_in)
+  {
+    pose = pose_in;
+  }
+
   /**
    * @brief Reset method for new search
    */
@@ -224,7 +243,7 @@ class Node2D
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates);
+    const CoordinateVector & goals_coords);
 
   /**
    * @brief Initialize the neighborhood to be used in A*
@@ -264,6 +283,7 @@ class Node2D
   bool backtracePath(CoordinateVector & path);
 
   Node2D * parent;
+  Coordinates pose;
   static float cost_travel_multiplier;
   static std::vector<int> _neighbors_grid_offsets;
 
diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
index cf6a9a6e89e..9e317c3cb3f 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
@@ -167,12 +167,12 @@ class NodeHybrid
     : x(x_in), y(y_in), theta(theta_in)
     {}
 
-    inline bool operator==(const Coordinates & rhs)
+    inline bool operator==(const Coordinates & rhs) const
     {
       return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta;
     }
 
-    inline bool operator!=(const Coordinates & rhs)
+    inline bool operator!=(const Coordinates & rhs) const
     {
       return !(*this == rhs);
     }
@@ -371,7 +371,7 @@ class NodeHybrid
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates);
+    const CoordinateVector & goals_coords);
 
   /**
    * @brief Initialize motion models
diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
index f3e3ff37f9a..df08f3a22bf 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
@@ -314,7 +314,7 @@ class NodeLattice
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates);
+    const CoordinateVector & goals_coords);
 
   /**
    * @brief Initialize motion models
@@ -408,8 +408,8 @@ class NodeLattice
   bool backtracePath(CoordinateVector & path);
 
   /**
-   * \brief add node to the path
-   * \param current_node
+   * @brief add node to the path
+   * @param current_node
    */
   void addNodeToPath(NodePtr current_node, CoordinateVector & path);
 
diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp
index f1653569440..651e8e13ab5 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp
@@ -123,6 +123,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   bool _debug_visualizations;
   std::string _motion_model_for_search;
   MotionModel _motion_model;
+  GoalHeadingMode _goal_heading_mode;
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     _planned_footprints_publisher;
diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp
index 57e225f2876..04325bc7147 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp
@@ -120,6 +120,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
     _planned_footprints_publisher;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
     _expansions_publisher;
+  GoalHeadingMode _goal_heading_mode;
   std::mutex _mutex;
   rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
 
diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
index aa95567123e..7827f090884 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -43,9 +43,9 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   _x_size(0),
   _y_size(0),
   _search_info(search_info),
-  _goal_coordinates(Coordinates()),
+  _goals_coordinates(CoordinateVector()),
   _start(nullptr),
-  _goal(nullptr),
+  _goalsSet(NodeSet()),
   _motion_model(motion_model)
 {
   _graph.reserve(100000);
@@ -192,35 +192,78 @@ template<>
 void AStarAlgorithm<Node2D>::setGoal(
   const float & mx,
   const float & my,
-  const unsigned int & dim_3)
+  const unsigned int & dim_3,
+  const GoalHeadingMode & /*goal_heading_mode*/)
 {
   if (dim_3 != 0) {
     throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3.");
   }
-
-  _goal = addToGraph(
-    Node2D::getIndex(
-      static_cast<unsigned int>(mx),
-      static_cast<unsigned int>(my),
-      getSizeX()));
-  _goal_coordinates = Node2D::Coordinates(mx, my);
+  _goals_coordinates.clear();
+  _goalsSet.clear();
+  _goalsSet.insert(addToGraph(Node2D::getIndex(mx, my, getSizeX())));
+  Node2D::Coordinates goal_coords = Node2D::Coordinates(mx, my);
+  (*_goalsSet.begin())->setPose(goal_coords);
+  _goals_coordinates.push_back(goal_coords);
 }
 
 template<typename NodeT>
 void AStarAlgorithm<NodeT>::setGoal(
   const float & mx,
   const float & my,
-  const unsigned int & dim_3)
+  const unsigned int & dim_3,
+  const GoalHeadingMode & goal_heading_mode)
 {
-  _goal = addToGraph(
-    NodeT::getIndex(
-      static_cast<unsigned int>(mx),
-      static_cast<unsigned int>(my),
-      dim_3));
-
-  typename NodeT::Coordinates goal_coords(mx, my, dim_3);
+  _goalsSet.clear();
+  NodeVector goals;
+  CoordinateVector goals_coordinates;
+  unsigned int num_bins = NodeT::motion_table.num_angle_quantization;
+  unsigned int dim_3_half_bin = 0;
+  switch (goal_heading_mode) {
+    case GoalHeadingMode::DEFAULT:
+      goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
+      goals_coordinates.push_back(
+        typename NodeT::Coordinates(
+          static_cast<float>(mx),
+          static_cast<float>(my),
+          static_cast<float>(dim_3)));
+      break;
+    case GoalHeadingMode::BIDIRECTIONAL:
+      // Add two goals, one for each direction
+      goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
+      // 180 degrees
+      dim_3_half_bin = (dim_3 + (num_bins / 2)) % num_bins;
+      goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin)));
+      goals_coordinates.push_back(
+        typename NodeT::Coordinates(
+          static_cast<float>(mx),
+          static_cast<float>(my),
+          static_cast<float>(dim_3)));
+      goals_coordinates.push_back(
+        typename NodeT::Coordinates(
+          static_cast<float>(mx),
+          static_cast<float>(my),
+          static_cast<float>(dim_3_half_bin)));
+      break;
+    case GoalHeadingMode::ALL_DIRECTION:
+      // Add all goals for each direction
+      for (unsigned int i = 0; i < num_bins; ++i) {
+        goals.push_back(addToGraph(NodeT::getIndex(mx, my, i)));
+        goals_coordinates.push_back(
+          typename NodeT::Coordinates(
+            static_cast<float>(mx),
+            static_cast<float>(my),
+            static_cast<float>(i)));
+      }
+      break;
+    case GoalHeadingMode::UNKNOWN:
+      throw std::runtime_error("Goal heading is UNKNOWN.");
+  }
 
-  if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) {
+  // we just have to check whether the x and y are the same because the dim3 is not used
+  // in the computation of the obstacle heuristic
+  if (!_search_info.cache_obstacle_heuristic ||
+    (goals_coordinates != _goals_coordinates))
+  {
     if (!_start) {
       throw std::runtime_error("Start must be set before goal.");
     }
@@ -229,8 +272,11 @@ void AStarAlgorithm<NodeT>::setGoal(
       _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my);
   }
 
-  _goal_coordinates = goal_coords;
-  _goal->setPose(_goal_coordinates);
+  _goals_coordinates = goals_coordinates;
+  for (unsigned int i = 0; i < goals.size(); i++) {
+    goals[i]->setPose(_goals_coordinates[i]);
+    _goalsSet.insert(goals[i]);
+  }
 }
 
 template<typename NodeT>
@@ -242,15 +288,27 @@ bool AStarAlgorithm<NodeT>::areInputsValid()
   }
 
   // Check if points were filled in
-  if (!_start || !_goal) {
+  if (!_start || _goalsSet.empty()) {
     throw std::runtime_error("Failed to compute path, no valid start or goal given.");
   }
 
   // Check if ending point is valid
-  if (getToleranceHeuristic() < 0.001 &&
-    !_goal->isNodeValid(_traverse_unknown, _collision_checker))
-  {
-    throw nav2_core::GoalOccupied("Goal was in lethal cost");
+  if (getToleranceHeuristic() < 0.001) {
+    // if a node is not valid, prune it from the goals set
+    for (auto it = _goalsSet.begin(); it != _goalsSet.end(); ) {
+      if (!(*it)->isNodeValid(_traverse_unknown, _collision_checker)) {
+        _goals_coordinates.erase(
+          std::remove(
+            _goals_coordinates.begin(), _goals_coordinates.end(), (*it)->pose),
+          _goals_coordinates.end());
+        it = _goalsSet.erase(it);
+      } else {
+        ++it;
+      }
+    }
+    if (_goalsSet.empty()) {
+      throw nav2_core::GoalOccupied("Goal was in lethal cost");
+    }
   }
 
   // Note: We do not check the if the start is valid because it is cleared
@@ -338,7 +396,8 @@ bool AStarAlgorithm<NodeT>::createPath(
     // 2.1) Use an analytic expansion (if available) to generate a path
     expansion_result = nullptr;
     expansion_result = _expander->tryAnalyticExpansion(
-      current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance);
+      current_node, getGoals(),
+      getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance);
     if (expansion_result != nullptr) {
       current_node = expansion_result;
     }
@@ -388,7 +447,7 @@ bool AStarAlgorithm<NodeT>::createPath(
 template<typename NodeT>
 bool AStarAlgorithm<NodeT>::isGoal(NodePtr & node)
 {
-  return node == getGoal();
+  return _goalsSet.find(node) != _goalsSet.end();
 }
 
 template<typename NodeT>
@@ -398,9 +457,9 @@ typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart()
 }
 
 template<typename NodeT>
-typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal()
+typename AStarAlgorithm<NodeT>::NodeSet & AStarAlgorithm<NodeT>::getGoals()
 {
-  return _goal;
+  return _goalsSet;
 }
 
 template<typename NodeT>
@@ -425,9 +484,7 @@ float AStarAlgorithm<NodeT>::getHeuristicCost(const NodePtr & node)
 {
   const Coordinates node_coords =
     NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3());
-  float heuristic = NodeT::getHeuristicCost(
-    node_coords, _goal_coordinates);
-
+  float heuristic = NodeT::getHeuristicCost(node_coords, getGoalsCoordinates());
   if (heuristic < _best_heuristic_node.first) {
     _best_heuristic_node = {heuristic, node->getIndex()};
   }
@@ -486,6 +543,12 @@ unsigned int & AStarAlgorithm<NodeT>::getSizeDim3()
   return _dim3_size;
 }
 
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::CoordinateVector & AStarAlgorithm<NodeT>::getGoalsCoordinates()
+{
+  return _goals_coordinates;
+}
+
 // Instantiate algorithm for the supported template types
 template class AStarAlgorithm<Node2D>;
 template class AStarAlgorithm<NodeHybrid>;
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index 73084859da7..25d5b4aa42a 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -48,7 +48,7 @@ void AnalyticExpansion<NodeT>::setCollisionChecker(
 
 template<typename NodeT>
 typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalyticExpansion(
-  const NodePtr & current_node, const NodePtr & goal_node,
+  const NodePtr & current_node, const NodeSet & goals_node, const CoordinateVector & goals_coords,
   const NodeGetter & getter, int & analytic_iterations,
   int & closest_distance)
 {
@@ -60,10 +60,14 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     const Coordinates node_coords =
       NodeT::getCoords(
       current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size);
+
+    float current_best_score = std::numeric_limits<float>::max();
+    AnalyticExpansionNodes current_best_analytic_nodes = {};
+    NodePtr current_best_goal = nullptr;
+    NodePtr current_best_node = nullptr;
     closest_distance = std::min(
       closest_distance,
-      static_cast<int>(NodeT::getHeuristicCost(node_coords, goal_node->pose)));
-
+      static_cast<int>(NodeT::getHeuristicCost(node_coords, goals_coords)));
     // 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)
@@ -80,81 +84,98 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     if (analytic_iterations <= 0) {
       // Reset the counter and try the analytic path expansion
       analytic_iterations = desired_iterations;
-      AnalyticExpansionNodes analytic_nodes =
-        getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space);
-      if (!analytic_nodes.empty()) {
-        // If we have a valid path, attempt to refine it
-        NodePtr node = current_node;
-        NodePtr test_node = current_node;
-        AnalyticExpansionNodes refined_analytic_nodes;
-        for (int i = 0; i < 8; i++) {
-          // Attempt to create better paths in 5 node increments, need to make sure
-          // they exist for each in order to do so (maximum of 40 points back).
-          if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent &&
-            test_node->parent->parent->parent->parent &&
-            test_node->parent->parent->parent->parent->parent)
-          {
-            test_node = test_node->parent->parent->parent->parent->parent;
-            refined_analytic_nodes =
-              getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space);
-            if (refined_analytic_nodes.empty()) {
+
+      for (auto goal_node : goals_node) {
+        AnalyticExpansionNodes analytic_nodes =
+          getAnalyticPath(
+          current_node, goal_node, getter,
+          current_node->motion_table.state_space);
+        if (!analytic_nodes.empty()) {
+          // If we have a valid path, attempt to refine it
+          NodePtr node = current_node;
+          NodePtr test_node = current_node;
+          AnalyticExpansionNodes refined_analytic_nodes;
+          for (int i = 0; i < 8; i++) {
+            // Attempt to create better paths in 5 node increments, need to make sure
+            // they exist for each in order to do so (maximum of 40 points back).
+            if (test_node->parent && test_node->parent->parent &&
+              test_node->parent->parent->parent &&
+              test_node->parent->parent->parent->parent &&
+              test_node->parent->parent->parent->parent->parent)
+            {
+              test_node = test_node->parent->parent->parent->parent->parent;
+              // print the goals pose
+              refined_analytic_nodes =
+                getAnalyticPath(
+                test_node, goal_node, getter,
+                test_node->motion_table.state_space);
+              if (refined_analytic_nodes.empty()) {
+                break;
+              }
+              analytic_nodes = refined_analytic_nodes;
+              node = test_node;
+            } else {
               break;
             }
-            analytic_nodes = refined_analytic_nodes;
-            node = test_node;
-          } else {
-            break;
           }
-        }
 
-        // The analytic expansion can short-cut near obstacles when closer to a goal
-        // So, we can attempt to refine it more by increasing the possible radius
-        // higher than the minimum turning radius and use the best solution based on
-        // a scoring function similar to that used in traveral cost estimation.
-        auto scoringFn = [&](const AnalyticExpansionNodes & expansion) {
-            if (expansion.size() < 2) {
-              return std::numeric_limits<float>::max();
+          // The analytic expansion can short-cut near obstacles when closer to a goal
+          // So, we can attempt to refine it more by increasing the possible radius
+          // higher than the minimum turning radius and use the best solution based on
+          // a scoring function similar to that used in traveral cost estimation.
+          auto scoringFn = [&](const AnalyticExpansionNodes & expansion) {
+              if (expansion.size() < 2) {
+                return std::numeric_limits<float>::max();
+              }
+
+              float score = 0.0;
+              float normalized_cost = 0.0;
+              // Analytic expansions are consistently spaced
+              const float distance = hypotf(
+                expansion[1].proposed_coords.x - expansion[0].proposed_coords.x,
+                expansion[1].proposed_coords.y - expansion[0].proposed_coords.y);
+              const float & weight = expansion[0].node->motion_table.cost_penalty;
+              for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) {
+                normalized_cost = iter->node->getCost() / 252.0f;
+                // Search's Traversal Cost Function
+                score += distance * (1.0 + weight * normalized_cost);
+              }
+              return score;
+            };
+
+          float best_score = scoringFn(analytic_nodes);
+          float score = std::numeric_limits<float>::max();
+          float min_turn_rad = node->motion_table.min_turning_radius;
+          const float max_min_turn_rad = 4.0 * min_turn_rad;  // Up to 4x the turning radius
+          while (min_turn_rad < max_min_turn_rad) {
+            min_turn_rad += 0.5;  // In Grid Coords, 1/2 cell steps
+            ompl::base::StateSpacePtr state_space;
+            if (node->motion_table.motion_model == MotionModel::DUBIN) {
+              state_space = std::make_shared<ompl::base::DubinsStateSpace>(min_turn_rad);
+            } else {
+              state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(min_turn_rad);
             }
-
-            float score = 0.0;
-            float normalized_cost = 0.0;
-            // Analytic expansions are consistently spaced
-            const float distance = hypotf(
-              expansion[1].proposed_coords.x - expansion[0].proposed_coords.x,
-              expansion[1].proposed_coords.y - expansion[0].proposed_coords.y);
-            const float & weight = expansion[0].node->motion_table.cost_penalty;
-            for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) {
-              normalized_cost = iter->node->getCost() / 252.0f;
-              // Search's Traversal Cost Function
-              score += distance * (1.0 + weight * normalized_cost);
+            refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space);
+            score = scoringFn(refined_analytic_nodes);
+            if (score <= best_score) {
+              analytic_nodes = refined_analytic_nodes;
+              best_score = score;
             }
-            return score;
-          };
-
-        float best_score = scoringFn(analytic_nodes);
-        float score = std::numeric_limits<float>::max();
-        float min_turn_rad = node->motion_table.min_turning_radius;
-        const float max_min_turn_rad = 4.0 * min_turn_rad;  // Up to 4x the turning radius
-        while (min_turn_rad < max_min_turn_rad) {
-          min_turn_rad += 0.5;  // In Grid Coords, 1/2 cell steps
-          ompl::base::StateSpacePtr state_space;
-          if (node->motion_table.motion_model == MotionModel::DUBIN) {
-            state_space = std::make_shared<ompl::base::DubinsStateSpace>(min_turn_rad);
-          } else {
-            state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(min_turn_rad);
           }
-          refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space);
-          score = scoringFn(refined_analytic_nodes);
-          if (score <= best_score) {
-            analytic_nodes = refined_analytic_nodes;
-            best_score = score;
+          if (best_score < current_best_score) {
+            current_best_score = best_score;
+            current_best_node = node;
+            current_best_goal = goal_node;
+            current_best_analytic_nodes = analytic_nodes;
           }
         }
-
-        return setAnalyticPath(node, goal_node, analytic_nodes);
       }
     }
-
+    if (!current_best_analytic_nodes.empty()) {
+      return setAnalyticPath(
+        current_best_node, current_best_goal,
+        current_best_analytic_nodes);
+    }
     analytic_iterations--;
   }
 
@@ -356,7 +377,7 @@ typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyt
 
 template<>
 typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyticExpansion(
-  const NodePtr &, const NodePtr &,
+  const NodePtr &, const NodeSet &, const CoordinateVector &,
   const NodeGetter &, int &,
   int &)
 {
diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp
index e0e23840aef..4bf2d9947a8 100644
--- a/nav2_smac_planner/src/node_2d.cpp
+++ b/nav2_smac_planner/src/node_2d.cpp
@@ -81,12 +81,12 @@ float Node2D::getTraversalCost(const NodePtr & child)
 
 float Node2D::getHeuristicCost(
   const Coordinates & node_coords,
-  const Coordinates & goal_coordinates)
+  const CoordinateVector & goals_coords)
 {
   // Using Moore distance as it more accurately represents the distances
   // even a Van Neumann neighborhood robot can navigate.
-  auto dx = goal_coordinates.x - node_coords.x;
-  auto dy = goal_coordinates.y - node_coords.y;
+  auto dx = goals_coords[0].x - node_coords.x;
+  auto dy = goals_coords[0].y - node_coords.y;
   return std::sqrt(dx * dx + dy * dy);
 }
 
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
index 0ec6da7a739..e3f8a294bfb 100644
--- a/nav2_smac_planner/src/node_hybrid.cpp
+++ b/nav2_smac_planner/src/node_hybrid.cpp
@@ -440,12 +440,18 @@ float NodeHybrid::getTraversalCost(const NodePtr & child)
 
 float NodeHybrid::getHeuristicCost(
   const Coordinates & node_coords,
-  const Coordinates & goal_coords)
+  const CoordinateVector & goals_coords)
 {
+  // obstacle heuristic does not depend on goal heading
   const float obstacle_heuristic =
-    getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty);
-  const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic);
-  return std::max(obstacle_heuristic, dist_heuristic);
+    getObstacleHeuristic(node_coords, goals_coords[0], motion_table.cost_penalty);
+  float distance_heuristic = std::numeric_limits<float>::max();
+  for (unsigned int i = 0; i < goals_coords.size(); i++) {
+    distance_heuristic = std::min(
+      distance_heuristic,
+      getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic));
+  }
+  return std::max(obstacle_heuristic, distance_heuristic);
 }
 
 void NodeHybrid::initMotionModel(
diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp
index 6c794b9a987..e5e8112a456 100644
--- a/nav2_smac_planner/src/node_lattice.cpp
+++ b/nav2_smac_planner/src/node_lattice.cpp
@@ -333,13 +333,18 @@ float NodeLattice::getTraversalCost(const NodePtr & child)
 
 float NodeLattice::getHeuristicCost(
   const Coordinates & node_coords,
-  const Coordinates & goal_coords)
+  const CoordinateVector & goals_coords)
 {
   // get obstacle heuristic value
+  // obstacle heuristic does not depend on goal heading
   const float obstacle_heuristic = getObstacleHeuristic(
-    node_coords, goal_coords, motion_table.cost_penalty);
-  const float distance_heuristic =
-    getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic);
+    node_coords, goals_coords[0], motion_table.cost_penalty);
+  float distance_heuristic = std::numeric_limits<float>::max();
+  for (unsigned int i = 0; i < goals_coords.size(); i++) {
+    distance_heuristic = std::min(
+      distance_heuristic,
+      getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic));
+  }
   return std::max(obstacle_heuristic, distance_heuristic);
 }
 
@@ -455,7 +460,7 @@ void NodeLattice::precomputeDistanceHeuristic(
   int dim_3_size_int = static_cast<int>(dim_3_size);
 
   // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
-  // to help drive the search towards admissible approaches. Deu to symmetries in the
+  // to help drive the search towards admissible approaches. Due to symmetries in the
   // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror
   // around the X axis any relative node lookup. This reduces memory overhead and increases
   // the size of a window a platform can store in memory.
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
index 02b85fef7a5..9b9a1213802 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -166,6 +166,17 @@ void SmacPlannerHybrid::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
   node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search);
+  std::string goal_heading_type;
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT"));
+  node->get_parameter(name + ".goal_heading_mode", goal_heading_type);
+  _goal_heading_mode = fromStringToGH(goal_heading_type);
+  if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) {
+    std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' "
+      "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ";
+    throw nav2_core::PlannerException(error_msg);
+  }
+
   _motion_model = fromString(_motion_model_for_search);
   if (_motion_model == MotionModel::UNKNOWN) {
     RCLCPP_WARN(
@@ -390,7 +401,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
     orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  _a_star->setGoal(mx, my, static_cast<unsigned int>(orientation_bin));
+  _a_star->setGoal(mx, my, static_cast<unsigned int>(orientation_bin), _goal_heading_mode);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -648,6 +659,23 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
             "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
             _motion_model_for_search.c_str());
         }
+      } else if (name == _name + ".goal_heading_mode") {
+        std::string goal_heading_type = parameter.as_string();
+        GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type);
+        RCLCPP_INFO(
+          _logger,
+          "GoalHeadingMode type set to '%s'.",
+          goal_heading_type.c_str());
+        if (goal_heading_mode == GoalHeadingMode::UNKNOWN) {
+          RCLCPP_WARN(
+            _logger,
+            "Unable to get GoalHeader type. Given '%s', "
+            "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ",
+            goal_heading_type.c_str());
+          throw nav2_core::PlannerException("Invalid GoalHeadingMode type");
+        } else {
+          _goal_heading_mode = goal_heading_mode;
+        }
       }
     }
   }
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index ebeead9c5c4..23696196913 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -141,6 +141,17 @@ void SmacPlannerLattice::configure(
     node, name + ".debug_visualizations", rclcpp::ParameterValue(false));
   node->get_parameter(name + ".debug_visualizations", _debug_visualizations);
 
+  std::string goal_heading_type;
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT"));
+  node->get_parameter(name + ".goal_heading_mode", goal_heading_type);
+  _goal_heading_mode = fromStringToGH(goal_heading_type);
+  if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) {
+    std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' "
+      "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ";
+    throw nav2_core::PlannerException(error_msg);
+  }
+
   _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
   _search_info.minimum_turning_radius =
     _metadata.min_turning_radius / (_costmap->getResolution());
@@ -304,7 +315,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   }
   _a_star->setGoal(
     mx, my,
-    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)));
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)),
+    _goal_heading_mode);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -541,6 +553,23 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
         _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
         _search_info.minimum_turning_radius =
           _metadata.min_turning_radius / (_costmap->getResolution());
+      } else if (name == _name + ".goal_heading_mode") {
+        std::string goal_heading_type = parameter.as_string();
+        GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type);
+        RCLCPP_INFO(
+          _logger,
+          "GoalHeadingMode type set to '%s'.",
+          goal_heading_type.c_str());
+        if (goal_heading_mode == GoalHeadingMode::UNKNOWN) {
+          RCLCPP_WARN(
+            _logger,
+            "Unable to get GoalHeader type. Given '%s', "
+            "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ",
+            goal_heading_type.c_str());
+          throw nav2_core::PlannerException("Invalid GoalHeadingMode type");
+        } else {
+          _goal_heading_mode = goal_heading_mode;
+        }
       }
     }
   }
diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
index 32ff9158251..a92463d18a2 100644
--- a/nav2_smac_planner/test/test_a_star.cpp
+++ b/nav2_smac_planner/test/test_a_star.cpp
@@ -131,7 +131,8 @@ TEST(AStarTest, test_a_star_2d)
   }
 
   EXPECT_TRUE(a_star_2.getStart() != nullptr);
-  EXPECT_TRUE(a_star_2.getGoal() != nullptr);
+  EXPECT_TRUE(!a_star_2.getGoals().empty());
+  EXPECT_EQ(a_star_2.getGoals().size(), 1);
   EXPECT_EQ(a_star_2.getSizeX(), 100u);
   EXPECT_EQ(a_star_2.getSizeY(), 100u);
   EXPECT_EQ(a_star_2.getSizeDim3(), 1u);
@@ -399,10 +400,17 @@ TEST(AStarTest, test_se2_single_pose_path)
     };
   // functional case testing
   a_star.setCollisionChecker(checker.get());
+  nav2_smac_planner::NodeHybrid::CoordinateVector path;
+
+  // test with no goals set nor start
+  EXPECT_THROW(
+    a_star.createPath(
+      path, num_it, tolerance,
+      dummy_cancel_checker), std::runtime_error);
+
   a_star.setStart(10u, 10u, 0u);
   // Goal is one costmap cell away
   a_star.setGoal(12u, 10u, 0u);
-  nav2_smac_planner::NodeHybrid::CoordinateVector path;
   EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker));
 
   // Check that the path is length one
@@ -414,6 +422,89 @@ TEST(AStarTest, test_se2_single_pose_path)
   nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }
 
+TEST(AStarTest, test_goal_heading_mode)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.1;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 2.0;
+  info.minimum_turning_radius = 8;  // in grid coordinates
+  info.retrospective_penalty = 0.015;
+  info.analytic_expansion_max_length = 20.0;  // in grid coordinates
+  info.analytic_expansion_ratio = 3.5;
+  unsigned int size_theta = 72;
+  info.cost_penalty = 1.7;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
+    nav2_smac_planner::MotionModel::DUBIN, info);
+  int max_iterations = 10000;
+  float tolerance = 10.0;
+  int it_on_approach = 10;
+  int terminal_checking_interval = 5000;
+  double max_planning_time = 120.0;
+  int num_it = 0;
+
+  // BIDIRECTIONAL goal heading mode
+  a_star.initialize(
+    false, max_iterations, it_on_approach, terminal_checking_interval,
+    max_planning_time, 401, size_theta);
+
+  nav2_costmap_2d::Costmap2D * costmapA =
+    new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
+
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmapA;
+
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, size_theta, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  a_star.setCollisionChecker(checker.get());
+
+  EXPECT_THROW(
+    a_star.setGoal(
+      80u, 80u, 40u,
+      nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL),
+    std::runtime_error);
+  a_star.setStart(10u, 10u, 0u);
+  a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL);
+  nav2_smac_planner::NodeHybrid::CoordinateVector path;
+  std::unique_ptr<std::vector<std::tuple<float, float, float>>> expansions = nullptr;
+  expansions = std::make_unique<std::vector<std::tuple<float, float, float>>>();
+
+  auto dummy_cancel_checker = []() {
+      return false;
+    };
+
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get()));
+
+  EXPECT_EQ(a_star.getGoals().size(), 2);
+  EXPECT_EQ(a_star.getGoals().size(), a_star.getGoalsCoordinates().size());
+
+
+  // ALL_DIRECTION goal heading mode
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(10u, 10u, 0u);
+  a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION);
+
+  unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization;
+  EXPECT_TRUE(a_star.getGoals().size() == num_bins);
+  EXPECT_EQ(a_star.getGoals().size(), a_star.getGoalsCoordinates().size());
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get()));
+
+  // UNKNOWN goal heading mode
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(10u, 10u, 0u);
+
+  EXPECT_THROW(
+    a_star.setGoal(
+      80u, 80u, 10u,
+      nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error);
+}
+
 TEST(AStarTest, test_constants)
 {
   nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN;  // unknown
@@ -425,6 +516,15 @@ TEST(AStarTest, test_constants)
   mm = nav2_smac_planner::MotionModel::REEDS_SHEPP;  // reeds-shepp
   EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp"));
 
+  nav2_smac_planner::GoalHeadingMode gh = nav2_smac_planner::GoalHeadingMode::UNKNOWN;
+  EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("Unknown"));
+  gh = nav2_smac_planner::GoalHeadingMode::DEFAULT;  // default
+  EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("DEFAULT"));
+  gh = nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL;  // bidirectional
+  EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("BIDIRECTIONAL"));
+  gh = nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION;  // all_direction
+  EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("ALL_DIRECTION"));
+
   EXPECT_EQ(
     nav2_smac_planner::fromString(
       "2D"), nav2_smac_planner::MotionModel::TWOD);
@@ -433,4 +533,16 @@ TEST(AStarTest, test_constants)
     nav2_smac_planner::fromString(
       "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP);
   EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN);
+
+  EXPECT_EQ(
+    nav2_smac_planner::fromStringToGH(
+      "DEFAULT"), nav2_smac_planner::GoalHeadingMode::DEFAULT);
+  EXPECT_EQ(
+    nav2_smac_planner::fromStringToGH(
+      "BIDIRECTIONAL"), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL);
+  EXPECT_EQ(
+    nav2_smac_planner::fromStringToGH(
+      "ALL_DIRECTION"), nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION);
+  EXPECT_EQ(
+    nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN);
 }
diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp
index 4aa66b04a1d..d021233a771 100644
--- a/nav2_smac_planner/test/test_node2d.cpp
+++ b/nav2_smac_planner/test/test_node2d.cpp
@@ -75,8 +75,10 @@ TEST(Node2DTest, test_node_2d)
 
   // check heuristic cost computation
   nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0);
+  nav2_smac_planner::Node2D::CoordinateVector B_vec;
   nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0);
-  EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02);
+  B_vec.push_back(B);
+  EXPECT_NEAR(testB.getHeuristicCost(A, B_vec), 11.18, 0.02);
 
   // check operator== works on index
   unsigned char costC = '2';
diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp
index 9952912596b..e973b2f94ca 100644
--- a/nav2_smac_planner/test/test_smac_hybrid.cpp
+++ b/nav2_smac_planner/test/test_smac_hybrid.cpp
@@ -37,6 +37,16 @@ class RclCppFixture
 };
 RclCppFixture g_rclcppfixture;
 
+// Simple wrapper to be able to call a private member
+class HybridWrap : public nav2_smac_planner::SmacPlannerHybrid
+{
+public:
+  void callDynamicParams(std::vector<rclcpp::Parameter> parameters)
+  {
+    dynamicParametersCallback(parameters);
+  }
+};
+
 // SMAC smoke tests for plugin-level issues rather than algorithms
 // (covered by more extensively testing in other files)
 // System tests in nav2_system_tests will actually plan with this work
@@ -68,6 +78,14 @@ TEST(SmacTest, test_smac_se2)
   goal.pose.position.y = 1.0;
   goal.pose.orientation.w = 1.0;
   auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>();
+
+  // invalid goal heading mode
+  nodeSE2->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN"));
+  nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+  EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error);
+
+  // valid goal heading mode
+  nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT")));
   planner->configure(nodeSE2, "test", nullptr, costmap_ros);
   planner->activate();
 
@@ -94,7 +112,7 @@ TEST(SmacTest, test_smac_se2_reconfigure)
     std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
   costmap_ros->on_configure(rclcpp_lifecycle::State());
 
-  auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>();
+  auto planner = std::make_unique<HybridWrap>();
   planner->configure(nodeSE2, "test", nullptr, costmap_ros);
   planner->activate();
 
@@ -126,7 +144,9 @@ TEST(SmacTest, test_smac_se2_reconfigure)
       rclcpp::Parameter("test.analytic_expansion_max_length", 42.0),
       rclcpp::Parameter("test.max_on_approach_iterations", 42),
       rclcpp::Parameter("test.terminal_checking_interval", 42),
-      rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))});
+      rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP")),
+      rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))});
+
 
   rclcpp::spin_until_future_complete(
     nodeSE2->get_node_base_interface(),
@@ -155,11 +175,18 @@ TEST(SmacTest, test_smac_se2_reconfigure)
   EXPECT_EQ(
     nodeSE2->get_parameter("test.motion_model_for_search").as_string(),
     std::string("REEDS_SHEPP"));
-
+  EXPECT_EQ(
+    nodeSE2->get_parameter("test.goal_heading_mode").as_string(),
+    std::string("BIDIRECTIONAL"));
   auto results2 = rec_param->set_parameters_atomically(
     {rclcpp::Parameter("resolution", 0.2)});
   rclcpp::spin_until_future_complete(
     nodeSE2->get_node_base_interface(),
     results2);
   EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2);
+
+  // Test reconfigure with invalid goal heading mode
+  std::vector<rclcpp::Parameter> parameters;
+  parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+  EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
 }
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
index dcc925e9e72..415d2021da6 100644
--- a/nav2_smac_planner/test/test_smac_lattice.cpp
+++ b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -73,6 +73,14 @@ TEST(SmacTest, test_smac_lattice)
   goal.pose.orientation.w = 1.0;
   auto planner = std::make_unique<nav2_smac_planner::SmacPlannerLattice>();
   try {
+    // invalid goal heading mode
+    nodeLattice->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN"));
+    nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+    EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error);
+
+    // valid goal heading mode
+    nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT")));
+
     // Expect to throw due to invalid prims file in param
     planner->configure(nodeLattice, "test", nullptr, costmap_ros);
   } catch (...) {
@@ -133,7 +141,8 @@ TEST(SmacTest, test_smac_lattice_reconfigure)
       rclcpp::Parameter("test.rotation_penalty", 42.0),
       rclcpp::Parameter("test.max_on_approach_iterations", 42),
       rclcpp::Parameter("test.terminal_checking_interval", 42),
-      rclcpp::Parameter("test.allow_reverse_expansion", true)});
+      rclcpp::Parameter("test.allow_reverse_expansion", true),
+      rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))});
 
   try {
     // All of these params will re-init A* which will involve loading the control set file
@@ -149,4 +158,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure)
   std::vector<rclcpp::Parameter> parameters;
   parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI")));
   EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
+
+  // same with invalid goal heading mode
+  parameters.clear();
+  parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+  EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
 }