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 8b127960cc3..53dcbbd8b9f 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -22,6 +22,7 @@
 #include <tuple>
 #include <utility>
 #include <vector>
+#include <unordered_set>
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_core/planner_exceptions.hpp"
@@ -49,6 +50,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;
@@ -69,6 +71,18 @@ class AStarAlgorithm
 
   typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
 
+  /**
+   * @struct nav2_smac_planner::GoalState
+   * @brief A struct to store the goal state
+   */
+  struct GoalState
+  {
+    NodePtr goal;
+    bool is_valid = true;
+  };
+
+  typedef std::vector<GoalState> GoalStateVector;
+
   /**
    * @brief A constructor for nav2_smac_planner::AStarAlgorithm
    */
@@ -90,6 +104,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,
@@ -125,11 +141,15 @@ class AStarAlgorithm
    * @param mx The node X index of the goal
    * @param my The node Y index of the goal
    * @param dim_3 The node dim_3 index of the goal
+   * @param goal_heading_mode The goal heading mode to use
+   * @param coarse_search_resolution The resolution to search for goal heading
    */
   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,
+    const int & coarse_search_resolution = 1);
 
   /**
    * @brief Set the starting pose for planning, as a node index
@@ -155,10 +175,22 @@ 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 state
+   * @return vector of node pointers reference to the goals state
+   */
+  GoalStateVector & getGoalsState();
+
+  /**
+   * @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
@@ -190,6 +222,20 @@ class AStarAlgorithm
    */
   unsigned int & getSizeDim3();
 
+  /**
+   * @brief Creates the coarse and fine lists of goals to expand
+   * @param coarse_list List of goals to expand
+   * @param fine_list List of goals to refine
+   */
+  void prepareGoalsForExpansion(
+    NodeVector & coarse_list, NodeVector & fine_list);
+
+  /**
+   * @brief Get the resolution of the coarse search
+   * @return Size of the goals to expand
+   */
+  unsigned int getCoarseSearchResolution();
+
 protected:
   /**
    * @brief Get pointer to next goal in open set
@@ -240,6 +286,11 @@ class AStarAlgorithm
    */
   inline void clearGraph();
 
+  /**
+   * @brief Check if node has been visited
+   * @param current_node Node to check if visited
+   * @return if node has been visited
+   */
   inline bool onVisitationCheckNode(const NodePtr & node);
 
   /**
@@ -260,11 +311,14 @@ class AStarAlgorithm
   unsigned int _x_size;
   unsigned int _y_size;
   unsigned int _dim3_size;
+  unsigned int _coarse_search_resolution;
   SearchInfo _search_info;
 
-  Coordinates _goal_coordinates;
+  CoordinateVector _goals_coordinates;
   NodePtr _start;
-  NodePtr _goal;
+  NodeSet _goals_set;
+  GoalStateVector _goals_state;
+
 
   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 6be84d1c109..68d8b3ccbb7 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
@@ -21,6 +21,7 @@
 #include <string>
 #include <vector>
 
+
 #include "nav2_smac_planner/node_2d.hpp"
 #include "nav2_smac_planner/node_hybrid.hpp"
 #include "nav2_smac_planner/node_lattice.hpp"
@@ -35,7 +36,9 @@ class AnalyticExpansion
 {
 public:
   typedef NodeT * NodePtr;
+  typedef std::vector<NodePtr> NodeVector;
   typedef typename NodeT::Coordinates Coordinates;
+  typedef typename NodeT::CoordinateVector CoordinateVector;
   typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
 
   /**
@@ -79,17 +82,22 @@ 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 coarse_list Coarse list of goals nodes to plan to
+   * @param fine_list Fine list of goals nodes 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
+   * @param closest_distance Closest distance to goal
+   * @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 NodeGetter & getter, int & iterations, int & best_cost);
+    const NodeVector & coarse_list,
+    const NodeVector & fine_list,
+    const CoordinateVector & goals_coords,
+    const NodeGetter & getter, int & iterations,
+    int & closest_distance);
 
   /**
    * @brief Perform an analytic path expansion to the goal
@@ -103,6 +111,20 @@ class AnalyticExpansion
     const NodePtr & node, const NodePtr & goal,
     const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space);
 
+  /**
+   * @brief Refined analytic path from the current node to the goal
+   * @param current_node The node to start the analytic path from
+   * @param goal_node The goal node to plan to
+   * @param getter The function object that gets valid nodes from the graph
+   * @param analytic_nodes The set of analytic nodes to refine
+   * @return The score of the refined path
+   */
+  float refineAnalyticPath(
+    const NodePtr & current_node,
+    const NodePtr & goal_node,
+    const NodeGetter & getter,
+    AnalyticExpansionNodes & analytic_nodes);
+
   /**
    * @brief Takes final analytic expansion and appends to current expanded node
    * @param node The node to start the analytic path from
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 1bd0993a8eb..6d54e1696ab 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 ee3a4bf231c..cc3650563a8 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);
     }
@@ -374,7 +374,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 824b435447d..5b07e5453bf 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 4469579f178..96f40f47664 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,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   bool _debug_visualizations;
   std::string _motion_model_for_search;
   MotionModel _motion_model;
+  GoalHeadingMode _goal_heading_mode;
+  int _coarse_search_resolution;
   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 e204c8e01fb..6db78ddccaf 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,8 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
     _planned_footprints_publisher;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
     _expansions_publisher;
+  GoalHeadingMode _goal_heading_mode;
+  int _coarse_search_resolution;
   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 52b94db14af..4c6f2858f5f 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -43,9 +43,10 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   _x_size(0),
   _y_size(0),
   _search_info(search_info),
-  _goal_coordinates(Coordinates()),
+  _goals_coordinates(CoordinateVector()),
   _start(nullptr),
-  _goal(nullptr),
+  _goals_set(NodeSet()),
+  _goals_state(GoalStateVector()),
   _motion_model(motion_model)
 {
   _graph.reserve(100000);
@@ -192,35 +193,89 @@ 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*/,
+  const int & /*coarse_search_resolution*/)
 {
   if (dim_3 != 0) {
     throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3.");
   }
-
-  _goal = addToGraph(
+  _goals_coordinates.clear();
+  _goals_set.clear();
+  _goals_state.clear();
+  _goals_set.insert(addToGraph(
     Node2D::getIndex(
       static_cast<unsigned int>(mx),
       static_cast<unsigned int>(my),
-      getSizeX()));
-  _goal_coordinates = Node2D::Coordinates(mx, my);
+      getSizeX())));
+  _goals_state.push_back({*_goals_set.begin(), true});
+  _goals_coordinates.push_back(Node2D::Coordinates(mx, my));
+  _coarse_search_resolution = 1;
 }
 
 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,
+  const int & coarse_search_resolution)
 {
-  _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);
+  _goals_set.clear();
+  _goals_state.clear();
+  NodeVector goals;
+  _coarse_search_resolution = 1;
+  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<int>(mx),
+          static_cast<int>(my),
+          static_cast<int>(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<int>(mx),
+          static_cast<int>(my),
+          static_cast<int>(dim_3)));
+      goals_coordinates.push_back(
+        typename NodeT::Coordinates(
+          static_cast<int>(mx),
+          static_cast<int>(my),
+          static_cast<int>(dim_3_half_bin)));
+      break;
+    case GoalHeadingMode::ALL_DIRECTION:
+      // Set the coarse search resolution only for all direction
+      _coarse_search_resolution = coarse_search_resolution;
+      // 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,28 +284,51 @@ 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]);
+    _goals_set.insert(goals[i]);
+    _goals_state.push_back({goals[i], true});
+  }
 }
 
 template<typename NodeT>
 bool AStarAlgorithm<NodeT>::areInputsValid()
 {
   // Check if graph was filled in
+
   if (_graph.empty()) {
     throw std::runtime_error("Failed to compute path, no costmap given.");
   }
 
   // Check if points were filled in
-  if (!_start || !_goal) {
+  if (!_start || _goals_set.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, goals vector, and goals coordinates
+    for (auto it = _goals_set.begin(); it != _goals_set.end(); ) {
+      if (!(*it)->isNodeValid(_traverse_unknown, _collision_checker)) {
+        _goals_coordinates.erase(
+          std::remove(
+            _goals_coordinates.begin(), _goals_coordinates.end(), (*it)->pose),
+          _goals_coordinates.end());
+        // we set the goal as invalid
+        auto goal_state_it = std::find_if(_goals_state.begin(), _goals_state.end(),
+            [&it](const GoalState & goal_state) {return goal_state.goal == *it;});
+        if (goal_state_it != _goals_state.end()) {
+          goal_state_it->is_valid = false;
+        }
+        it = _goals_set.erase(it);
+      } else {
+        ++it;
+      }
+    }
+    if (_goals_set.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
@@ -273,6 +351,9 @@ bool AStarAlgorithm<NodeT>::createPath(
     return false;
   }
 
+  NodeVector coarse_list, fine_list;
+  prepareGoalsForExpansion(coarse_list, fine_list);
+
   // 0) Add starting point to the open set
   addNode(0.0, getStart());
   getStart()->setAccumulatedCost(0.0);
@@ -339,7 +420,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, coarse_list, fine_list,
+      getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance);
     if (expansion_result != nullptr) {
       current_node = expansion_result;
     }
@@ -389,7 +471,7 @@ bool AStarAlgorithm<NodeT>::createPath(
 template<typename NodeT>
 bool AStarAlgorithm<NodeT>::isGoal(NodePtr & node)
 {
-  return node == getGoal();
+  return _goals_set.find(node) != _goals_set.end();
 }
 
 template<typename NodeT>
@@ -399,9 +481,15 @@ 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 _goals_set;
+}
+
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::GoalStateVector & AStarAlgorithm<NodeT>::getGoalsState()
+{
+  return _goals_state;
 }
 
 template<typename NodeT>
@@ -426,9 +514,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()};
   }
@@ -493,6 +579,33 @@ unsigned int & AStarAlgorithm<NodeT>::getSizeDim3()
   return _dim3_size;
 }
 
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::CoordinateVector & AStarAlgorithm<NodeT>::getGoalsCoordinates()
+{
+  return _goals_coordinates;
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::prepareGoalsForExpansion(
+  NodeVector & coarse_list, NodeVector & fine_list)
+{
+  for (unsigned int i = 0; i < _goals_state.size(); i++) {
+    if (_goals_state[i].is_valid) {
+      if (i % _coarse_search_resolution == 0) {
+        coarse_list.push_back(_goals_state[i].goal);
+      } else {
+        fine_list.push_back(_goals_state[i].goal);
+      }
+    }
+  }
+}
+
+template<typename NodeT>
+unsigned int AStarAlgorithm<NodeT>::getCoarseSearchResolution()
+{
+  return _coarse_search_resolution;
+}
+
 // 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 ec0ccc31f02..c584a9aa11d 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -48,7 +48,10 @@ 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 NodeVector & coarse_list,
+  const NodeVector & fine_list,
+  const CoordinateVector & goals_coords,
   const NodeGetter & getter, int & analytic_iterations,
   int & closest_distance)
 {
@@ -60,10 +63,15 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     const Coordinates node_coords =
       NodeT::getCoords(
       current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size);
+
+    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(
       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)
@@ -78,83 +86,56 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     // Always run the expansion on the first run in case there is a
     // trivial path to be found
     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()) {
-              break;
-            }
-            analytic_nodes = refined_analytic_nodes;
-            node = test_node;
-          } else {
-            break;
+      bool found_valid_expansion = false;
+
+      // First check the coarse search resolution goals
+      for (auto & current_goal_node : coarse_list) {
+        AnalyticExpansionNodes analytic_nodes =
+          getAnalyticPath(
+          current_node, current_goal_node, getter,
+          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);
+          // 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_node = current_node;
+            current_best_score = score;
           }
         }
+      }
 
-        // 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 traversal 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);
+      // perform a final search if we found a goal
+      if (found_valid_expansion) {
+        for (auto & current_goal_node : fine_list) {
+          AnalyticExpansionNodes analytic_nodes =
+            getAnalyticPath(
+            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);
+            // 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_node = current_node;
+              current_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;
           }
         }
-
-        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--;
   }
 
@@ -179,7 +160,6 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
 
   float d = state_space->distance(from(), to());
 
-  // A move of sqrt(2) is guaranteed to be in a new cell
   static const float sqrt_2 = sqrtf(2.0f);
 
   // If the length is too far, exit. This prevents unsafe shortcutting of paths
@@ -292,6 +272,87 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   return possible_nodes;
 }
 
+template<typename NodeT>
+float AnalyticExpansion<NodeT>::refineAnalyticPath(
+  const NodePtr & current_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++) {
+    // 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;
+    }
+  }
+
+  // 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);
+    }
+    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 best_score;
+}
+
 template<typename NodeT>
 typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::setAnalyticPath(
   const NodePtr & node,
@@ -345,6 +406,16 @@ getAnalyticPath(
   return AnalyticExpansionNodes();
 }
 
+template<>
+float AnalyticExpansion<Node2D>::refineAnalyticPath(
+  const NodePtr &,
+  const NodePtr &,
+  const NodeGetter &,
+  AnalyticExpansionNodes &)
+{
+  return std::numeric_limits<float>::max();
+}
+
 template<>
 typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyticPath(
   const NodePtr &,
@@ -356,7 +427,10 @@ typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyt
 
 template<>
 typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyticExpansion(
-  const NodePtr &, const NodePtr &,
+  const NodePtr &,
+  const NodeVector &,
+  const NodeVector &,
+  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 bf64ae29e3e..11fdf8b3b41 100644
--- a/nav2_smac_planner/src/node_2d.cpp
+++ b/nav2_smac_planner/src/node_2d.cpp
@@ -85,12 +85,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 0eaef5fe6be..6383d93b7e5 100644
--- a/nav2_smac_planner/src/node_hybrid.cpp
+++ b/nav2_smac_planner/src/node_hybrid.cpp
@@ -442,12 +442,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 dcbc33f197d..13d052f4c7a 100644
--- a/nav2_smac_planner/src/node_lattice.cpp
+++ b/nav2_smac_planner/src/node_lattice.cpp
@@ -345,13 +345,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);
 }
 
@@ -467,7 +472,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 fcce89cdfea..48e4d9b747a 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -166,6 +166,22 @@ 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);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".coarse_search_resolution", rclcpp::ParameterValue(4));
+  node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution);
+
+  _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(
@@ -189,6 +205,22 @@ void SmacPlannerHybrid::configure(
     _max_iterations = std::numeric_limits<int>::max();
   }
 
+  if (_coarse_search_resolution <= 0) {
+    RCLCPP_WARN(
+      _logger, "coarse iteration resolution selected as <= 0, "
+      "disabling coarse iteration resolution search for goal heading"
+    );
+
+    _coarse_search_resolution = 1;
+  }
+
+  if (_angle_quantizations % _coarse_search_resolution != 0) {
+    RCLCPP_WARN(
+      _logger, "coarse iteration should be an increment of the number of angular bins configured"
+    );
+    _coarse_search_resolution = 1;
+  }
+
   if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) {
     RCLCPP_WARN(
       _logger, "Min turning radius cannot be less than the search grid cell resolution!");
@@ -400,7 +432,8 @@ 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_goal, my_goal, static_cast<unsigned int>(orientation_bin));
+  _a_star->setGoal(mx_goal, my_goal, static_cast<unsigned int>(orientation_bin),
+                  _goal_heading_mode, _coarse_search_resolution);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -662,6 +695,14 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
         int angle_quantizations = parameter.as_int();
         _angle_bin_size = 2.0 * M_PI / angle_quantizations;
         _angle_quantizations = static_cast<unsigned int>(angle_quantizations);
+
+        if (_angle_quantizations % _coarse_search_resolution != 0) {
+          RCLCPP_WARN(
+            _logger, "coarse iteration should be an increment of the "
+            "number of angular bins configured"
+          );
+          _coarse_search_resolution = 1;
+        }
       }
     } else if (type == ParameterType::PARAMETER_STRING) {
       if (name == _name + ".motion_model_for_search") {
@@ -674,6 +715,39 @@ 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());
+        } else {
+          _goal_heading_mode = goal_heading_mode;
+        }
+      } else if (name == _name + ".coarse_search_resolution") {
+        _coarse_search_resolution = parameter.as_int();
+        if (_coarse_search_resolution <= 0) {
+          RCLCPP_WARN(
+            _logger, "coarse iteration resolution selected as <= 0, "
+            "disabling coarse iteration resolution search for goal heading"
+          );
+          _coarse_search_resolution = 1;
+        }
+
+        if (_angle_quantizations % _coarse_search_resolution != 0) {
+          RCLCPP_WARN(
+            _logger,
+              "coarse iteration should be an increment of the number of angular bins configured"
+          );
+          _coarse_search_resolution = 1;
+        }
       }
     }
   }
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index 82bdcc9810e..0a42406fe82 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -141,6 +141,22 @@ 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);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1));
+  node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution);
+
+  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());
@@ -160,6 +176,21 @@ void SmacPlannerLattice::configure(
     _max_iterations = std::numeric_limits<int>::max();
   }
 
+  if (_coarse_search_resolution <= 0) {
+    RCLCPP_WARN(
+      _logger, "coarse iteration resolution selected as <= 0, "
+      "disabling coarse iteration resolution search for goal heading"
+    );
+    _coarse_search_resolution = 1;
+  }
+
+  if (_metadata.number_of_headings % _coarse_search_resolution != 0) {
+    RCLCPP_WARN(
+      _logger, "coarse iteration should be an increment of the number of angular bins configured"
+    );
+    _coarse_search_resolution = 1;
+  }
+
   float lookup_table_dim =
     static_cast<float>(_lookup_table_size) /
     static_cast<float>(_costmap->getResolution());
@@ -314,7 +345,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   }
   _a_star->setGoal(
     mx_goal, my_goal,
-    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)));
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)),
+    _goal_heading_mode, _coarse_search_resolution);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -567,6 +599,45 @@ 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());
+        if (_metadata.number_of_headings % _coarse_search_resolution != 0) {
+          RCLCPP_WARN(
+            _logger, "coarse iteration should be an increment of the number "
+            "of angular bins configured"
+          );
+          _coarse_search_resolution = 1;
+        }
+      } 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());
+        } else {
+          _goal_heading_mode = goal_heading_mode;
+        }
+      } else if (name == _name + ".coarse_search_resolution") {
+        _coarse_search_resolution = parameter.as_int();
+        if (_coarse_search_resolution <= 0) {
+          RCLCPP_WARN(
+            _logger, "coarse iteration resolution selected as <= 0, "
+            "disabling coarse iteration resolution search for goal heading"
+          );
+          _coarse_search_resolution = 1;
+        }
+        if (_metadata.number_of_headings % _coarse_search_resolution != 0) {
+          RCLCPP_WARN(
+            _logger,
+              "coarse iteration should be an increment of the number of angular bins configured"
+          );
+          _coarse_search_resolution = 1;
+        }
       }
     }
   }
diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
index ef09202dbbb..7b0b913ccba 100644
--- a/nav2_smac_planner/test/test_a_star.cpp
+++ b/nav2_smac_planner/test/test_a_star.cpp
@@ -131,7 +131,7 @@ TEST(AStarTest, test_a_star_2d)
   }
 
   EXPECT_TRUE(a_star_2.getStart() != nullptr);
-  EXPECT_TRUE(a_star_2.getGoal() != nullptr);
+  EXPECT_NE(a_star_2.getGoals().size(), 0);
   EXPECT_EQ(a_star_2.getSizeX(), 100u);
   EXPECT_EQ(a_star_2.getSizeY(), 100u);
   EXPECT_EQ(a_star_2.getSizeDim3(), 1u);
@@ -399,10 +399,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 +421,101 @@ 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
+  unsigned int coarse_search_resolution = 16;
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(10u, 10u, 0u);
+  a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION,
+    coarse_search_resolution);
+  EXPECT_TRUE(a_star.getCoarseSearchResolution() == coarse_search_resolution);
+
+  unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization;
+
+  // get number of valid goal states
+  unsigned int num_valid_goals = 0;
+  for (auto & goal_state : a_star.getGoalsState()) {
+    if(goal_state.is_valid) {
+      num_valid_goals++;
+    }
+  }
+  EXPECT_TRUE(a_star.getGoals().size() == num_bins);
+  EXPECT_TRUE(a_star.getGoals().size() == num_valid_goals);
+  EXPECT_TRUE(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 +527,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 +544,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 a70ae86fc7d..ca751f8b080 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,38 @@ 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>();
+
+  nodeSE2->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN"));
+  nodeSE2->declare_parameter("test.coarse_search_resolution", -1);
+  nodeSE2->declare_parameter("test.motion_model_for_search", std::string("Invalid"));
+  nodeSE2->declare_parameter("test.max_on_approach_iterations", -1);
+  nodeSE2->declare_parameter("test.max_iterations", -1);
+
+  // invalid goal heading mode
+  nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+  EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error);
+  nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT")));
+
+  // Invalid motion model
+  nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("invalid")));
+  EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error);
+  nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("DUBIN")));
+
+  nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1));
+  nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1));
+  nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", -1));
+  EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros));
+
+
+  // valid Configuration
+  nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 1));
+  nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000));
+  nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000));
+  nodeSE2->set_parameter(rclcpp::Parameter("test.angle_quantization_bins", 72));
+
+  // angle_quantizations not multiple of coarse search resolution-> default to 1
+  nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 5));
+
   planner->configure(nodeSE2, "test", nullptr, costmap_ros);
   planner->activate();
 
@@ -101,7 +143,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();
 
@@ -133,7 +175,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::Parameter("test.coarse_search_resolution", -1)});
 
   rclcpp::spin_until_future_complete(
     nodeSE2->get_node_base_interface(),
@@ -162,11 +206,25 @@ 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);
+  EXPECT_EQ(nodeSE2->get_parameter("test.coarse_search_resolution").as_int(), -1);
+
+  // test coarse resolution edge cases. Consider when coarse resolution
+  // is not multiple of angle bin quantization
+  auto result3 = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test.coarse_search_resolution", 7),
+      rclcpp::Parameter("test.coarse_search_resolution", 10),
+      rclcpp::Parameter("test.angle_quantization_bins", 87)}
+  );
+  rclcpp::spin_until_future_complete(
+    nodeSE2->get_node_base_interface(),
+    result3);
 }
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
index c187abd8591..f8d38658fbe 100644
--- a/nav2_smac_planner/test/test_smac_lattice.cpp
+++ b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -73,6 +73,23 @@ 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);
+    nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT")));
+
+    // invalid COnfiguration resolution
+    nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1));
+    nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", -1));
+
+    EXPECT_NO_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros));
+    // Valid configuration
+    nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000));
+
+    // Coarse search resolution will default to 1, not muiltiple of number of heading(16 default)
+    nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 3));
+
     // Expect to throw due to invalid prims file in param
     planner->configure(nodeLattice, "test", nullptr, costmap_ros);
   } catch (...) {
@@ -156,4 +173,18 @@ 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);
+
+  // test edge cases for coarse search resolution/goal heading mode
+  auto results2 = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test.goal_heading_mode", std::string("invalid")),
+      rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL")),
+      rclcpp::Parameter("test.coarse_search_resolution", -1),
+      rclcpp::Parameter("test.coarse_search_resolution", 7)});
+
+  try {
+    rclcpp::spin_until_future_complete(
+      nodeLattice->get_node_base_interface(),
+      results2);
+  } catch (...) {
+  }
 }