diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
index 9837dd93c82..c66d32257e7 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
@@ -114,24 +114,14 @@ class PathHandler
   geometry_msgs::msg::PoseStamped
   transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose);
 
-  /**
-    * @brief Transform a plan to the costmap reference frame
-    * @param begin Start of path to transform
-    * @param end End of path to transform
-    * @param stamp Timestamp to use for transformation
-    * @return output path in costmap reference frame
-    */
-  nav_msgs::msg::Path
-  transformPlanPosesToCostmapFrame(
-    PathIterator begin, PathIterator end,
-    const builtin_interfaces::msg::Time & stamp);
-
   /**
     * @brief Get global plan within window of the local costmap size
     * @param global_pose Robot pose
-    * @return Range of path iterators belonging to the path within costmap window
+    * @return plan transformed in the costmap frame and iterator to the first pose of the global
+    * plan (for pruning)
     */
-  PathRange getGlobalPlanConsideringBounds(const geometry_msgs::msg::PoseStamped & global_pose);
+  std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(
+    const geometry_msgs::msg::PoseStamped & global_pose);
 
   /**
     * @brief Prune global path to only interesting portions
diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp
index 9768eeccbf4..83ec415e172 100644
--- a/nav2_mppi_controller/src/path_handler.cpp
+++ b/nav2_mppi_controller/src/path_handler.cpp
@@ -37,10 +37,12 @@ void PathHandler::initialize(
   getParam(transform_tolerance_, "transform_tolerance", 0.1);
 }
 
-PathRange PathHandler::getGlobalPlanConsideringBounds(
+std::pair<nav_msgs::msg::Path, PathIterator>
+PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
   const geometry_msgs::msg::PoseStamped & global_pose)
 {
   using nav2_util::geometry_utils::euclidean_distance;
+
   auto begin = global_plan_.poses.begin();
   auto end = global_plan_.poses.end();
 
@@ -55,19 +57,38 @@ PathRange PathHandler::getGlobalPlanConsideringBounds(
       return euclidean_distance(global_pose, ps);
     });
 
-  // Find the furthest relevent point on the path to consider within costmap
-  // bounds
-  const auto * costmap = costmap_->getCostmap();
+  nav_msgs::msg::Path transformed_plan;
+  transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
+  transformed_plan.header.stamp = global_pose.header.stamp;
+
   unsigned int mx, my;
-  auto last_point =
-    std::find_if(
-    closest_point, end, [&](const geometry_msgs::msg::PoseStamped & global_plan_pose) {
-      auto distance = euclidean_distance(global_pose, global_plan_pose);
-      return distance >= prune_distance_ || !costmap->worldToMap(
-        global_plan_pose.pose.position.x, global_plan_pose.pose.position.y, mx, my);
-    });
+  // Find the furthest relevent pose on the path to consider within costmap
+  // bounds
+  // Transforming it to the costmap frame in the same loop
+  for (auto global_plan_pose = closest_point; global_plan_pose != end; ++global_plan_pose) {
+    // Distance relative to robot pose check
+    auto distance = euclidean_distance(global_pose, *global_plan_pose);
+    if (distance >= prune_distance_) {
+      return {transformed_plan, closest_point};
+    }
+
+    // Transform from global plan frame to costmap frame
+    geometry_msgs::msg::PoseStamped costmap_plan_pose;
+    global_plan_pose->header.stamp = global_pose.header.stamp;
+    transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose);
+
+    // Check if pose is inside the costmap
+    if (!costmap_->getCostmap()->worldToMap(
+        costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
+    {
+      return {transformed_plan, closest_point};
+    }
+
+    // Filling the transformed plan to return with the transformed pose
+    transformed_plan.poses.push_back(costmap_plan_pose);
+  }
 
-  return {closest_point, last_point};
+  return {transformed_plan, closest_point};
 }
 
 geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
@@ -92,12 +113,7 @@ nav_msgs::msg::Path PathHandler::transformPath(
   // Find relevent bounds of path to use
   geometry_msgs::msg::PoseStamped global_pose =
     transformToGlobalPlanFrame(robot_pose);
-  auto [lower_bound, upper_bound] = getGlobalPlanConsideringBounds(global_pose);
-
-  // Transform these bounds into the local costmap frame and prune older points
-  const auto & stamp = global_pose.header.stamp;
-  nav_msgs::msg::Path transformed_plan =
-    transformPlanPosesToCostmapFrame(lower_bound, upper_bound, stamp);
+  auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
 
   pruneGlobalPlan(lower_bound);
 
@@ -136,31 +152,6 @@ double PathHandler::getMaxCostmapDist()
          costmap->getResolution() / 2.0;
 }
 
-nav_msgs::msg::Path PathHandler::transformPlanPosesToCostmapFrame(
-  PathIterator begin, PathIterator end, const builtin_interfaces::msg::Time & stamp)
-{
-  std::string frame = costmap_->getGlobalFrameID();
-  auto transformToFrame = [&](const auto & global_plan_pose) {
-      geometry_msgs::msg::PoseStamped from_pose;
-      geometry_msgs::msg::PoseStamped to_pose;
-
-      from_pose.header.frame_id = global_plan_.header.frame_id;
-      from_pose.header.stamp = stamp;
-      from_pose.pose = global_plan_pose.pose;
-
-      transformPose(frame, from_pose, to_pose);
-      return to_pose;
-    };
-
-  nav_msgs::msg::Path plan;
-  plan.header.frame_id = frame;
-  plan.header.stamp = stamp;
-
-  std::transform(begin, end, std::back_inserter(plan.poses), transformToFrame);
-
-  return plan;
-}
-
 void PathHandler::setPath(const nav_msgs::msg::Path & plan)
 {
   global_plan_ = plan;
diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp
index e2427ac475d..2e1179dd5b6 100644
--- a/nav2_mppi_controller/test/path_handler_test.cpp
+++ b/nav2_mppi_controller/test/path_handler_test.cpp
@@ -18,6 +18,7 @@
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_mppi_controller/tools/path_handler.hpp"
+#include "tf2_ros/transform_broadcaster.h"
 
 // Tests path handling
 
@@ -47,9 +48,10 @@ class PathHandlerWrapper : public PathHandler
     return getMaxCostmapDist();
   }
 
-  PathRange getGlobalPlanConsideringBoundsWrapper(const geometry_msgs::msg::PoseStamped & pose)
+  std::pair<nav_msgs::msg::Path, PathIterator>
+  getGlobalPlanConsideringBoundsInCostmapFrameWrapper(const geometry_msgs::msg::PoseStamped & pose)
   {
-    return getGlobalPlanConsideringBounds(pose);
+    return getGlobalPlanConsideringBoundsInCostmapFrame(pose);
   }
 
   bool transformPoseWrapper(
@@ -59,12 +61,6 @@ class PathHandlerWrapper : public PathHandler
     return transformPose(frame, in_pose, out_pose);
   }
 
-  nav_msgs::msg::Path transformPlanPosesToCostmapFrameWrapper(
-    PathIterator begin, PathIterator end, const builtin_interfaces::msg::Time & stamp)
-  {
-    return transformPlanPosesToCostmapFrame(begin, end, stamp);
-  }
-
   geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper(
     const geometry_msgs::msg::PoseStamped & pose)
   {
@@ -98,6 +94,9 @@ TEST(PathHandlerTests, TestBounds)
   node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9));
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", "dummy_costmap", true);
+  auto results = costmap_ros->set_parameters_atomically(
+    {rclcpp::Parameter("global_frame", "odom"),
+      rclcpp::Parameter("robot_base_frame", "base_link")});
   ParametersHandler param_handler(node);
   rclcpp_lifecycle::State state;
   costmap_ros->on_configure(state);
@@ -106,22 +105,35 @@ TEST(PathHandlerTests, TestBounds)
   handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), &param_handler);
   EXPECT_EQ(handler.getMaxCostmapDistWrapper(), 2.5);
 
+  // Set tf between map odom and base_link
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
+    std::make_unique<tf2_ros::TransformBroadcaster>(node);
+  geometry_msgs::msg::TransformStamped t;
+  //t.header.stamp = node->get_clock()->now();
+  t.header.frame_id = "map";
+  t.child_frame_id = "base_link";
+  tf_broadcaster_->sendTransform(t);
+  t.header.frame_id = "map";
+  t.child_frame_id = "odom";
+  tf_broadcaster_->sendTransform(t);
+
   // Test getting the global plans within a bounds window
   nav_msgs::msg::Path path;
-  path.header.frame_id = "odom";
+  path.header.frame_id = "map";
   path.poses.resize(100);
   for (unsigned int i = 0; i != path.poses.size(); i++) {
     path.poses[i].pose.position.x = i;
+    path.poses[i].header.frame_id = "map";
   }
   geometry_msgs::msg::PoseStamped robot_pose;
   robot_pose.header.frame_id = "odom";
   robot_pose.pose.position.x = 25.0;
 
   handler.setPath(path);
-  auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose);
+  auto [transformed_plan, closest] =
+    handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
   auto & path_in = handler.getPath();
   EXPECT_EQ(closest - path_in.poses.begin(), 25);
-  EXPECT_EQ(furthest - path_in.poses.begin(), 25);
   handler.pruneGlobalPlanWrapper(closest);
   auto & path_pruned = handler.getPath();
   EXPECT_EQ(path_pruned.poses.size(), 75u);
@@ -141,15 +153,28 @@ TEST(PathHandlerTests, TestTransforms)
   // Test basic transformations and path handling
   handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), &param_handler);
 
+  // Set tf between map odom and base_link
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
+    std::make_unique<tf2_ros::TransformBroadcaster>(node);
+  geometry_msgs::msg::TransformStamped t;
+  //t.header.stamp = node->get_clock()->now();
+  t.header.frame_id = "map";
+  t.child_frame_id = "base_link";
+  tf_broadcaster_->sendTransform(t);
+  t.header.frame_id = "map";
+  t.child_frame_id = "odom";
+  tf_broadcaster_->sendTransform(t);
+
   nav_msgs::msg::Path path;
   path.header.frame_id = "map";
   path.poses.resize(100);
   for (unsigned int i = 0; i != path.poses.size(); i++) {
     path.poses[i].pose.position.x = i;
+    path.poses[i].header.frame_id = "map";
   }
 
   geometry_msgs::msg::PoseStamped robot_pose, output_pose;
-  robot_pose.header.frame_id = "map";
+  robot_pose.header.frame_id = "odom";
   robot_pose.pose.position.x = 2.5;
 
   EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose));
@@ -159,10 +184,8 @@ TEST(PathHandlerTests, TestTransforms)
   handler.setPath(path);
   EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose));
 
-  auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose);
-
-  builtin_interfaces::msg::Time stamp;
-  auto path_out = handler.transformPlanPosesToCostmapFrameWrapper(closest, furthest, stamp);
+  auto [path_out, closest] =
+    handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
 
   // Put it all together
   auto final_path = handler.transformPath(robot_pose);
diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
index 6f84b05382d..5a7fa2dd895 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)
@@ -30,6 +31,7 @@ set(dependencies
   nav2_core
   tf2
   tf2_geometry_msgs
+  nav2_amcl
 )
 
 set(library_name nav2_regulated_pure_pursuit_controller)
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 358010f9486..ebe48122a2b 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
@@ -50,6 +50,7 @@ struct Parameters
   double regulated_linear_scaling_min_radius;
   double regulated_linear_scaling_min_speed;
   bool use_rotate_to_heading;
+  bool use_rotate_to_path;
   double max_angular_accel;
   double rotate_to_heading_min_angle;
   bool allow_reversing;
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..01d76819f67 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
@@ -76,10 +76,18 @@ class PathHandler
     const geometry_msgs::msg::PoseStamped & in_pose,
     geometry_msgs::msg::PoseStamped & out_pose) const;
 
-  void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;}
+  void setPlan(const nav_msgs::msg::Path & path)
+  {
+    global_plan_ = path;
+    if (path.poses.size() > 1) {
+      previous_last_pose_ = *std::prev(path.poses.end(), 2);
+    }
+  }
 
   nav_msgs::msg::Path getPlan() {return global_plan_;}
 
+  geometry_msgs::msg::PoseStamped getBeforeLastPose() {return previous_last_pose_;}
+
 protected:
   /**
    * Get the greatest extent of the costmap in meters from the center.
@@ -92,6 +100,7 @@ class PathHandler
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   nav_msgs::msg::Path global_plan_;
+  geometry_msgs::msg::PoseStamped previous_last_pose_;
 };
 
 }  // namespace nav2_regulated_pure_pursuit_controller
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..c88421d2f72 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
@@ -134,7 +134,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @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 sign);
 
   /**
    * @brief Whether robot should rotate to final goal orientation
@@ -210,6 +210,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>>
+  interpolated_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 cffd2c1f499..53dbc00df0b 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 cee6c01d582..bec5721270c 100644
--- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
@@ -77,6 +77,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".use_rotate_to_path", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785));
   declare_parameter_if_not_declared(
@@ -141,6 +143,7 @@ ParameterHandler::ParameterHandler(
     plugin_name_ + ".regulated_linear_scaling_min_speed",
     params_.regulated_linear_scaling_min_speed);
   node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading);
+  node->get_parameter(plugin_name_ + ".use_rotate_to_path", params_.use_rotate_to_path);
   node->get_parameter(
     plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle);
   node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel);
@@ -240,6 +243,8 @@ ParameterHandler::dynamicParametersCallback(
         params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_collision_detection") {
         params_.use_collision_detection = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_rotate_to_path") {
+        params_.use_rotate_to_path = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_rotate_to_heading") {
         if (parameter.as_bool() && params_.allow_reversing) {
           RCLCPP_WARN(
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 b543e4fff54..d0e901c9bf7 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
@@ -25,6 +25,7 @@
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include <nav2_amcl/angleutils.hpp>
 
 using std::hypot;
 using std::min;
@@ -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);
+  interpolated_carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
+    "interpolated_lookahead_point", 1);
 }
 
 void RegulatedPurePursuitController::cleanup()
@@ -84,6 +87,7 @@ void RegulatedPurePursuitController::cleanup()
     plugin_name_.c_str());
   global_path_pub_.reset();
   carrot_pub_.reset();
+  interpolated_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();
+  interpolated_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();
+  interpolated_carrot_pub_->on_deactivate();
 }
 
 std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController::createCarrotMsg(
@@ -186,49 +192,66 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   // Handle special case of the carrot point being the last point of the path
   // We compute the curvature based on the interpolated position of the lookahead point
-  if (params_->interpolate_curvature_at_goal
-      && carrot_pose.pose.position == transformed_plan.poses.back().pose.position)
+  // Interpolation is done based on the orientation of the vector connecting the last two poses
+  geometry_msgs::msg::PoseStamped interpolated_carrot = carrot_pose;
+  if (params_->interpolate_curvature_at_goal &&
+    carrot_pose.pose.position == transformed_plan.poses.back().pose.position)
   {
     double end_path_orientation;
+    double distance_after_last_pose = lookahead_dist - sqrt(carrot_dist2);
+
     geometry_msgs::msg::Point last_point = transformed_plan.poses.back().pose.position;
+    geometry_msgs::msg::Point before_last_point;
+
     if (transformed_plan.poses.size() == 1) {
-      // Handling only one pose left on path
-      end_path_orientation = tf2::getYaw(transformed_plan.poses.back().pose.orientation);
+      // Handle only one pose left on path
+      distance_after_last_pose = lookahead_dist;
+      geometry_msgs::msg::PoseStamped before_last_pose = path_handler_->getBeforeLastPose();
+      before_last_pose.header.stamp = rclcpp::Time(0);
+      path_handler_->transformPose(
+        transformed_plan.poses.back().header.frame_id, before_last_pose, before_last_pose);
+      before_last_point = before_last_pose.pose.position;
     } else {
-      geometry_msgs::msg::Point previous_last_point = std::prev(transformed_plan.poses.end(),2)->pose.position;
-      end_path_orientation = atan2(last_point.y - previous_last_point.y,
-                                   last_point.x - previous_last_point.x);
+      before_last_point = std::prev(transformed_plan.poses.end(), 2)->pose.position;
     }
-    double distance_after_last = lookahead_dist - sqrt(carrot_dist2);
-    geometry_msgs::msg::Point interpolated_carrot;
-    interpolated_carrot.x = last_point.x + cos(end_path_orientation) * distance_after_last;
-    interpolated_carrot.y = last_point.y + sin(end_path_orientation) * distance_after_last;
+
+    end_path_orientation = atan2(
+      last_point.y - before_last_point.y,
+      last_point.x - before_last_point.x);
+
+    interpolated_carrot.pose.position.x = last_point.x +
+      cos(end_path_orientation) * distance_after_last_pose;
+    interpolated_carrot.pose.position.y = last_point.y +
+      sin(end_path_orientation) * distance_after_last_pose;
+
     double interpolated_carrot_dist2 =
-        (interpolated_carrot.x * interpolated_carrot.x) +
-        (interpolated_carrot.y * interpolated_carrot.y);
-    curvature = 2.0 * interpolated_carrot.y / interpolated_carrot_dist2;
+      (interpolated_carrot.pose.position.x * interpolated_carrot.pose.position.x) +
+      (interpolated_carrot.pose.position.y * interpolated_carrot.pose.position.y);
+    curvature = 2.0 * interpolated_carrot.pose.position.y / interpolated_carrot_dist2;
+
+    interpolated_carrot_pub_->publish(createCarrotMsg(interpolated_carrot));
   }
 
   carrot_pub_->publish(createCarrotMsg(carrot_pose));
 
   double linear_vel, angular_vel;
 
-
-
   // Setting the velocity direction
-  double sign = 1.0;
+  double sign, interpolated_sign = 1.0;
   if (params_->allow_reversing) {
     sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
+    interpolated_sign = interpolated_carrot.pose.position.x >= 0.0 ? 1.0 : -1.0;
   }
 
   linear_vel = params_->desired_linear_vel;
 
   // Make sure we're in compliance with basic constraints
+  // Using interpolated carrot for rotate to Path (if interpolate_curvature_at_goal = true)
   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(interpolated_carrot, angle_to_heading, interpolated_sign)) {
     rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed);
   } else {
     applyConstraints(
@@ -257,11 +280,16 @@ 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 sign)
 {
   // Whether we should rotate robot to rough path heading
-  angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x);
-  return params_->use_rotate_to_heading &&
+  if (sign >= 0) {
+    angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x);
+  } else {
+    angle_to_path = nav2_amcl::angleutils::normalize(
+      atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x) + M_PI);
+  }
+  return params_->use_rotate_to_path &&
          fabs(angle_to_path) > params_->rotate_to_heading_min_angle;
 }
 
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 09892755d93..bc68816314e 100644
--- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
+++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
@@ -77,7 +77,7 @@ 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);
+    return shouldRotateToPath(carrot_pose, angle_to_path, 1.0);
   }
 
   bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose)