diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
index 8669aafeedb..4116df114f1 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
@@ -107,6 +107,24 @@ class GracefulController : public nav2_core::Controller
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
 protected:
+  /**
+   * @brief Validate a given target pose for calculating command velocity
+   * @param target_pose Target pose to validate
+   * @param dist_to_target Distance to target pose
+   * @param dist_to_goal Distance to navigation goal
+   * @param trajectory Trajectory to validate in simulation
+   * @param costmap_transform Transform between global and local costmap
+   * @param cmd_vel Initial command velocity to validate in simulation
+   * @return true if target pose is valid, false otherwise
+   */
+  bool validateTargetPose(
+    geometry_msgs::msg::PoseStamped & target_pose,
+    double dist_to_target,
+    double dist_to_goal,
+    nav_msgs::msg::Path & trajectory,
+    geometry_msgs::msg::TransformStamped & costmap_transform,
+    geometry_msgs::msg::TwistStamped & cmd_vel);
+
   /**
    * @brief Simulate trajectory calculating in every step the new velocity command based on
    * a new curvature value and checking for collisions.
diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp
index da880c7dc8e..d394aac7e39 100644
--- a/nav2_graceful_controller/src/graceful_controller.cpp
+++ b/nav2_graceful_controller/src/graceful_controller.cpp
@@ -18,6 +18,7 @@
 #include "angles/angles.h"
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/controller_utils.hpp"
 #include "nav2_graceful_controller/graceful_controller.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
@@ -195,46 +196,35 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     // Else, fall through and see if we should follow control law longer
   }
 
-  // Precompute distance to candidate poses
+  // Find a valid target pose and its trajectory
+  nav_msgs::msg::Path local_plan;
+  geometry_msgs::msg::PoseStamped target_pose;
+
+  double dist_to_target;
   std::vector<double> target_distances;
   computeDistanceAlongPath(transformed_plan.poses, target_distances);
 
-  // Work back from the end of plan to find valid target pose
-  for (int i = transformed_plan.poses.size() - 1; i >= 0; --i) {
-    // Underlying control law needs a single target pose, which should:
-    //  * Be as far away as possible from the robot (for smoothness)
-    //  * But no further than the max_lookahed_ distance
-    //  * Be feasible to reach in a collision free manner
-    geometry_msgs::msg::PoseStamped target_pose = transformed_plan.poses[i];
-    double dist_to_target = target_distances[i];
-
-    // Continue if target_pose is too far away from robot
-    if (dist_to_target > params_->max_lookahead) {continue;}
-
-    if (dist_to_goal < params_->max_lookahead) {
-      if (params_->prefer_final_rotation) {
-        // Avoid instability and big sweeping turns at the end of paths by
-        // ignoring final heading
-        double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x);
-        target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
-      }
-    } else if (dist_to_target < params_->min_lookahead) {
-      // Make sure target is far enough away to avoid instability
-      break;
-    }
-
-    // Flip the orientation of the motion target if the robot is moving backwards
-    bool reversing = false;
-    if (params_->allow_backward && target_pose.pose.position.x < 0.0) {
-      reversing = true;
-      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
-        tf2::getYaw(target_pose.pose.orientation) + M_PI);
+  for (int i = transformed_plan.poses.size(); i >= 0; --i) {
+    if (i == static_cast<int>(transformed_plan.poses.size())) {
+      // Calculate target pose through lookahead interpolation to get most accurate
+      // lookahead point, if possible
+      dist_to_target = params_->max_lookahead;
+      // Interpolate after goal false for graceful controller
+      // Requires interpolating the orientation which is not yet implemented
+      target_pose = nav2_util::getLookAheadPoint(dist_to_target, transformed_plan, false);
+    } else {
+      // Underlying control law needs a single target pose, which should:
+      //  * Be as far away as possible from the robot (for smoothness)
+      //  * But no further than the max_lookahed_ distance
+      //  * Be feasible to reach in a collision free manner
+      dist_to_target = target_distances[i];
+      target_pose = transformed_plan.poses[i];
     }
 
-    // Actually simulate our path
-    nav_msgs::msg::Path local_plan;
-    if (simulateTrajectory(target_pose, costmap_transform, local_plan, cmd_vel, reversing)) {
-      // Successfully simulated to target_pose - compute velocity at this moment
+    // Compute velocity at this moment if valid target pose is found
+    if (validateTargetPose(
+        target_pose, dist_to_target, dist_to_goal, local_plan, costmap_transform, cmd_vel))
+    {
       // Publish the selected target_pose
       motion_target_pub_->publish(target_pose);
       // Publish marker for slowdown radius around motion target for debugging / visualization
@@ -283,6 +273,49 @@ void GracefulController::setSpeedLimit(
   }
 }
 
+bool GracefulController::validateTargetPose(
+  geometry_msgs::msg::PoseStamped & target_pose,
+  double dist_to_target,
+  double dist_to_goal,
+  nav_msgs::msg::Path & trajectory,
+  geometry_msgs::msg::TransformStamped & costmap_transform,
+  geometry_msgs::msg::TwistStamped & cmd_vel)
+{
+  // Continue if target_pose is too far away from robot
+  if (dist_to_target > params_->max_lookahead) {
+    return false;
+  }
+
+  if (dist_to_goal < params_->max_lookahead) {
+    if (params_->prefer_final_rotation) {
+      // Avoid instability and big sweeping turns at the end of paths by
+      // ignoring final heading
+      double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x);
+      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+    }
+  } else if (dist_to_target < params_->min_lookahead) {
+    // Make sure target is far enough away to avoid instability
+    return false;
+  }
+
+  // Flip the orientation of the motion target if the robot is moving backwards
+  bool reversing = false;
+  if (params_->allow_backward && target_pose.pose.position.x < 0.0) {
+    reversing = true;
+    target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+      tf2::getYaw(target_pose.pose.orientation) + M_PI);
+  }
+
+  // Actually simulate the path
+  if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) {
+    // Successfully simulated to target_pose
+    return true;
+  }
+
+  // Validation not successful
+  return false;
+}
+
 bool GracefulController::simulateTrajectory(
   const geometry_msgs::msg::PoseStamped & motion_target,
   const geometry_msgs::msg::TransformStamped & costmap_transform,
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 2d766ea967f..fc801dafa79 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
@@ -174,32 +174,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller
     const double & pose_cost, const nav_msgs::msg::Path & path,
     double & linear_vel, double & sign);
 
-  /**
-   * @brief Find the intersection a circle and a line segment.
-   * This assumes the circle is centered at the origin.
-   * If no intersection is found, a floating point error will occur.
-   * @param p1 first endpoint of line segment
-   * @param p2 second endpoint of line segment
-   * @param r radius of circle
-   * @return point of intersection
-   */
-  static geometry_msgs::msg::Point circleSegmentIntersection(
-    const geometry_msgs::msg::Point & p1,
-    const geometry_msgs::msg::Point & p2,
-    double r);
-
-  /**
-   * @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 &,
-    bool interpolate_after_goal = false);
-
   /**
    * @brief checks for the cusp position
    * @param pose Pose input to determine the cusp position
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 2f43f427f2f..5819e54b68b 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_core/controller_exceptions.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/controller_utils.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
 using std::hypot;
@@ -204,7 +205,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 carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_plan);
   auto rotate_to_path_carrot_pose = carrot_pose;
   carrot_pub_->publish(createCarrotMsg(carrot_pose));
 
@@ -214,7 +215,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   double regulation_curvature = lookahead_curvature;
   if (params_->use_fixed_curvature_lookahead) {
-    auto curvature_lookahead_pose = getLookAheadPoint(
+    auto curvature_lookahead_pose = nav2_util::getLookAheadPoint(
       curv_lookahead_dist,
       transformed_plan, params_->interpolate_curvature_after_goal);
     rotate_to_path_carrot_pose = curvature_lookahead_pose;
@@ -352,100 +353,6 @@ void RegulatedPurePursuitController::rotateToHeading(
   angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
 }
 
-geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection(
-  const geometry_msgs::msg::Point & p1,
-  const geometry_msgs::msg::Point & p2,
-  double r)
-{
-  // Formula for intersection of a line with a circle centered at the origin,
-  // modified to always return the point that is on the segment between the two points.
-  // https://mathworld.wolfram.com/Circle-LineIntersection.html
-  // This works because the poses are transformed into the robot frame.
-  // This can be derived from solving the system of equations of a line and a circle
-  // which results in something that is just a reformulation of the quadratic formula.
-  // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
-  // https://www.desmos.com/calculator/td5cwbuocd
-  double x1 = p1.x;
-  double x2 = p2.x;
-  double y1 = p1.y;
-  double y2 = p2.y;
-
-  double dx = x2 - x1;
-  double dy = y2 - y1;
-  double dr2 = dx * dx + dy * dy;
-  double D = x1 * y2 - x2 * y1;
-
-  // Augmentation to only return point within segment
-  double d1 = x1 * x1 + y1 * y1;
-  double d2 = x2 * x2 + y2 * y2;
-  double dd = d2 - d1;
-
-  geometry_msgs::msg::Point p;
-  double sqrt_term = std::sqrt(r * r * dr2 - D * D);
-  p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2;
-  p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2;
-  return p;
-}
-
-geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(
-  const double & lookahead_dist,
-  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(
-    transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) {
-      return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist;
-    });
-
-  // If the no pose is not far enough, take the last pose
-  if (goal_pose_it == 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)
-    // This can be found with a closed form for the intersection of a segment and a circle
-    // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
-    // and goal_pose is guaranteed to be outside the circle.
-    auto prev_pose_it = std::prev(goal_pose_it);
-    auto point = circleSegmentIntersection(
-      prev_pose_it->pose.position,
-      goal_pose_it->pose.position, lookahead_dist);
-    geometry_msgs::msg::PoseStamped pose;
-    pose.header.frame_id = prev_pose_it->header.frame_id;
-    pose.header.stamp = goal_pose_it->header.stamp;
-    pose.pose.position = point;
-    return pose;
-  }
-
-  return *goal_pose_it;
-}
-
 void RegulatedPurePursuitController::applyConstraints(
   const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/,
   const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign)
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 9c4dd1cade5..1744e4b4aef 100644
--- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
+++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
@@ -52,28 +52,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
     return getLookAheadDistance(twist);
   }
 
-  static geometry_msgs::msg::Point circleSegmentIntersectionWrapper(
-    const geometry_msgs::msg::Point & p1,
-    const geometry_msgs::msg::Point & p2,
-    double r)
-  {
-    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)
-  {
-    return getLookAheadPoint(dist, path);
-  }
-
   bool shouldRotateToPathWrapper(
     const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path)
   {
@@ -207,239 +185,6 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange)
   EXPECT_EQ(rtn, std::numeric_limits<double>::max());
 }
 
-using CircleSegmentIntersectionParam = std::tuple<
-  std::pair<double, double>,
-  std::pair<double, double>,
-  double,
-  std::pair<double, double>
->;
-
-class CircleSegmentIntersectionTest
-  : public ::testing::TestWithParam<CircleSegmentIntersectionParam>
-{};
-
-TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection)
-{
-  auto pair1 = std::get<0>(GetParam());
-  auto pair2 = std::get<1>(GetParam());
-  auto r = std::get<2>(GetParam());
-  auto expected_pair = std::get<3>(GetParam());
-  auto pair_to_point = [](std::pair<double, double> p) -> geometry_msgs::msg::Point {
-      geometry_msgs::msg::Point point;
-      point.x = p.first;
-      point.y = p.second;
-      point.z = 0.0;
-      return point;
-    };
-  auto p1 = pair_to_point(pair1);
-  auto p2 = pair_to_point(pair2);
-  auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper(p1, p2, r);
-  auto expected_point = pair_to_point(expected_pair);
-  EXPECT_DOUBLE_EQ(actual.x, expected_point.x);
-  EXPECT_DOUBLE_EQ(actual.y, expected_point.y);
-  // Expect that the intersection point is actually r away from the origin
-  EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y));
-}
-
-INSTANTIATE_TEST_SUITE_P(
-  InterpolationTest,
-  CircleSegmentIntersectionTest,
-  testing::Values(
-    // Origin to the positive X axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {2.0, 0.0},
-  1.0,
-  {1.0, 0.0}
-},
-    // Origin to the negative X axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-2.0, 0.0},
-  1.0,
-  {-1.0, 0.0}
-},
-    // Origin to the positive Y axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, 2.0},
-  1.0,
-  {0.0, 1.0}
-},
-    // Origin to the negative Y axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, -2.0},
-  1.0,
-  {0.0, -1.0}
-},
-    // non-origin to the X axis with non-unit circle, with the second point inside
-    CircleSegmentIntersectionParam{
-  {4.0, 0.0},
-  {-1.0, 0.0},
-  2.0,
-  {2.0, 0.0}
-},
-    // non-origin to the Y axis with non-unit circle, with the second point inside
-    CircleSegmentIntersectionParam{
-  {0.0, 4.0},
-  {0.0, -0.5},
-  2.0,
-  {0.0, 2.0}
-},
-    // origin to the positive X axis, on the circle
-    CircleSegmentIntersectionParam{
-  {2.0, 0.0},
-  {0.0, 0.0},
-  2.0,
-  {2.0, 0.0}
-},
-    // origin to the positive Y axis, on the circle
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, 2.0},
-  2.0,
-  {0.0, 2.0}
-},
-    // origin to the upper-right quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {6.0, 8.0},
-  5.0,
-  {3.0, 4.0}
-},
-    // origin to the lower-left quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-6.0, -8.0},
-  5.0,
-  {-3.0, -4.0}
-},
-    // origin to the upper-left quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-6.0, 8.0},
-  5.0,
-  {-3.0, 4.0}
-},
-    // origin to the lower-right quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {6.0, -8.0},
-  5.0,
-  {3.0, -4.0}
-}
-));
-
-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 back
-  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>();
@@ -476,24 +221,6 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI)
   twist.linear.x = 0.0;
   rtn = ctrl->getLookAheadDistanceWrapper(twist);
   EXPECT_EQ(rtn, 0.3);
-
-  // test getLookAheadPoint
-  double dist = 1.0;
-  nav_msgs::msg::Path path;
-  path.poses.resize(10);
-  for (uint i = 0; i != path.poses.size(); i++) {
-    path.poses[i].pose.position.x = static_cast<double>(i);
-  }
-
-  // test exact hits
-  auto pt = ctrl->getLookAheadPointWrapper(dist, path);
-  EXPECT_EQ(pt.pose.position.x, 1.0);
-
-  // test interpolation
-  ctrl->configure(node, name, tf, costmap);
-  dist = 3.8;
-  pt = ctrl->getLookAheadPointWrapper(dist, path);
-  EXPECT_EQ(pt.pose.position.x, 3.8);
 }
 
 TEST(RegulatedPurePursuitTest, rotateTests)
diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp
new file mode 100644
index 00000000000..dbe04b1f2f3
--- /dev/null
+++ b/nav2_util/include/nav2_util/controller_utils.hpp
@@ -0,0 +1,54 @@
+// Copyright (c) 2020 Shrijit Singh
+// Copyright (c) 2020 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef NAV2_UTIL__CONTROLLER_UTILS_HPP_
+#define NAV2_UTIL__CONTROLLER_UTILS_HPP_
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+namespace nav2_util
+{
+/**
+* @brief Find the intersection a circle and a line segment.
+* This assumes the circle is centered at the origin.
+* If no intersection is found, a floating point error will occur.
+* @param p1 first endpoint of line segment
+* @param p2 second endpoint of line segment
+* @param r radius of circle
+* @return point of intersection
+*/
+geometry_msgs::msg::Point circleSegmentIntersection(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  double r);
+
+/**
+* @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(
+  double &, const nav_msgs::msg::Path &,
+  const bool interpolate_after_goal = false);
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__CONTROLLER_UTILS_HPP_
diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt
index 41261f512a5..ee4506667c6 100644
--- a/nav2_util/src/CMakeLists.txt
+++ b/nav2_util/src/CMakeLists.txt
@@ -6,6 +6,7 @@ add_library(${library_name} SHARED
   lifecycle_utils.cpp
   lifecycle_node.cpp
   robot_utils.cpp
+  controller_utils.cpp
   node_thread.cpp
   odometry_utils.cpp
   array_parser.cpp
diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp
new file mode 100644
index 00000000000..df0d12ee8cc
--- /dev/null
+++ b/nav2_util/src/controller_utils.cpp
@@ -0,0 +1,147 @@
+// Copyright (c) 2020 Shrijit Singh
+// Copyright (c) 2020 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_util/controller_utils.hpp"
+
+namespace nav2_util
+{
+geometry_msgs::msg::Point circleSegmentIntersection(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  double r)
+{
+  // Formula for intersection of a line with a circle centered at the origin,
+  // modified to always return the point that is on the segment between the two points.
+  // https://mathworld.wolfram.com/Circle-LineIntersection.html
+  // This works because the poses are transformed into the robot frame.
+  // This can be derived from solving the system of equations of a line and a circle
+  // which results in something that is just a reformulation of the quadratic formula.
+  // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
+  // https://www.desmos.com/calculator/td5cwbuocd
+  double x1 = p1.x;
+  double x2 = p2.x;
+  double y1 = p1.y;
+  double y2 = p2.y;
+
+  double dx = x2 - x1;
+  double dy = y2 - y1;
+  double dr2 = dx * dx + dy * dy;
+  double D = x1 * y2 - x2 * y1;
+
+  // Augmentation to only return point within segment
+  double d1 = x1 * x1 + y1 * y1;
+  double d2 = x2 * x2 + y2 * y2;
+  double dd = d2 - d1;
+
+  geometry_msgs::msg::Point p;
+  double sqrt_term = std::sqrt(r * r * dr2 - D * D);
+  p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2;
+  p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2;
+  return p;
+}
+
+geometry_msgs::msg::PoseStamped getLookAheadPoint(
+  double & lookahead_dist,
+  const nav_msgs::msg::Path & transformed_plan,
+  const bool interpolate_after_goal)
+{
+  // Find the first pose which is at a distance greater than the lookahead distance
+  // Using distance along the path
+  const auto & poses = transformed_plan.poses;
+  auto goal_pose_it = poses.begin();
+  double d = 0.0;
+
+  bool pose_found = false;
+  for (size_t i = 1; i < poses.size(); i++) {
+    const auto & prev_pose = poses[i - 1].pose.position;
+    const auto & curr_pose = poses[i].pose.position;
+
+    d += std::hypot(curr_pose.x - prev_pose.x, curr_pose.y - prev_pose.y);
+    if (d >= lookahead_dist) {
+      goal_pose_it = poses.begin() + i;
+      pose_found = true;
+      break;
+    }
+  }
+
+  if (!pose_found) {
+    goal_pose_it = poses.end();
+  }
+
+  // If the no pose is not far enough, take the last pose
+  if (goal_pose_it == 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 {
+      lookahead_dist = d;
+      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)
+    // This can be found with a closed form for the intersection of a segment and a circle
+    // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
+    // and goal_pose is guaranteed to be outside the circle.
+    auto prev_pose_it = std::prev(goal_pose_it);
+    auto point = circleSegmentIntersection(
+      prev_pose_it->pose.position,
+      goal_pose_it->pose.position, lookahead_dist);
+
+    // Calculate orientation towards interpolated position
+    // Convert yaw to quaternion
+    double yaw = atan2(
+      point.y - prev_pose_it->pose.position.y,
+      point.x - prev_pose_it->pose.position.x);
+
+    geometry_msgs::msg::Quaternion orientation;
+    orientation.x = 0.0;
+    orientation.y = 0.0;
+    orientation.z = sin(yaw / 2.0);
+    orientation.w = cos(yaw / 2.0);
+
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.frame_id = prev_pose_it->header.frame_id;
+    pose.header.stamp = goal_pose_it->header.stamp;
+    pose.pose.position = point;
+    pose.pose.orientation = orientation;
+    return pose;
+  }
+
+  return *goal_pose_it;
+}
+}  // namespace nav2_util
diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt
index 9ba40dfe512..339957521a9 100644
--- a/nav2_util/test/CMakeLists.txt
+++ b/nav2_util/test/CMakeLists.txt
@@ -38,6 +38,9 @@ target_link_libraries(test_odometry_utils ${library_name} ${nav_msgs_TARGETS} ${
 ament_add_gtest(test_robot_utils test_robot_utils.cpp)
 target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS})
 
+ament_add_gtest(test_controller_utils test_controller_utils.cpp)
+target_link_libraries(test_controller_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS})
+
 ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp)
 target_include_directories(test_base_footprint_publisher PRIVATE "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/src>")
 
diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp
new file mode 100644
index 00000000000..3c13d8e9bbc
--- /dev/null
+++ b/nav2_util/test/test_controller_utils.cpp
@@ -0,0 +1,265 @@
+// Copyright (c) 2020 Samsung Research
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_util/controller_utils.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "gtest/gtest.h"
+
+
+using CircleSegmentIntersectionParam = std::tuple<
+  std::pair<double, double>,
+  std::pair<double, double>,
+  double,
+  std::pair<double, double>
+>;
+
+class CircleSegmentIntersectionTest
+  : public ::testing::TestWithParam<CircleSegmentIntersectionParam>
+{};
+
+TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection)
+{
+  auto pair1 = std::get<0>(GetParam());
+  auto pair2 = std::get<1>(GetParam());
+  auto r = std::get<2>(GetParam());
+  auto expected_pair = std::get<3>(GetParam());
+  auto pair_to_point = [](std::pair<double, double> p) -> geometry_msgs::msg::Point {
+      geometry_msgs::msg::Point point;
+      point.x = p.first;
+      point.y = p.second;
+      point.z = 0.0;
+      return point;
+    };
+  auto p1 = pair_to_point(pair1);
+  auto p2 = pair_to_point(pair2);
+  auto actual = nav2_util::circleSegmentIntersection(p1, p2, r);
+  auto expected_point = pair_to_point(expected_pair);
+  EXPECT_DOUBLE_EQ(actual.x, expected_point.x);
+  EXPECT_DOUBLE_EQ(actual.y, expected_point.y);
+  // Expect that the intersection point is actually r away from the origin
+  EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  InterpolationTest,
+  CircleSegmentIntersectionTest,
+  testing::Values(
+    // Origin to the positive X axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {2.0, 0.0},
+  1.0,
+  {1.0, 0.0}
+},
+    // Origin to the negative X axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-2.0, 0.0},
+  1.0,
+  {-1.0, 0.0}
+},
+    // Origin to the positive Y axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, 2.0},
+  1.0,
+  {0.0, 1.0}
+},
+    // Origin to the negative Y axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, -2.0},
+  1.0,
+  {0.0, -1.0}
+},
+    // non-origin to the X axis with non-unit circle, with the second point inside
+    CircleSegmentIntersectionParam{
+  {4.0, 0.0},
+  {-1.0, 0.0},
+  2.0,
+  {2.0, 0.0}
+},
+    // non-origin to the Y axis with non-unit circle, with the second point inside
+    CircleSegmentIntersectionParam{
+  {0.0, 4.0},
+  {0.0, -0.5},
+  2.0,
+  {0.0, 2.0}
+},
+    // origin to the positive X axis, on the circle
+    CircleSegmentIntersectionParam{
+  {2.0, 0.0},
+  {0.0, 0.0},
+  2.0,
+  {2.0, 0.0}
+},
+    // origin to the positive Y axis, on the circle
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, 2.0},
+  2.0,
+  {0.0, 2.0}
+},
+    // origin to the upper-right quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {6.0, 8.0},
+  5.0,
+  {3.0, 4.0}
+},
+    // origin to the lower-left quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-6.0, -8.0},
+  5.0,
+  {-3.0, -4.0}
+},
+    // origin to the upper-left quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-6.0, 8.0},
+  5.0,
+  {-3.0, 4.0}
+},
+    // origin to the lower-right quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {6.0, -8.0},
+  5.0,
+  {3.0, -4.0}
+}
+));
+
+TEST(InterpolationUtils, lookaheadInterpolation)
+{
+  // test Lookahead Point Interpolation
+  double dist = 1.0;
+  nav_msgs::msg::Path path;
+  path.poses.resize(10);
+  for (uint i = 0; i != path.poses.size(); i++) {
+    path.poses[i].pose.position.x = static_cast<double>(i);
+  }
+
+  // test exact hits
+  auto pt = nav2_util::getLookAheadPoint(dist, path);
+  EXPECT_EQ(pt.pose.position.x, 1.0);
+
+  // test interpolation
+  dist = 3.8;
+  pt = nav2_util::getLookAheadPoint(dist, path);
+  EXPECT_EQ(pt.pose.position.x, 3.8);
+}
+
+TEST(InterpolationUtils, lookaheadExtrapolation)
+{
+  // Test extrapolation beyond goal
+  double EPSILON = std::numeric_limits<float>::epsilon();
+
+  nav_msgs::msg::Path path;
+  double lookahead_dist = 10.0;
+  // 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 = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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 = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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 = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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 = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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 = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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 back
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[1].pose.position.x = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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 = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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 = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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 = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  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);
+}