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..73c20e14ec8 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*
@@ -263,7 +282,14 @@ class Node2D
    */
   bool backtracePath(CoordinateVector & path);
 
+  /**
+   * @brief Recompute the bidirectional and all direction distance heuristics
+   * @param goal_coords All valid goal coordinates
+   */
+  static void ReComputeDistanceHeuristic(const CoordinateVector & /*goals_coords*/);
+
   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..229de087964 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
@@ -423,7 +423,7 @@ class NodeHybrid
    */
   static float getDistanceHeuristic(
     const Coordinates & node_coords,
-    const Coordinates & goal_coords,
+    const CoordinateVector & goals_coords,
     const float & obstacle_heuristic);
 
   /**
@@ -475,6 +475,23 @@ class NodeHybrid
     costmap_ros.reset();
   }
 
+  /**
+   * @brief Recompute the bidirectional and all direction distance heuristics
+   * @param goal_coords All valid goal coordinates
+   */
+  static void ReComputeDistanceHeuristic(
+    const CoordinateVector & goals_coords
+  );
+  
+  /**
+    * @brief sets the heading of the goal
+    * @param goal_heading_mode_in The heading mode of the goal
+    */
+  static void setGoalHeadingMode(const GoalHeadingMode & goal_heading_mode_in)
+  {
+    goal_heading_mode = goal_heading_mode_in;
+  }
+
   NodeHybrid * parent;
   Coordinates pose;
 
@@ -489,7 +506,11 @@ class NodeHybrid
   static std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer;
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   static LookupTable dist_heuristic_lookup_table;
+  static LookupTable dist_heuristic_lookup_table_bidirectional;
+  static LookupTable dist_heuristic_lookup_table_all_direction;
   static float size_lookup;
+  static int dim_3_size_int;
+  static GoalHeadingMode goal_heading_mode;
 
 private:
   float _cell_cost;
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..2c06c5002ac 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
@@ -383,7 +383,7 @@ class NodeLattice
    */
   static float getDistanceHeuristic(
     const Coordinates & node_coords,
-    const Coordinates & goal_coords,
+    const CoordinateVector & goals_coords,
     const float & obstacle_heuristic);
 
   /**
@@ -408,17 +408,38 @@ 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);
 
+  /**
+   * @brief Recompute the bidirectional and all direction distance heuristics
+   * @param goal_coords All valid goal coordinates
+   */
+  static void ReComputeDistanceHeuristic(
+    const CoordinateVector & goals_coords
+  );
+
+  /**
+   * @brief sets the heading of the goal
+   * @param goal_heading_mode_in The heading mode of the goal
+   */
+  static void setGoalHeadingMode(const GoalHeadingMode & goal_heading_mode_in)
+  {
+    goal_heading_mode = goal_heading_mode_in;
+  }
+
   NodeLattice * parent;
   Coordinates pose;
   static LatticeMotionTable motion_table;
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   static LookupTable dist_heuristic_lookup_table;
+  static LookupTable dist_heuristic_lookup_table_bidirectional;
+  static LookupTable dist_heuristic_lookup_table_all_direction;
   static float size_lookup;
+  static int dim_3_size_int;
+  static GoalHeadingMode goal_heading_mode;
 
 private:
   float _cell_cost;
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..b109eecf107 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,80 @@ 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.");
+      break;
+  }
 
-  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[0].x != _goals_coordinates[0].x &&
+    goals_coordinates[0].y != _goals_coordinates[0].y))
+  {
     if (!_start) {
       throw std::runtime_error("Start must be set before goal.");
     }
@@ -228,9 +273,13 @@ void AStarAlgorithm<NodeT>::setGoal(
     NodeT::resetObstacleHeuristic(
       _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my);
   }
-
-  _goal_coordinates = goal_coords;
-  _goal->setPose(_goal_coordinates);
+  
+  NodeT::setGoalHeadingMode(goal_heading_mode);
+  _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,18 +291,30 @@ 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
+  // We recreate the lookup table taking into conisderation the valid goals
+  NodeT::ReComputeDistanceHeuristic(_goals_coordinates); 
   return true;
 }
 
@@ -338,7 +399,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 +450,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 +460,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 +487,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 +546,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..28766eb60da 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,17 +60,20 @@ 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)
     int desired_iterations = std::max(
       static_cast<int>(closest_distance / _search_info.analytic_expansion_ratio),
       static_cast<int>(std::ceil(_search_info.analytic_expansion_ratio)));
-
     // If we are closer now, we should update the target number of iterations to go
     analytic_iterations =
       std::min(analytic_iterations, desired_iterations);
@@ -78,83 +81,99 @@ 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()) {
+      for (auto goal_node : goals_node) {
+        // 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;
+              // 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 +375,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..674ae5fbdfe 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);
 }
 
@@ -168,4 +168,10 @@ bool Node2D::backtracePath(CoordinateVector & path)
   return true;
 }
 
+void Node2D::ReComputeDistanceHeuristic(const CoordinateVector & /*goals_coords*/)
+{
+  // No need to recompute the distance heuristic for 2D nodes
+  return;
+}
+
 }  // namespace nav2_smac_planner
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
index 0ec6da7a739..20f06ce7aad 100644
--- a/nav2_smac_planner/src/node_hybrid.cpp
+++ b/nav2_smac_planner/src/node_hybrid.cpp
@@ -39,9 +39,13 @@ LookupTable NodeHybrid::obstacle_heuristic_lookup_table;
 float NodeHybrid::travel_distance_cost = sqrtf(2.0f);
 HybridMotionTable NodeHybrid::motion_table;
 float NodeHybrid::size_lookup = 25;
+int NodeHybrid::dim_3_size_int = 0;
 LookupTable NodeHybrid::dist_heuristic_lookup_table;
+LookupTable NodeHybrid::dist_heuristic_lookup_table_bidirectional;
+LookupTable NodeHybrid::dist_heuristic_lookup_table_all_direction;
 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> NodeHybrid::costmap_ros = nullptr;
 std::shared_ptr<nav2_costmap_2d::InflationLayer> NodeHybrid::inflation_layer = nullptr;
+GoalHeadingMode NodeHybrid::goal_heading_mode = GoalHeadingMode::DEFAULT;
 
 ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
 
@@ -440,12 +444,13 @@ 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);
+  const float distance_heuristic =  getDistanceHeuristic(node_coords, goals_coords, obstacle_heuristic);
+  return std::max(obstacle_heuristic, distance_heuristic);
 }
 
 void NodeHybrid::initMotionModel(
@@ -744,7 +749,7 @@ float NodeHybrid::getObstacleHeuristic(
 
 float NodeHybrid::getDistanceHeuristic(
   const Coordinates & node_coords,
-  const Coordinates & goal_coords,
+  const CoordinateVector & goals_coords,
   const float & obstacle_heuristic)
 {
   // rotate and translate node_coords such that goal_coords relative is (0,0,0)
@@ -754,6 +759,8 @@ float NodeHybrid::getDistanceHeuristic(
 
   // This angle is negative since we are de-rotating the current node
   // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th)
+  // we will use the first goal of the vector
+  const Coordinates goal_coords = goals_coords[0];
   const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
   const float cos_th = trig_vals.first;
   const float sin_th = -trig_vals.second;
@@ -790,22 +797,51 @@ float NodeHybrid::getDistanceHeuristic(
     }
     const int x_pos = node_coords_relative.x + floored_size;
     const int y_pos = static_cast<int>(mirrored_relative_y);
-    const int index =
+    int index = 0;
+    if (goal_heading_mode == GoalHeadingMode::DEFAULT)
+    {
+      index =
       x_pos * ceiling_size * motion_table.num_angle_quantization +
       y_pos * motion_table.num_angle_quantization +
       theta_pos;
-    motion_heuristic = dist_heuristic_lookup_table[index];
+      motion_heuristic = dist_heuristic_lookup_table[index];
+    }
+    else if(goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL)
+    {
+      // convert the theta to the bidirectional theta between 0 and 180 
+      int theta_pos_bidirectional = theta_pos % int(motion_table.num_angle_quantization / 2);
+      index =
+      x_pos * ceiling_size * int(motion_table.num_angle_quantization / 2) +
+      y_pos * int(motion_table.num_angle_quantization / 2) +
+      theta_pos_bidirectional;
+      motion_heuristic = dist_heuristic_lookup_table_bidirectional[index];
+    }
+    else if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION)
+    {
+      // just x and y, no theta 
+      index = x_pos * ceiling_size + y_pos;
+      motion_heuristic = dist_heuristic_lookup_table_all_direction[index];
+    }
+
+    
   } else if (obstacle_heuristic <= 0.0) {
     // If no obstacle heuristic value, must have some H to use
     // In nominal situations, this should never be called.
     static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
-    to[0] = goal_coords.x;
-    to[1] = goal_coords.y;
-    to[2] = goal_coords.theta * motion_table.num_angle_quantization;
     from[0] = node_coords.x;
     from[1] = node_coords.y;
     from[2] = node_coords.theta * motion_table.num_angle_quantization;
-    motion_heuristic = motion_table.state_space->distance(from(), to());
+    float min_motion_heuristic = std::numeric_limits<float>::max();
+    // TODO(@stevedanomodolor): This is a very expensive operation, we might want to optimize this
+    // for the case where we have multiple goals
+    for (unsigned int i = 0; i != goals_coords.size(); i++) {
+      to[0] = goals_coords[i].x;
+      to[1] = goals_coords[i].y;
+      to[2] = goals_coords[i].theta * motion_table.num_angle_quantization;
+      const float min_motion_heuristic_i = motion_table.state_space->distance(from(), to());
+      min_motion_heuristic = std::min(min_motion_heuristic,min_motion_heuristic_i);
+    }
+    motion_heuristic = min_motion_heuristic;
   }
 
   return motion_heuristic;
@@ -837,7 +873,7 @@ void NodeHybrid::precomputeDistanceHeuristic(
   size_lookup = lookup_table_dim;
   float motion_heuristic = 0.0;
   unsigned int index = 0;
-  int dim_3_size_int = static_cast<int>(dim_3_size);
+  dim_3_size_int = static_cast<int>(dim_3_size);
   float angular_bin_size = 2 * M_PI / static_cast<float>(dim_3_size);
 
   // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
@@ -860,6 +896,91 @@ void NodeHybrid::precomputeDistanceHeuristic(
   }
 }
 
+void NodeHybrid::ReComputeDistanceHeuristic(
+    const CoordinateVector & goals_coords
+  )
+  {
+    if(goal_heading_mode != GoalHeadingMode::BIDIRECTIONAL || goal_heading_mode != GoalHeadingMode::ALL_DIRECTION)
+    {
+      return;
+    }
+    // store all the heading in a set to avoid recomputing the same heading
+    std::set<int> heading_set;
+    // is this right?
+    for (unsigned int i = 0; i < goals_coords.size(); i++) {
+      // we need to rotate the goal coords such that the goal is relative to (0,0,0)
+      double dtheta_bin = - goals_coords[i].theta;
+      if (dtheta_bin < 0) {
+        dtheta_bin += motion_table.num_angle_quantization;
+      }
+      if (dtheta_bin > motion_table.num_angle_quantization) {
+        dtheta_bin -= motion_table.num_angle_quantization;
+      }
+      heading_set.insert(dtheta_bin);
+    }
+   
+    // we want to store the min of each angle bin in the distance heuristic lookup table
+    if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL)
+    {
+      unsigned int index = 0;
+      int dim_3_size_int_half = static_cast<int>(dim_3_size_int / 2);
+      dist_heuristic_lookup_table_bidirectional.clear();
+      dist_heuristic_lookup_table_bidirectional.reserve(
+        size_lookup * ceil(size_lookup / 2.0) * dim_3_size_int_half);
+
+      // we will only store half of the angle bins since the other half is a mirror of the first half
+      for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
+        for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
+          for (int heading = 0; heading != dim_3_size_int_half; heading++) {
+            // check  the if angle is apth of the goals coords, we only apply the heuristic to the goal heading that has a valid input
+          const int heading_mirror = heading + dim_3_size_int_half;
+          const int index_heading = x * size_lookup * dim_3_size_int + y * dim_3_size_int + heading;
+          const int index_heading_mirror = x * size_lookup * dim_3_size_int + y * dim_3_size_int + heading_mirror;
+          float distance_heading = dist_heuristic_lookup_table[index_heading];
+          float distance_heading_mirror = dist_heuristic_lookup_table[index_heading_mirror];
+          float min_distance = std::numeric_limits<float>::max();
+            if(heading_set.find(heading) == heading_set.end() && heading_set.find(heading_mirror) != heading_set.end()){
+              min_distance = distance_heading_mirror;
+            }
+            else if(heading_set.find(heading) != heading_set.end() && heading_set.find(heading_mirror) == heading_set.end()){
+              min_distance = distance_heading;
+            }
+            else if(heading_set.find(heading) != heading_set.end() && heading_set.find(heading_mirror) != heading_set.end()){
+              min_distance = std::min(distance_heading, distance_heading_mirror);
+            }
+            dist_heuristic_lookup_table_bidirectional[index] = min_distance;              
+            index++;
+          }
+        }
+      } 
+    }
+    else if(goal_heading_mode == GoalHeadingMode::ALL_DIRECTION)
+    {
+      dist_heuristic_lookup_table_all_direction.clear();
+      dist_heuristic_lookup_table_all_direction.reserve(
+      size_lookup * ceil(size_lookup / 2.0));
+      unsigned int index = 0;
+      for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
+        for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
+          float min_distance = std::numeric_limits<float>::max();
+          for (int heading = 0; heading != dim_3_size_int; heading++) {
+            // check  the if angle is part of the goals coords, we only apply the heuristic to the goal heading that has a valid input
+            if(heading_set.find(heading) == heading_set.end()){
+              continue;
+            }
+            const int lookup_index = x * size_lookup * dim_3_size_int + y * dim_3_size_int + heading;
+            min_distance = std::min(min_distance, dist_heuristic_lookup_table[lookup_index]);
+          }
+          dist_heuristic_lookup_table_all_direction[index] = min_distance;
+          index++;
+        }
+      }
+    }
+    
+
+   
+  }
+
 void NodeHybrid::getNeighbors(
   std::function<bool(const uint64_t &,
   nav2_smac_planner::NodeHybrid * &)> & NeighborGetter,
diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp
index 6c794b9a987..781de2306a7 100644
--- a/nav2_smac_planner/src/node_lattice.cpp
+++ b/nav2_smac_planner/src/node_lattice.cpp
@@ -38,7 +38,11 @@ namespace nav2_smac_planner
 // defining static member for all instance to share
 LatticeMotionTable NodeLattice::motion_table;
 float NodeLattice::size_lookup = 25;
+int NodeLattice::dim_3_size_int = 0;
 LookupTable NodeLattice::dist_heuristic_lookup_table;
+LookupTable NodeLattice::dist_heuristic_lookup_table_bidirectional;
+LookupTable NodeLattice::dist_heuristic_lookup_table_all_direction;
+GoalHeadingMode NodeLattice::goal_heading_mode = GoalHeadingMode::DEFAULT;
 
 // Each of these tables are the projected motion models through
 // time and space applied to the search on the current node in
@@ -333,13 +337,13 @@ 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);
+  const float distance_heuristic =  getDistanceHeuristic(node_coords, goals_coords, obstacle_heuristic);
   return std::max(obstacle_heuristic, distance_heuristic);
 }
 
@@ -361,7 +365,7 @@ void NodeLattice::initMotionModel(
 
 float NodeLattice::getDistanceHeuristic(
   const Coordinates & node_coords,
-  const Coordinates & goal_coords,
+  const CoordinateVector & goals_coords,
   const float & obstacle_heuristic)
 {
   // rotate and translate node_coords such that goal_coords relative is (0,0,0)
@@ -371,6 +375,7 @@ float NodeLattice::getDistanceHeuristic(
 
   // This angle is negative since we are de-rotating the current node
   // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th)
+  const Coordinates goal_coords = goals_coords[0];
   const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
   const float cos_th = trig_vals.first;
   const float sin_th = -trig_vals.second;
@@ -407,20 +412,47 @@ float NodeLattice::getDistanceHeuristic(
     }
     const int x_pos = node_coords_relative.x + floored_size;
     const int y_pos = static_cast<int>(mirrored_relative_y);
-    const int index =
+    int index = 0;
+    if (goal_heading_mode == GoalHeadingMode::DEFAULT)
+    {
+      index =
       x_pos * ceiling_size * motion_table.num_angle_quantization +
       y_pos * motion_table.num_angle_quantization +
       theta_pos;
-    motion_heuristic = dist_heuristic_lookup_table[index];
+      motion_heuristic = dist_heuristic_lookup_table[index];
+    }
+    else if(goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL)
+    {
+      // convert the theta to the bidirectional theta between 0 and 180 
+      int theta_pos_bidirectional = theta_pos % int(motion_table.num_angle_quantization / 2);
+      index =
+      x_pos * ceiling_size * int(motion_table.num_angle_quantization / 2) +
+      y_pos * int(motion_table.num_angle_quantization / 2) +
+      theta_pos_bidirectional;
+      motion_heuristic = dist_heuristic_lookup_table_bidirectional[index];
+    }
+    else if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION)
+    {
+      // just x and y, no theta 
+      index = x_pos * ceiling_size + y_pos;
+      motion_heuristic = dist_heuristic_lookup_table_all_direction[index];
+    }
   } else if (obstacle_heuristic == 0.0) {
     static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
-    to[0] = goal_coords.x;
-    to[1] = goal_coords.y;
-    to[2] = motion_table.getAngleFromBin(goal_coords.theta);
     from[0] = node_coords.x;
     from[1] = node_coords.y;
     from[2] = motion_table.getAngleFromBin(node_coords.theta);
-    motion_heuristic = motion_table.state_space->distance(from(), to());
+    float min_motion_heuristic = std::numeric_limits<float>::max();
+    // TODO(@stevedanomodolor): This is a very expensive operation, we might want to optimize this
+    // for the case where we have multiple goals
+    for (unsigned int i = 0; i != goals_coords.size(); i++) {
+      to[0] = goals_coords[i].x;
+      to[1] = goals_coords[i].y;
+      to[2] = goals_coords[i].theta * motion_table.num_angle_quantization;
+      const float min_motion_heuristic_i = motion_table.state_space->distance(from(), to());
+      min_motion_heuristic = std::min(min_motion_heuristic,min_motion_heuristic_i);
+    }
+    motion_heuristic = min_motion_heuristic;
   }
 
   return motion_heuristic;
@@ -455,7 +487,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.
@@ -474,6 +506,92 @@ void NodeLattice::precomputeDistanceHeuristic(
   }
 }
 
+
+void NodeLattice::ReComputeDistanceHeuristic(
+    const CoordinateVector & goals_coords
+  )
+  {
+    if(goal_heading_mode != GoalHeadingMode::BIDIRECTIONAL || goal_heading_mode != GoalHeadingMode::ALL_DIRECTION)
+    {
+      return;
+    }
+    // store all the heading in a set to avoid recomputing the same heading
+    std::set<int> heading_set;
+    // is this right?
+    for (unsigned int i = 0; i < goals_coords.size(); i++) {
+      // we need to rotate the goal coords such that the goal is relative to (0,0,0)
+      double dtheta_bin = - goals_coords[i].theta;
+      if (dtheta_bin < 0) {
+        dtheta_bin += motion_table.num_angle_quantization;
+      }
+      if (dtheta_bin > motion_table.num_angle_quantization) {
+        dtheta_bin -= motion_table.num_angle_quantization;
+      }
+      heading_set.insert(dtheta_bin);
+    }
+   
+    // we want to store the min of each angle bin in the distance heuristic lookup table
+    if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL)
+    {
+      unsigned int index = 0;
+      int dim_3_size_int_half = static_cast<int>(dim_3_size_int / 2);
+      dist_heuristic_lookup_table_bidirectional.clear();
+      dist_heuristic_lookup_table_bidirectional.reserve(
+        size_lookup * ceil(size_lookup / 2.0) * dim_3_size_int_half);
+
+      // we will only store half of the angle bins since the other half is a mirror of the first half
+      for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
+        for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
+          for (int heading = 0; heading != dim_3_size_int_half; heading++) {
+            // check  the if angle is apth of the goals coords, we only apply the heuristic to the goal heading that has a valid input
+          const int heading_mirror = heading + dim_3_size_int_half;
+          const int index_heading = x * size_lookup * dim_3_size_int + y * dim_3_size_int + heading;
+          const int index_heading_mirror = x * size_lookup * dim_3_size_int + y * dim_3_size_int + heading_mirror;
+          float distance_heading = dist_heuristic_lookup_table[index_heading];
+          float distance_heading_mirror = dist_heuristic_lookup_table[index_heading_mirror];
+          float min_distance = std::numeric_limits<float>::max();
+            if(heading_set.find(heading) == heading_set.end() && heading_set.find(heading_mirror) != heading_set.end()){
+              min_distance = distance_heading_mirror;
+            }
+            else if(heading_set.find(heading) != heading_set.end() && heading_set.find(heading_mirror) == heading_set.end()){
+              min_distance = distance_heading;
+            }
+            else if(heading_set.find(heading) != heading_set.end() && heading_set.find(heading_mirror) != heading_set.end()){
+              min_distance = std::min(distance_heading, distance_heading_mirror);
+            }
+            dist_heuristic_lookup_table_bidirectional[index] = min_distance;              
+            index++;
+          }
+        }
+      } 
+    }
+    else if(goal_heading_mode == GoalHeadingMode::ALL_DIRECTION)
+    {
+      dist_heuristic_lookup_table_all_direction.clear();
+      dist_heuristic_lookup_table_all_direction.reserve(
+      size_lookup * ceil(size_lookup / 2.0));
+      unsigned int index = 0;
+      for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
+        for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
+          float min_distance = std::numeric_limits<float>::max();
+          for (int heading = 0; heading != dim_3_size_int; heading++) {
+            // check  the if angle is part of the goals coords, we only apply the heuristic to the goal heading that has a valid input
+            if(heading_set.find(heading) == heading_set.end()){
+              continue;
+            }
+            const int lookup_index = x * size_lookup * dim_3_size_int + y * dim_3_size_int + heading;
+            min_distance = std::min(min_distance, dist_heuristic_lookup_table[lookup_index]);
+          }
+          dist_heuristic_lookup_table_all_direction[index] = min_distance;
+          index++;
+        }
+      }
+    }
+    
+
+   
+  }
+
 void NodeLattice::getNeighbors(
   std::function<bool(const uint64_t &,
   nav2_smac_planner::NodeLattice * &)> & NeighborGetter,
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
index 02b85fef7a5..6ea7709bfed 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -166,6 +166,19 @@ 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);
+  GoalHeadingMode 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);
+  } else {
+    _goal_heading_mode = goal_heading_mode;
+  }
+
   _motion_model = fromString(_motion_model_for_search);
   if (_motion_model == MotionModel::UNKNOWN) {
     RCLCPP_WARN(
@@ -390,7 +403,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 +661,22 @@ 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;
+        }
       }
     }
   }
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index ebeead9c5c4..0c36a0184cf 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -141,6 +141,20 @@ 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);
+  GoalHeadingMode goal_heading_mode = fromStringToGH(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);
+  } else {
+    _goal_heading_mode = goal_heading_mode;
+  }
+
   _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
   _search_info.minimum_turning_radius =
     _metadata.min_turning_radius / (_costmap->getResolution());
@@ -304,7 +318,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 +556,22 @@ 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());
+        } 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..22fc1deb60d 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);
@@ -414,6 +415,88 @@ 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, 40u,
+      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 +508,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 +525,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..f5b48935f81 100644
--- a/nav2_smac_planner/test/test_smac_hybrid.cpp
+++ b/nav2_smac_planner/test/test_smac_hybrid.cpp
@@ -126,7 +126,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,7 +157,9 @@ 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(
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
index dcc925e9e72..128db09fdc8 100644
--- a/nav2_smac_planner/test/test_smac_lattice.cpp
+++ b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -133,7 +133,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
diff --git a/tools/planner_benchmarking/costmap.pickle b/tools/planner_benchmarking/costmap.pickle
new file mode 100644
index 00000000000..52e05f793ec
Binary files /dev/null and b/tools/planner_benchmarking/costmap.pickle differ
diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py
index 0ae0f883efe..a8e245303b5 100644
--- a/tools/planner_benchmarking/metrics.py
+++ b/tools/planner_benchmarking/metrics.py
@@ -97,29 +97,71 @@ def main():
     navigator = BasicNavigator()
 
     # Set map to use, other options: 100by100_15, 100by100_10
-    map_path = os.getcwd() + '/' + glob.glob('**/100by100_20.yaml', recursive=True)[0]
-    navigator.changeMap(map_path)
-    time.sleep(2)
+    # map_path = os.getcwd() + '/' + glob.glob('**/100by100_20.yaml', recursive=True)[0]
+    # navigator.changeMap(map_path)
+    # time.sleep(2)
 
     # Get the costmap for start/goal validation
     costmap_msg = navigator.getGlobalCostmap()
     costmap = np.asarray(costmap_msg.data)
     costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x)
 
-    planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice']
+    planners = ["DEFAULT_Smac2d", "BIDIRECTIONAL_Smac2d", "ALL_DIRECTION_Smac2d", "DEFAULT_SmacHybrid", "BIDIRECTIONAL_SmacHybrid", "ALL_DIRECTION_SmacHybrid", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacLattice"]
     max_cost = 210
     side_buffer = 100
     time_stamp = navigator.get_clock().now().to_msg()
     results = []
     seed(33)
 
+    # # lets get the data 
+    # with open('/extra_files/goals.pickle', 'rb') as f:
+    #     goals = pickle.load(f)
+    
+    # start_goals = goals[0]
+    # end_goals = goals[1]
+    start_goals = []
+    end_goals = []
+
+    start_goal = PoseStamped()
+    start_goal.header.frame_id = 'map'
+    start_goal.header.stamp = time_stamp
+    start_goal.pose.position.x = -1.7
+    start_goal.pose.position.y = -1.0
+    yaw = math.pi/2.0
+    quad = euler2quat(0.0, 0.0, yaw)
+    start_goal.pose.orientation.w = quad[0]
+    start_goal.pose.orientation.x = quad[1]
+    start_goal.pose.orientation.y = quad[2]
+    start_goal.pose.orientation.z = quad[3]
+
+    end_goal = PoseStamped()
+    end_goal.header.frame_id = 'map'
+    end_goal.header.stamp = time_stamp
+    end_goal.pose.position.x = -1.7
+    end_goal.pose.position.y = 1.0
+    yaw = math.pi/2.0
+    quad = euler2quat(0.0, 0.0, yaw)
+    end_goal.pose.orientation.w = quad[0]
+    end_goal.pose.orientation.x = quad[1]
+    end_goal.pose.orientation.y = quad[2]
+    end_goal.pose.orientation.z = quad[3]
+
+    start_goals.append(start_goal)
+    end_goals.append(end_goal)
+
+    n = len(start_goals)
+
+
     random_pairs = 100
     res = costmap_msg.metadata.resolution
     i = 0
-    while len(results) != random_pairs:
-        print('Cycle: ', i, 'out of: ', random_pairs)
-        start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res)
-        goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res)
+    # while len(results) != random_pairs:
+    for i in range(n):
+        print('Cycle: ', i, 'out of: ', n)
+        # start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res)
+        # goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res)
+        start = start_goals[i]
+        goal = end_goals[i]
         print('Start', start)
         print('Goal', goal)
         result = getPlannerResults(navigator, start, goal, planners)
diff --git a/tools/planner_benchmarking/planners.pickle b/tools/planner_benchmarking/planners.pickle
new file mode 100644
index 00000000000..62eb3138edd
Binary files /dev/null and b/tools/planner_benchmarking/planners.pickle differ
diff --git a/tools/planner_benchmarking/planning_benchmark_bringup.py b/tools/planner_benchmarking/planning_benchmark_bringup.py
index 8ed9f400168..043781b763a 100644
--- a/tools/planner_benchmarking/planning_benchmark_bringup.py
+++ b/tools/planner_benchmarking/planning_benchmark_bringup.py
@@ -23,9 +23,10 @@
 
 def generate_launch_description():
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
-    config = os.path.join(
-        get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml'
-    )
+    # config = os.path.join(
+    #     get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml'
+    # )
+    config = '/extra_files/nav2_params.yaml'
     map_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
     lifecycle_nodes = ['map_server', 'planner_server']
 
diff --git a/tools/planner_benchmarking/results.pickle b/tools/planner_benchmarking/results.pickle
new file mode 100644
index 00000000000..d2d8732e54f
Binary files /dev/null and b/tools/planner_benchmarking/results.pickle differ