diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
index 2360d09c253..030a85f38cd 100644
--- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp
+++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -35,6 +35,7 @@
 #include "nav2_amcl/sensors/laser/laser.hpp"
 #include "nav2_msgs/msg/particle.hpp"
 #include "nav2_msgs/msg/particle_cloud.hpp"
+#include "nav2_msgs/srv/set_initial_pose.hpp"
 #include "nav_msgs/srv/set_map.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "std_srvs/srv/empty.hpp"
@@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode
     const std::shared_ptr<std_srvs::srv::Empty::Request> request,
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
+  // service server for providing an initial pose guess
+  rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
+  /*
+   * @brief Service callback for an initial pose guess request
+   */
+  void initialPoseReceivedSrv(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request,
+    std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
+
   // Let amcl update samples without requiring motion
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
   /*
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index 192111439f4..b311b7ac5e9 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -325,7 +325,9 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   // Get rid of the inputs first (services and message filter input), so we
   // don't continue to process incoming messages
   global_loc_srv_.reset();
+  initial_guess_srv_.reset();
   nomotion_update_srv_.reset();
+  executor_thread_.reset();  //  to make sure initial_pose_sub_ completely exit
   initial_pose_sub_.reset();
   laser_scan_connection_.disconnect();
   tf_listener_.reset();  //  listener may access lase_scan_filter_, so it should be reset earlier
@@ -494,6 +496,15 @@ AmclNode::globalLocalizationCallback(
   pf_init_ = false;
 }
 
+void
+AmclNode::initialPoseReceivedSrv(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> req,
+  std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response>/*res*/)
+{
+  initialPoseReceived(std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(req->pose));
+}
+
 // force nomotion updates (amcl updating without requiring motion)
 void
 AmclNode::nomotionUpdateCallback(
@@ -1103,20 +1114,20 @@ AmclNode::initParameters()
   // Semantic checks
   if (laser_likelihood_max_dist_ < 0) {
     RCLCPP_WARN(
-      get_logger(), "You've set laser_likelihood_max_dist to be negtive,"
+      get_logger(), "You've set laser_likelihood_max_dist to be negative,"
       " this isn't allowed so it will be set to default value 2.0.");
     laser_likelihood_max_dist_ = 2.0;
   }
   if (max_particles_ < 0) {
     RCLCPP_WARN(
-      get_logger(), "You've set max_particles to be negtive,"
+      get_logger(), "You've set max_particles to be negative,"
       " this isn't allowed so it will be set to default value 2000.");
     max_particles_ = 2000;
   }
 
   if (min_particles_ < 0) {
     RCLCPP_WARN(
-      get_logger(), "You've set min_particles to be negtive,"
+      get_logger(), "You've set min_particles to be negative,"
       " this isn't allowed so it will be set to default value 500.");
     min_particles_ = 500;
   }
@@ -1130,7 +1141,7 @@ AmclNode::initParameters()
 
   if (resample_interval_ <= 0) {
     RCLCPP_WARN(
-      get_logger(), "You've set resample_interval to be zero or negtive,"
+      get_logger(), "You've set resample_interval to be zero or negative,"
       " this isn't allowed so it will be set to default value to 1.");
     resample_interval_ = 1;
   }
@@ -1543,6 +1554,10 @@ AmclNode::initServices()
     "reinitialize_global_localization",
     std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3));
 
+  initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
+    "set_initial_pose",
+    std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3));
+
   nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
     "request_nomotion_update",
     std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3));
diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
index 491e64ecf1e..1148c2828db 100644
--- a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
+++ b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
@@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
 
   self = reinterpret_cast<LikelihoodFieldModel *>(data->laser);
 
+  // Pre-compute a couple of things
+  double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
+  double z_rand_mult = 1.0 / data->range_max;
+
+  step = (data->range_count - 1) / (self->max_beams_ - 1);
+
+  // Step size must be at least 1
+  if (step < 1) {
+    step = 1;
+  }
+
   total_weight = 0.0;
 
   // Compute the sample weights
@@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
 
     p = 1.0;
 
-    // Pre-compute a couple of things
-    double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
-    double z_rand_mult = 1.0 / data->range_max;
-
-    step = (data->range_count - 1) / (self->max_beams_ - 1);
-
-    // Step size must be at least 1
-    if (step < 1) {
-      step = 1;
-    }
-
     for (i = 0; i < data->range_count; i += step) {
       obs_range = data->ranges[i][0];
       obs_bearing = data->ranges[i][1];
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
index 6193a91a868..846f8d13853 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -97,8 +97,9 @@ class BtActionNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
     if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
-        action_name.c_str());
+        node_->get_logger(), "\"%s\" action server not available after waiting for %f s",
+        action_name.c_str(),
+        wait_for_service_timeout_.count() / 1000.0);
       throw std::runtime_error(
               std::string("Action server ") + action_name +
               std::string(" not available"));
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
index 331ca237c8f..2b323a67985 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -251,6 +251,9 @@ class BtActionServer
   // The timeout value for waiting for a service to response
   std::chrono::milliseconds wait_for_service_timeout_;
 
+  // should the BT be reloaded even if the same xml filename is requested?
+  bool always_reload_bt_xml_ = false;
+
   // User-provided callbacks
   OnGoalReceivedCallback on_goal_received_callback_;
   OnLoopCallback on_loop_callback_;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index 5e005fc610d..deaeea1bdc2 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -64,6 +64,12 @@ BtActionServer<ActionT>::BtActionServer(
   if (!node->has_parameter("action_server_result_timeout")) {
     node->declare_parameter("action_server_result_timeout", 900.0);
   }
+  if (!node->has_parameter("always_reload_bt_xml")) {
+    node->declare_parameter("always_reload_bt_xml", false);
+  }
+  if (!node->has_parameter("wait_for_service_timeout")) {
+    node->declare_parameter("wait_for_service_timeout", 1000);
+  }
 
   std::vector<std::string> error_code_names = {
     "follow_path_error_code",
@@ -161,6 +167,7 @@ bool BtActionServer<ActionT>::on_configure()
   int wait_for_service_timeout;
   node->get_parameter("wait_for_service_timeout", wait_for_service_timeout);
   wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout);
+  node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_);
 
   // Get error code id names to grab off of the blackboard
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
@@ -220,8 +227,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   // Empty filename is default for backward compatibility
   auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
 
-  // Use previous BT if it is the existing one
-  if (current_bt_xml_filename_ == filename) {
+  // Use previous BT if it is the existing one and always reload flag is not set to true
+  if (!always_reload_bt_xml_ && current_bt_xml_filename_ == filename) {
     RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded");
     return true;
   }
diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp
index 26e929a5805..fd5f986c76a 100644
--- a/nav2_graceful_controller/test/test_graceful_controller.cpp
+++ b/nav2_graceful_controller/test/test_graceful_controller.cpp
@@ -814,7 +814,7 @@ TEST(GracefulControllerTest, pruningPlan) {
 
   // Check results: the plan should be pruned
   auto transformed_plan = controller->transformGlobalPlan(robot_pose);
-  EXPECT_EQ(transformed_plan.poses.size(), 3);
+  EXPECT_EQ(transformed_plan.poses.size(), 3u);
 }
 
 TEST(GracefulControllerTest, pruningPlanOutsideCostmap) {
@@ -880,7 +880,7 @@ TEST(GracefulControllerTest, pruningPlanOutsideCostmap) {
 
   // Check results: the plan should be pruned
   auto transformed_plan = controller->transformGlobalPlan(robot_pose);
-  EXPECT_EQ(transformed_plan.poses.size(), 2);
+  EXPECT_EQ(transformed_plan.poses.size(), 2u);
 }
 
 TEST(GracefulControllerTest, computeVelocityCommandRotate) {
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index 206ae86322b..12637abca6a 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -38,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/ManageLifecycleNodes.srv"
   "srv/LoadMap.srv"
   "srv/SaveMap.srv"
+  "srv/SetInitialPose.srv"
   "action/AssistedTeleop.action"
   "action/BackUp.action"
   "action/ComputePathToPose.action"
diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv
new file mode 100644
index 00000000000..ec1e50b65f1
--- /dev/null
+++ b/nav2_msgs/srv/SetInitialPose.srv
@@ -0,0 +1,3 @@
+geometry_msgs/PoseWithCovarianceStamped pose
+---
+
diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
index 6f84b05382d..78beab89fea 100644
--- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
+++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_regulated_pure_pursuit_controller)
 
 find_package(ament_cmake REQUIRED)
+find_package(nav2_amcl REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
@@ -26,6 +27,7 @@ set(dependencies
   nav2_costmap_2d
   pluginlib
   nav_msgs
+  nav2_amcl
   nav2_util
   nav2_core
   tf2
diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md
index 350b1926da4..57b820b16d5 100644
--- a/nav2_regulated_pure_pursuit_controller/README.md
+++ b/nav2_regulated_pure_pursuit_controller/README.md
@@ -90,6 +90,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | 
 | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | 
 | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | 
+| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvate calculation (to avoid oscilaltions at the end of the path) | 
 
 Example fully-described XML with default parameter values:
 
@@ -137,6 +138,7 @@ controller_server:
       rotate_to_heading_min_angle: 0.785
       max_angular_accel: 3.2
       max_robot_pose_search_dist: 10.0
+      interpolate_curvature_after_goal: false
       cost_scaling_dist: 0.3
       cost_scaling_gain: 1.0
       inflation_cost_scaling_factor: 3.0
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
index 39f5de6fa0c..d6ca585a49a 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
@@ -56,6 +56,7 @@ struct Parameters
   double rotate_to_heading_min_angle;
   bool allow_reversing;
   double max_robot_pose_search_dist;
+  bool interpolate_curvature_after_goal;
   bool use_collision_detection;
   double transform_tolerance;
 };
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
index 22bc0266a66..1e603f3c8bb 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
@@ -58,11 +58,12 @@ class PathHandler
    * - Outside the local_costmap (collision avoidance cannot be assured)
    * @param pose pose to transform
    * @param max_robot_pose_search_dist Distance to search for matching nearest path point
+   * @param reject_unit_path If true, fail if path has only one pose
    * @return Path in new frame
    */
   nav_msgs::msg::Path transformGlobalPlan(
     const geometry_msgs::msg::PoseStamped & pose,
-    double max_robot_pose_search_dist);
+    double max_robot_pose_search_dist, bool reject_unit_path = false);
 
   /**
    * @brief Transform a pose to another frame.
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
index 7b28ca9720e..417dc1c811f 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
@@ -131,10 +131,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @brief Whether robot should rotate to rough path heading
    * @param carrot_pose current lookahead point
    * @param angle_to_path Angle of robot output relatie to carrot marker
+   * @param x_vel_sign Velocoty sign (forward or backward)
    * @return Whether should rotate to path heading
    */
   bool shouldRotateToPath(
-    const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path);
+    const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path,
+    double & x_vel_sign);
 
   /**
    * @brief Whether robot should rotate to final goal orientation
@@ -185,9 +187,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @brief Get lookahead point
    * @param lookahead_dist Optimal lookahead distance
    * @param path Current global path
+   * @param interpolate_after_goal If true, interpolate the lookahead point after the goal based
+   * on the orientation given by the position of the last two pose of the path
    * @return Lookahead point
    */
-  geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &);
+  geometry_msgs::msg::PoseStamped getLookAheadPoint(
+    const double &, const nav_msgs::msg::Path &,
+    bool interpolate_after_goal = false);
 
   /**
    * @brief checks for the cusp position
@@ -210,6 +216,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
   carrot_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
+  curvature_carrot_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
   std::unique_ptr<nav2_regulated_pure_pursuit_controller::PathHandler> path_handler_;
   std::unique_ptr<nav2_regulated_pure_pursuit_controller::ParameterHandler> param_handler_;
diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml
index 5b62ce5aeaa..c56147a9693 100644
--- a/nav2_regulated_pure_pursuit_controller/package.xml
+++ b/nav2_regulated_pure_pursuit_controller/package.xml
@@ -10,6 +10,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>nav2_amcl</depend>
   <depend>nav2_common</depend>
   <depend>nav2_core</depend>
   <depend>nav2_util</depend>
diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
index 016a626c288..dea2bc59a43 100644
--- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
@@ -90,6 +90,9 @@ ParameterHandler::ParameterHandler(
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_robot_pose_search_dist",
     rclcpp::ParameterValue(costmap_size_x / 2.0));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".interpolate_curvature_after_goal",
+    rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".use_collision_detection",
     rclcpp::ParameterValue(true));
@@ -159,6 +162,15 @@ ParameterHandler::ParameterHandler(
     params_.max_robot_pose_search_dist = std::numeric_limits<double>::max();
   }
 
+  node->get_parameter(
+    plugin_name_ + ".interpolate_curvature_after_goal",
+    params_.interpolate_curvature_after_goal);
+  if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) {
+    RCLCPP_WARN(
+      logger_, "For interpolate_curvature_after_goal to be set to true, "
+      "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling.");
+    params_.interpolate_curvature_after_goal = false;
+  }
   node->get_parameter(
     plugin_name_ + ".use_collision_detection",
     params_.use_collision_detection);
@@ -170,16 +182,6 @@ ParameterHandler::ParameterHandler(
     params_.use_cost_regulated_linear_velocity_scaling = false;
   }
 
-  /** Possible to drive in reverse direction if and only if
-   "use_rotate_to_heading" parameter is set to false **/
-
-  if (params_.use_rotate_to_heading && params_.allow_reversing) {
-    RCLCPP_WARN(
-      logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing "
-      "parameter cannot be set to true. By default setting use_rotate_to_heading true");
-    params_.allow_reversing = false;
-  }
-
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(
       &ParameterHandler::dynamicParametersCallback,
@@ -250,12 +252,6 @@ ParameterHandler::dynamicParametersCallback(
       } else if (name == plugin_name_ + ".use_collision_detection") {
         params_.use_collision_detection = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_rotate_to_heading") {
-        if (parameter.as_bool() && params_.allow_reversing) {
-          RCLCPP_WARN(
-            logger_, "Both use_rotate_to_heading and allow_reversing "
-            "parameter cannot be set to true. Rejecting parameter update.");
-          continue;
-        }
         params_.use_rotate_to_heading = parameter.as_bool();
       } else if (name == plugin_name_ + ".allow_reversing") {
         if (params_.use_rotate_to_heading && parameter.as_bool()) {
diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp
index 90b7c4a3e0c..66b19be3237 100644
--- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp
@@ -47,12 +47,17 @@ double PathHandler::getCostmapMaxExtent() const
 
 nav_msgs::msg::Path PathHandler::transformGlobalPlan(
   const geometry_msgs::msg::PoseStamped & pose,
-  double max_robot_pose_search_dist)
+  double max_robot_pose_search_dist,
+  bool reject_unit_path)
 {
   if (global_plan_.poses.empty()) {
     throw nav2_core::InvalidPath("Received plan with zero length");
   }
 
+  if (reject_unit_path && global_plan_.poses.size() == 1) {
+    throw nav2_core::InvalidPath("Received plan with length of one");
+  }
+
   // let's get the pose of the robot in the frame of the plan
   geometry_msgs::msg::PoseStamped robot_pose;
   if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) {
@@ -73,6 +78,15 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
       return euclidean_distance(robot_pose, ps);
     });
 
+  // Make sure we always have at least 2 points on the transformed plan and that we don't prune
+  // the global plan below 2 points in order to have always enough point to interpolate the
+  // end of path direction
+  if (global_plan_.poses.begin() != closest_pose_upper_bound && global_plan_.poses.size() > 1 &&
+    transformation_begin == std::prev(closest_pose_upper_bound))
+  {
+    transformation_begin = std::prev(std::prev(closest_pose_upper_bound));
+  }
+
   // We'll discard points on the plan that are outside the local costmap
   const double max_costmap_extent = getCostmapMaxExtent();
   auto transformation_end = std::find_if(
diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
index 76837754e86..09012a9fb3e 100644
--- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
@@ -20,6 +20,7 @@
 #include <vector>
 #include <utility>
 
+#include "nav2_amcl/angleutils.hpp"
 #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -73,6 +74,8 @@ void RegulatedPurePursuitController::configure(
 
   global_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("received_global_plan", 1);
   carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>("lookahead_point", 1);
+  curvature_carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
+    "curvature_lookahead_point", 1);
 }
 
 void RegulatedPurePursuitController::cleanup()
@@ -84,6 +87,7 @@ void RegulatedPurePursuitController::cleanup()
     plugin_name_.c_str());
   global_path_pub_.reset();
   carrot_pub_.reset();
+  curvature_carrot_pub_.reset();
 }
 
 void RegulatedPurePursuitController::activate()
@@ -95,6 +99,7 @@ void RegulatedPurePursuitController::activate()
     plugin_name_.c_str());
   global_path_pub_->on_activate();
   carrot_pub_->on_activate();
+  curvature_carrot_pub_->on_activate();
 }
 
 void RegulatedPurePursuitController::deactivate()
@@ -106,6 +111,7 @@ void RegulatedPurePursuitController::deactivate()
     plugin_name_.c_str());
   global_path_pub_->on_deactivate();
   carrot_pub_->on_deactivate();
+  curvature_carrot_pub_->on_deactivate();
 }
 
 std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController::createCarrotMsg(
@@ -171,7 +177,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   // Transform path to robot base frame
   auto transformed_plan = path_handler_->transformGlobalPlan(
-    pose, params_->max_robot_pose_search_dist);
+    pose, params_->max_robot_pose_search_dist, params_->interpolate_curvature_after_goal);
   global_path_pub_->publish(transformed_plan);
 
   // Find look ahead distance and point on path and publish
@@ -190,6 +196,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   // Get the particular point on the path at the lookahead distance
   auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);
+  auto rotate_to_path_carrot_pose = carrot_pose;
   carrot_pub_->publish(createCarrotMsg(carrot_pose));
 
   double linear_vel, angular_vel;
@@ -200,33 +207,39 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   if (params_->use_fixed_curvature_lookahead) {
     auto curvature_lookahead_pose = getLookAheadPoint(
       params_->curvature_lookahead_dist,
-      transformed_plan);
+      transformed_plan, params_->interpolate_curvature_after_goal);
+    rotate_to_path_carrot_pose = curvature_lookahead_pose;
     regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position);
+    curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose));
   }
 
   // Setting the velocity direction
-  double sign = 1.0;
+  double x_vel_sign = 1.0;
   if (params_->allow_reversing) {
-    sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
+    x_vel_sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
   }
 
   linear_vel = params_->desired_linear_vel;
 
   // Make sure we're in compliance with basic constraints
+  // For shouldRotateToPath, using x_vel_sign in order to support allow_reversing
+  // and rotate_to_path_carrot_pose for the direction carrot pose:
+  //        - equal to "normal" carrot_pose when curvature_lookahead_pose = false
+  //        - otherwise equal to curvature_lookahead_pose (which can be interpolated after goal)
   double angle_to_heading;
   if (shouldRotateToGoalHeading(carrot_pose)) {
     double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation);
     rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed);
-  } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) {
+  } else if (shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)) {
     rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed);
   } else {
     applyConstraints(
       regulation_curvature, speed,
       collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan,
-      linear_vel, sign);
+      linear_vel, x_vel_sign);
 
     // Apply curvature to angular velocity after constraining linear velocity
-    angular_vel = linear_vel * lookahead_curvature;
+    angular_vel = linear_vel * regulation_curvature;
   }
 
   // Collision checking on this velocity heading
@@ -246,10 +259,15 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 }
 
 bool RegulatedPurePursuitController::shouldRotateToPath(
-  const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path)
+  const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path,
+  double & x_vel_sign)
 {
   // Whether we should rotate robot to rough path heading
   angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x);
+  // In case we are reversing
+  if (x_vel_sign < 0.0) {
+    angle_to_path = nav2_amcl::angleutils::normalize(angle_to_path + M_PI);
+  }
   return params_->use_rotate_to_heading &&
          fabs(angle_to_path) > params_->rotate_to_heading_min_angle;
 }
@@ -314,7 +332,8 @@ geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersect
 
 geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(
   const double & lookahead_dist,
-  const nav_msgs::msg::Path & transformed_plan)
+  const nav_msgs::msg::Path & transformed_plan,
+  bool interpolate_after_goal)
 {
   // Find the first pose which is at a distance greater than the lookahead distance
   auto goal_pose_it = std::find_if(
@@ -324,7 +343,32 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin
 
   // If the no pose is not far enough, take the last pose
   if (goal_pose_it == transformed_plan.poses.end()) {
-    goal_pose_it = std::prev(transformed_plan.poses.end());
+    if (interpolate_after_goal) {
+      auto last_pose_it = std::prev(transformed_plan.poses.end());
+      auto prev_last_pose_it = std::prev(last_pose_it);
+
+      double end_path_orientation = atan2(
+        last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y,
+        last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x);
+
+      // Project the last segment out to guarantee it is beyond the look ahead
+      // distance
+      auto projected_position = last_pose_it->pose.position;
+      projected_position.x += cos(end_path_orientation) * lookahead_dist;
+      projected_position.y += sin(end_path_orientation) * lookahead_dist;
+
+      // Use the circle intersection to find the position at the correct look
+      // ahead distance
+      const auto interpolated_position = circleSegmentIntersection(
+        last_pose_it->pose.position, projected_position, lookahead_dist);
+
+      geometry_msgs::msg::PoseStamped interpolated_pose;
+      interpolated_pose.header = last_pose_it->header;
+      interpolated_pose.pose.position = interpolated_position;
+      return interpolated_pose;
+    } else {
+      goal_pose_it = std::prev(transformed_plan.poses.end());
+    }
   } else if (goal_pose_it != transformed_plan.poses.begin()) {
     // Find the point on the line segment between the two poses
     // that is exactly the lookahead distance away from the robot pose (the origin)
diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
index 1557dad2d2d..38cc195bc61 100644
--- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
+++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
@@ -68,6 +68,14 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
     return circleSegmentIntersection(p1, p2, r);
   }
 
+  geometry_msgs::msg::PoseStamped
+  projectCarrotPastGoalWrapper(
+    const double & dist,
+    const nav_msgs::msg::Path & path)
+  {
+    return getLookAheadPoint(dist, path, true);
+  }
+
   geometry_msgs::msg::PoseStamped getLookAheadPointWrapper(
     const double & dist, const nav_msgs::msg::Path & path)
   {
@@ -77,7 +85,8 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
   bool shouldRotateToPathWrapper(
     const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path)
   {
-    return shouldRotateToPath(carrot_pose, angle_to_path);
+    double x_vel_sign = 1.0;
+    return shouldRotateToPath(carrot_pose, angle_to_path, x_vel_sign);
   }
 
   bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose)
@@ -330,6 +339,115 @@ INSTANTIATE_TEST_SUITE_P(
 }
 ));
 
+TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
+  auto ctrl = std::make_shared<BasicAPIRPP>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPP");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto costmap =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+  ctrl->configure(node, name, tf, costmap);
+
+  double EPSILON = std::numeric_limits<float>::epsilon();
+
+  nav_msgs::msg::Path path;
+  // More than 2 poses
+  path.poses.resize(4);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[1].pose.position.x = 1.0;
+  path.poses[2].pose.position.x = 2.0;
+  path.poses[3].pose.position.x = 3.0;
+  auto pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses fwd
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[1].pose.position.x = 3.0;
+  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses at 45°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = 3.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at 90°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = 0.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at 135°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = -3.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses bck
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[1].pose.position.x = -3.0;
+  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses at -135°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = -3.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at -90°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = 0.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at -45°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = 3.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
+  EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON);
+}
+
 TEST(RegulatedPurePursuitTest, lookaheadAPI)
 {
   auto ctrl = std::make_shared<BasicAPIRPP>();
diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md
index 6e92209068d..a9f59c4e6a0 100644
--- a/nav2_smac_planner/README.md
+++ b/nav2_smac_planner/README.md
@@ -134,7 +134,7 @@ planner_server:
         max_iterations: 1000
         w_smooth: 0.3
         w_data: 0.2
-        tolerance: 1e-10
+        tolerance: 1.0e-10
         do_refinement: true               # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further
 ```
 
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 967d4f65e94..ba728247b4a 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
@@ -75,7 +75,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
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index 893f2ceb484..0590da7f1cc 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;
 }
diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
index 2f23442097a..c9ce030fab5 100644
--- a/nav2_smac_planner/src/collision_checker.cpp
+++ b/nav2_smac_planner/src/collision_checker.cpp
@@ -66,6 +66,7 @@ void GridCollisionChecker::setFootprint(
     return;
   }
 
+  oriented_footprints_.clear();
   oriented_footprints_.reserve(angles_.size());
   double sin_th, cos_th;
   geometry_msgs::msg::Point new_pt;
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
index 5c597c8c754..a614e8e179f 100644
--- a/nav2_smac_planner/src/node_hybrid.cpp
+++ b/nav2_smac_planner/src/node_hybrid.cpp
@@ -662,10 +662,10 @@ float NodeHybrid::getObstacleHeuristic(
         if (existing_cost <= 0.0f) {
           if (motion_table.use_quadratic_cost_penalty) {
             travel_cost =
-              (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 64516.0f));  // 254^2
+              (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 63504.0f));  // 252^2
           } else {
             travel_cost =
-              ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 254.0f));
+              ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 252.0f));
           }
 
           new_cost = c_cost + travel_cost;
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
index 16fa22b032c..382be82da65 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -352,6 +352,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   }
 
   // Set collision checker and costmap information
+  _collision_checker.setFootprint(
+    _costmap_ros->getRobotFootprint(),
+    _costmap_ros->getUseRadius(),
+    findCircumscribedCost(_costmap_ros));
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point, in A* bin search coordinates
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index 3e1e0962acc..8442d170f45 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -289,6 +289,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
 
   // Set collision checker and costmap information
+  _collision_checker.setFootprint(
+    _costmap_ros->getRobotFootprint(),
+    _costmap_ros->getUseRadius(),
+    findCircumscribedCost(_costmap_ros));
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point, in A* bin search coordinates
diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp
index eec9c2dd9f7..c8b1660840f 100644
--- a/nav2_util/include/nav2_util/simple_action_server.hpp
+++ b/nav2_util/include/nav2_util/simple_action_server.hpp
@@ -315,7 +315,7 @@ class SimpleActionServer
       if (steady_clock::now() - start_time >= server_timeout_) {
         terminate_all();
         if (completion_callback_) {completion_callback_();}
-        throw std::runtime_error("Action callback is still running and missed deadline to stop");
+        error_msg("Action callback is still running and missed deadline to stop");
       }
     }