diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md
index b769f1e4e5..cd7a20b1e2 100644
--- a/nav2_smac_planner/README.md
+++ b/nav2_smac_planner/README.md
@@ -115,6 +115,8 @@ planner_server:
       angle_quantization_bins: 64         # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
       analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
       analytic_expansion_max_length: 3.0    # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
+      analytic_expansion_max_cost: true   # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal)
+      analytic_expansion_max_cost_override: false  #  For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
       minimum_turning_radius: 0.40        # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
       reverse_penalty: 2.1                # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
       change_penalty: 0.0                 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
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 fa1b2f1fdd..10c4224a07 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
@@ -73,7 +73,7 @@ class AnalyticExpansion
    * @brief Sets the collision checker and costmap to use in expansion validation
    * @param collision_checker Collision checker to use
    */
-  void setCollisionChecker(GridCollisionChecker * collision_checker);
+  void setCollisionChecker(GridCollisionChecker * & collision_checker);
 
   /**
    * @brief Attempt an analytic path completion
@@ -95,11 +95,12 @@ class AnalyticExpansion
    * @param node The node to start the analytic path from
    * @param goal The goal node to plan to
    * @param getter The function object that gets valid nodes from the graph
+   * @param state_space State space to use for computing analytic expansions
    * @return A set of analytically expanded nodes to the goal from current node, if possible
    */
   AnalyticExpansionNodes getAnalyticPath(
     const NodePtr & node, const NodePtr & goal,
-    const NodeGetter & getter);
+    const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space);
 
   /**
    * @brief Takes final analytic expansion and appends to current expanded node
diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
index 5933a6dc92..c1878a8863 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
@@ -116,10 +116,10 @@ class GridCollisionChecker
 protected:
   std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
   nav2_costmap_2d::Footprint unoriented_footprint_;
-  double footprint_cost_;
+  float footprint_cost_;
   bool footprint_is_radius_;
   std::vector<float> angles_;
-  double possible_inscribed_cost_{-1};
+  float possible_inscribed_cost_{-1};
   rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};
   rclcpp::Clock::SharedPtr clock_;
 };
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 4150deaee7..392e325fac 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
@@ -87,7 +87,7 @@ class Node2D
    * @brief Gets the accumulated cost at this node
    * @return accumulated cost
    */
-  inline float & getAccumulatedCost()
+  inline float getAccumulatedCost()
   {
     return _accumulated_cost;
   }
@@ -105,7 +105,7 @@ class Node2D
    * @brief Gets the costmap cost at this node
    * @return costmap cost
    */
-  inline float & getCost()
+  inline float getCost()
   {
     return _cell_cost;
   }
@@ -123,7 +123,7 @@ class Node2D
    * @brief Gets if cell has been visited in search
    * @param If cell was visited
    */
-  inline bool & wasVisited()
+  inline bool wasVisited()
   {
     return _was_visited;
   }
@@ -158,7 +158,7 @@ class Node2D
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int & getIndex()
+  inline unsigned int getIndex()
   {
     return _index;
   }
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 e822663cf9..8ba117e80e 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
@@ -217,7 +217,7 @@ class NodeHybrid
    * @brief Gets the accumulated cost at this node
    * @return accumulated cost
    */
-  inline float & getAccumulatedCost()
+  inline float getAccumulatedCost()
   {
     return _accumulated_cost;
   }
@@ -263,7 +263,7 @@ class NodeHybrid
    * @brief Gets the costmap cost at this node
    * @return costmap cost
    */
-  inline float & getCost()
+  inline float getCost()
   {
     return _cell_cost;
   }
@@ -272,7 +272,7 @@ class NodeHybrid
    * @brief Gets if cell has been visited in search
    * @param If cell was visited
    */
-  inline bool & wasVisited()
+  inline bool wasVisited()
   {
     return _was_visited;
   }
@@ -289,7 +289,7 @@ class NodeHybrid
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int & getIndex()
+  inline unsigned int getIndex()
   {
     return _index;
   }
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 577cc28e19..807e068c88 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
@@ -105,12 +105,14 @@ struct LatticeMotionTable
   float reverse_penalty;
   float travel_distance_reward;
   float rotation_penalty;
+  float min_turning_radius;
   bool allow_reverse_expansion;
   std::vector<std::vector<MotionPrimitive>> motion_primitives;
   ompl::base::StateSpacePtr state_space;
   std::vector<TrigValues> trig_values;
   std::string current_lattice_filepath;
   LatticeMetadata lattice_metadata;
+  MotionModel motion_model = MotionModel::UNKNOWN;
 };
 
 /**
@@ -183,7 +185,7 @@ class NodeLattice
    * @brief Gets the accumulated cost at this node
    * @return accumulated cost
    */
-  inline float & getAccumulatedCost()
+  inline float getAccumulatedCost()
   {
     return _accumulated_cost;
   }
@@ -201,7 +203,7 @@ class NodeLattice
    * @brief Gets the costmap cost at this node
    * @return costmap cost
    */
-  inline float & getCost()
+  inline float getCost()
   {
     return _cell_cost;
   }
@@ -210,7 +212,7 @@ class NodeLattice
    * @brief Gets if cell has been visited in search
    * @param If cell was visited
    */
-  inline bool & wasVisited()
+  inline bool wasVisited()
   {
     return _was_visited;
   }
@@ -227,7 +229,7 @@ class NodeLattice
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int & getIndex()
+  inline unsigned int getIndex()
   {
     return _index;
   }
diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp
index d199c67775..6baf51f4a4 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp
@@ -43,6 +43,8 @@ struct SearchInfo
   float rotation_penalty{5.0};
   float analytic_expansion_ratio{3.5};
   float analytic_expansion_max_length{60.0};
+  float analytic_expansion_max_cost{200.0};
+  bool analytic_expansion_max_cost_override{false};
   std::string lattice_filepath;
   bool cache_obstacle_heuristic{false};
   bool allow_reverse_expansion{false};
diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
index 55fe35e531..e57db9952a 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -110,7 +110,7 @@ void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision
     _y_size = y_size;
     NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info);
   }
-  _expander->setCollisionChecker(collision_checker);
+  _expander->setCollisionChecker(_collision_checker);
 }
 
 template<typename NodeT>
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index d0e068c634..10cb3bdd5a 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -41,7 +41,7 @@ AnalyticExpansion<NodeT>::AnalyticExpansion(
 
 template<typename NodeT>
 void AnalyticExpansion<NodeT>::setCollisionChecker(
-  GridCollisionChecker * collision_checker)
+  GridCollisionChecker * & collision_checker)
 {
   _collision_checker = collision_checker;
 }
@@ -80,7 +80,8 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     if (analytic_iterations <= 0) {
       // Reset the counter and try the analytic path expansion
       analytic_iterations = desired_iterations;
-      AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, goal_node, getter);
+      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;
@@ -94,7 +95,8 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
             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);
+            refined_analytic_nodes =
+              getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space);
             if (refined_analytic_nodes.empty()) {
               break;
             }
@@ -105,6 +107,50 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
           }
         }
 
+        // 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 setAnalyticPath(node, goal_node, analytic_nodes);
       }
     }
@@ -120,10 +166,10 @@ template<typename NodeT>
 typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<NodeT>::getAnalyticPath(
   const NodePtr & node,
   const NodePtr & goal,
-  const NodeGetter & node_getter)
+  const NodeGetter & node_getter,
+  const ompl::base::StateSpacePtr & state_space)
 {
-  static ompl::base::ScopedState<> from(node->motion_table.state_space), to(
-    node->motion_table.state_space), s(node->motion_table.state_space);
+  static ompl::base::ScopedState<> from(state_space), to(state_space), s(state_space);
   from[0] = node->pose.x;
   from[1] = node->pose.y;
   from[2] = node->motion_table.getAngleFromBin(node->pose.theta);
@@ -131,7 +177,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   to[1] = goal->pose.y;
   to[2] = node->motion_table.getAngleFromBin(goal->pose.theta);
 
-  float d = node->motion_table.state_space->distance(from(), to());
+  float d = state_space->distance(from(), to());
 
   // If the length is too far, exit. This prevents unsafe shortcutting of paths
   // into higher cost areas far out from the goal itself, let search to the work of getting
@@ -142,8 +188,8 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   }
 
   // A move of sqrt(2) is guaranteed to be in a new cell
-  static const float sqrt_2 = std::sqrt(2.);
-  unsigned int num_intervals = std::floor(d / sqrt_2);
+  static const float sqrt_2 = std::sqrt(2.0f);
+  unsigned int num_intervals = static_cast<unsigned int>(std::floor(d / sqrt_2));
 
   AnalyticExpansionNodes possible_nodes;
   // When "from" and "to" are zero or one cell away,
@@ -159,6 +205,8 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   float angle = 0.0;
   Coordinates proposed_coordinates;
   bool failure = false;
+  std::vector<float> node_costs;
+  node_costs.reserve(num_intervals);
 
   // Check intermediary poses (non-goal, non-start)
   for (float i = 1; i <= num_intervals; i++) {
@@ -182,6 +230,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
       if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) {
         // Save the node, and its previous coordinates in case we need to abort
         possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates);
+        node_costs.emplace_back(next->getCost());
         prev = next;
       } else {
         // Abort
@@ -196,6 +245,38 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
     }
   }
 
+  if (!failure) {
+    // We found 'a' valid expansion. Now to tell if its a quality option...
+    const float max_cost = _search_info.analytic_expansion_max_cost;
+    if (*std::max_element(node_costs.begin(), node_costs.end()) > max_cost) {
+      // If any element is above the comfortable cost limit, check edge cases:
+      // (1) Check if goal is in greater than max_cost space requiring
+      //  entering it, but only entering it on final approach, not in-and-out
+      // (2) Checks if goal is in normal space, but enters costed space unnecessarily
+      //  mid-way through, skirting obstacle or in non-globally confined space
+      bool cost_exit_high_cost_region = false;
+      for (auto iter = node_costs.rbegin(); iter != node_costs.rend(); ++iter) {
+        const float & curr_cost = *iter;
+        if (curr_cost <= max_cost) {
+          cost_exit_high_cost_region = true;
+        } else if (curr_cost > max_cost && cost_exit_high_cost_region) {
+          failure = true;
+          break;
+        }
+      }
+
+      // (3) Handle exception: there may be no other option close to goal
+      // if max cost is set too low (optional)
+      if (failure) {
+        if (d < 2.0f * M_PI * goal->motion_table.min_turning_radius &&
+          _search_info.analytic_expansion_max_cost_override)
+        {
+          failure = false;
+        }
+      }
+    }
+  }
+
   // Reset to initial poses to not impact future searches
   for (const auto & node_pose : possible_nodes) {
     const auto & n = node_pose.node;
@@ -256,7 +337,8 @@ typename AnalyticExpansion<Node2D>::AnalyticExpansionNodes AnalyticExpansion<Nod
 getAnalyticPath(
   const NodePtr & node,
   const NodePtr & goal,
-  const NodeGetter & node_getter)
+  const NodeGetter & node_getter,
+  const ompl::base::StateSpacePtr & state_space)
 {
   return AnalyticExpansionNodes();
 }
diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
index 4db86a4d68..c31c836e64 100644
--- a/nav2_smac_planner/src/collision_checker.cpp
+++ b/nav2_smac_planner/src/collision_checker.cpp
@@ -49,7 +49,7 @@ void GridCollisionChecker::setFootprint(
   const bool & radius,
   const double & possible_inscribed_cost)
 {
-  possible_inscribed_cost_ = possible_inscribed_cost;
+  possible_inscribed_cost_ = static_cast<float>(possible_inscribed_cost);
   footprint_is_radius_ = radius;
 
   // Use radius, no caching required
@@ -107,8 +107,8 @@ bool GridCollisionChecker::inCollision(
   if (!footprint_is_radius_) {
     // if footprint, then we check for the footprint's points, but first see
     // if the robot is even potentially in an inscribed collision
-    footprint_cost_ = costmap_->getCost(
-      static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5));
+    footprint_cost_ = static_cast<float>(costmap_->getCost(
+        static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
 
     if (footprint_cost_ < possible_inscribed_cost_) {
       if (possible_inscribed_cost_ > 0) {
@@ -147,7 +147,7 @@ bool GridCollisionChecker::inCollision(
       current_footprint.push_back(new_pt);
     }
 
-    footprint_cost_ = footprintCost(current_footprint);
+    footprint_cost_ = static_cast<float>(footprintCost(current_footprint));
 
     if (footprint_cost_ == UNKNOWN && traverse_unknown) {
       return false;
@@ -157,15 +157,15 @@ bool GridCollisionChecker::inCollision(
     return footprint_cost_ >= OCCUPIED;
   } else {
     // if radius, then we can check the center of the cost assuming inflation is used
-    footprint_cost_ = costmap_->getCost(
-      static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5));
+    footprint_cost_ = static_cast<float>(costmap_->getCost(
+        static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
 
     if (footprint_cost_ == UNKNOWN && traverse_unknown) {
       return false;
     }
 
     // if occupied or unknown and not to traverse unknown space
-    return static_cast<double>(footprint_cost_) >= INSCRIBED;
+    return footprint_cost_ >= INSCRIBED;
   }
 }
 
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
index 02b7bd2d13..24db0e5fc7 100644
--- a/nav2_smac_planner/src/node_hybrid.cpp
+++ b/nav2_smac_planner/src/node_hybrid.cpp
@@ -139,7 +139,7 @@ void HybridMotionTable::initDubin(
   }
 
   // Create the correct OMPL state space
-  state_space = std::make_unique<ompl::base::DubinsStateSpace>(min_turning_radius);
+  state_space = std::make_shared<ompl::base::DubinsStateSpace>(min_turning_radius);
 
   // Precompute projection deltas
   delta_xs.resize(projections.size());
@@ -260,7 +260,7 @@ void HybridMotionTable::initReedsShepp(
   }
 
   // Create the correct OMPL state space
-  state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(min_turning_radius);
+  state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(min_turning_radius);
 
   // Precompute projection deltas
   delta_xs.resize(projections.size());
@@ -748,10 +748,10 @@ void NodeHybrid::precomputeDistanceHeuristic(
 {
   // Dubin or Reeds-Shepp shortest distances
   if (motion_model == MotionModel::DUBIN) {
-    motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(
+    motion_table.state_space = std::make_shared<ompl::base::DubinsStateSpace>(
       search_info.minimum_turning_radius);
   } else if (motion_model == MotionModel::REEDS_SHEPP) {
-    motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
+    motion_table.state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(
       search_info.minimum_turning_radius);
   } else {
     throw std::runtime_error(
diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp
index dfdf47ff4b..0076979c44 100644
--- a/nav2_smac_planner/src/node_lattice.cpp
+++ b/nav2_smac_planner/src/node_lattice.cpp
@@ -64,6 +64,7 @@ void LatticeMotionTable::initMotionModel(
   current_lattice_filepath = search_info.lattice_filepath;
   allow_reverse_expansion = search_info.allow_reverse_expansion;
   rotation_penalty = search_info.rotation_penalty;
+  min_turning_radius = search_info.minimum_turning_radius;
 
   // Get the metadata about this minimum control set
   lattice_metadata = getLatticeMetadata(current_lattice_filepath);
@@ -77,11 +78,13 @@ void LatticeMotionTable::initMotionModel(
 
   if (!state_space) {
     if (!allow_reverse_expansion) {
-      state_space = std::make_unique<ompl::base::DubinsStateSpace>(
+      state_space = std::make_shared<ompl::base::DubinsStateSpace>(
         lattice_metadata.min_turning_radius);
+      motion_model = MotionModel::DUBIN;
     } else {
-      state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
+      state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(
         lattice_metadata.min_turning_radius);
+      motion_model = MotionModel::REEDS_SHEPP;
     }
   }
 
@@ -426,11 +429,13 @@ void NodeLattice::precomputeDistanceHeuristic(
 {
   // Dubin or Reeds-Shepp shortest distances
   if (!search_info.allow_reverse_expansion) {
-    motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(
+    motion_table.state_space = std::make_shared<ompl::base::DubinsStateSpace>(
       search_info.minimum_turning_radius);
+    motion_table.motion_model = MotionModel::DUBIN;
   } else {
-    motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
+    motion_table.state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(
       search_info.minimum_turning_radius);
+    motion_table.motion_model = MotionModel::REEDS_SHEPP;
   }
   motion_table.lattice_metadata =
     LatticeMotionTable::getLatticeMetadata(search_info.lattice_filepath);
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
index a56c3af3f8..12d4c2976f 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -125,6 +125,15 @@ void SmacPlannerHybrid::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5));
   node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".analytic_expansion_max_cost", rclcpp::ParameterValue(200.0));
+  node->get_parameter(
+    name + ".analytic_expansion_max_cost", _search_info.analytic_expansion_max_cost);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".analytic_expansion_max_cost_override", rclcpp::ParameterValue(false));
+  node->get_parameter(
+    name + ".analytic_expansion_max_cost_override",
+    _search_info.analytic_expansion_max_cost_override);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".use_quadratic_cost_penalty", rclcpp::ParameterValue(false));
   node->get_parameter(
@@ -548,6 +557,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
         reinit_a_star = true;
         _search_info.analytic_expansion_max_length =
           static_cast<float>(parameter.as_double()) / _costmap->getResolution();
+      } else if (name == _name + ".analytic_expansion_max_cost") {
+        reinit_a_star = true;
+        _search_info.analytic_expansion_max_cost = static_cast<float>(parameter.as_double());
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == _name + ".downsample_costmap") {
@@ -568,6 +580,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
         } else {
           _smoother.reset();
         }
+      } else if (name == _name + ".analytic_expansion_max_cost_override") {
+        _search_info.analytic_expansion_max_cost_override = parameter.as_bool();
+        reinit_a_star = true;
       }
     } else if (type == ParameterType::PARAMETER_INTEGER) {
       if (name == _name + ".downsampling_factor") {
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index 083ad1f952..a5f16bcdda 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -111,6 +111,15 @@ void SmacPlannerLattice::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5));
   node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".analytic_expansion_max_cost", rclcpp::ParameterValue(200.0));
+  node->get_parameter(
+    name + ".analytic_expansion_max_cost", _search_info.analytic_expansion_max_cost);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".analytic_expansion_max_cost_override", rclcpp::ParameterValue(false));
+  node->get_parameter(
+    name + ".analytic_expansion_max_cost_override",
+    _search_info.analytic_expansion_max_cost_override);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0));
   node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m);
@@ -465,6 +474,9 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
         reinit_a_star = true;
         _search_info.analytic_expansion_max_length =
           static_cast<float>(parameter.as_double()) / _costmap->getResolution();
+      } else if (name == _name + ".analytic_expansion_max_cost") {
+        reinit_a_star = true;
+        _search_info.analytic_expansion_max_cost = static_cast<float>(parameter.as_double());
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == _name + ".allow_unknown") {
@@ -482,6 +494,9 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
         } else {
           _smoother.reset();
         }
+      } else if (name == _name + ".analytic_expansion_max_cost_override") {
+        _search_info.analytic_expansion_max_cost_override = parameter.as_bool();
+        reinit_a_star = true;
       }
     } else if (type == ParameterType::PARAMETER_INTEGER) {
       if (name == _name + ".max_iterations") {