[Graceful Controller] Fix Incorrect Motion Target Heading output by controller (#5530)

* Fix graceful controller lookahead bug

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

* Shorten logic with goal_pose

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

* Add Linear Interpolation Fix

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

* Update const double and comments

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

---------

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

Author: SakshayMahna

nav2_util/include/nav2_util/controller_utils.hpp

@@ -37,6 +37,19 @@ geometry_msgs::msg::Point circleSegmentIntersection(
   const geometry_msgs::msg::Point & p2,
   double r);
 
+/**
+* @brief Find the linear interpolation between two points
+* at a given distance starting from first endpoint.
+* @param p1 first endpoint of line segment
+* @param p2 second endpoint of line segment
+* @param target_dist interpolation distance from first endpoint of line segment
+* @return point of intersection
+*/
+geometry_msgs::msg::Point linearInterpolation(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  const double target_dist);
+
 /**
 * @brief Get lookahead point
 * @param lookahead_dist Optimal lookahead distance

nav2_util/src/controller_utils.cpp

@@ -54,6 +54,24 @@ geometry_msgs::msg::Point circleSegmentIntersection(
   return p;
 }
 
+geometry_msgs::msg::Point linearInterpolation(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  const double target_dist)
+{
+  geometry_msgs::msg::Point result;
+
+  double dx = p2.x - p1.x;
+  double dy = p2.y - p1.y;
+  double d_dist = std::hypot(dx, dy);
+
+  double target_ratio = target_dist / d_dist;
+
+  result.x = p1.x + target_ratio * dx;
+  result.y = p1.y + target_ratio * dy;
+  return result;
+}
+
 geometry_msgs::msg::PoseStamped getLookAheadPoint(
   double & lookahead_dist,
   const nav_msgs::msg::Path & transformed_plan,
@@ -63,21 +81,25 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint(
   // Using distance along the path
   const auto & poses = transformed_plan.poses;
   auto goal_pose_it = poses.begin();
-  double d = 0.0;
+  double path_dist = 0.0;
+  double interpolation_dist = 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) {
+    const double d = std::hypot(curr_pose.x - prev_pose.x, curr_pose.y - prev_pose.y);
+    if (path_dist + d >= lookahead_dist) {
       goal_pose_it = poses.begin() + i;
       pose_found = true;
       break;
     }
+
+    path_dist += d;
   }
 
+  interpolation_dist = lookahead_dist - path_dist;
   if (!pose_found) {
     goal_pose_it = poses.end();
   }
@@ -98,30 +120,30 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint(
       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
+      // Use the linear interpolation to find the position at the correct look
       // ahead distance
-      const auto interpolated_position = circleSegmentIntersection(
-        last_pose_it->pose.position, projected_position, lookahead_dist);
+      const auto interpolated_position = linearInterpolation(
+        last_pose_it->pose.position, projected_position, interpolation_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;  // Updating lookahead distance since using the final point
+      lookahead_dist = path_dist;  // Updating lookahead distance since using the final point
       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
+    // Find the point on the robot path
     // 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.
+    // This can be found with a linear interpolation between the prev_pose and
+    // the goal_pose, moving interpolation_dist starting from prev_pose in the
+    // direction of goal_pose.
     auto prev_pose_it = std::prev(goal_pose_it);
-    auto point = circleSegmentIntersection(
+    auto point = linearInterpolation(
       prev_pose_it->pose.position,
-      goal_pose_it->pose.position, lookahead_dist);
+      goal_pose_it->pose.position, interpolation_dist);
 
     // Calculate orientation towards interpolated position
     // Convert yaw to quaternion

nav2_util/test/test_controller_utils.cpp

@@ -142,6 +142,130 @@ INSTANTIATE_TEST_SUITE_P(
 }
 ));
 
+using linearInterpolationParam = std::tuple<
+  std::pair<double, double>,
+  std::pair<double, double>,
+  double,
+  std::pair<double, double>
+>;
+
+class linearInterpolationTest
+  : public ::testing::TestWithParam<linearInterpolationParam>
+{};
+
+TEST_P(linearInterpolationTest, linearInterpolation)
+{
+  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::linearInterpolation(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(p1.x - actual.x, p1.y - actual.y));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  InterpolationTest,
+  linearInterpolationTest,
+  testing::Values(
+    // Origin to the positive X axis
+    linearInterpolationParam{
+  {0.0, 0.0},
+  {2.0, 0.0},
+  1.0,
+  {1.0, 0.0}
+},
+    // Origin to the negative X axis
+    linearInterpolationParam{
+  {0.0, 0.0},
+  {-2.0, 0.0},
+  1.0,
+  {-1.0, 0.0}
+},
+    // Origin to the positive Y axis
+    linearInterpolationParam{
+  {0.0, 0.0},
+  {0.0, 2.0},
+  1.0,
+  {0.0, 1.0}
+},
+    // Origin to the negative Y axis
+    linearInterpolationParam{
+  {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
+    linearInterpolationParam{
+  {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
+    linearInterpolationParam{
+  {0.0, 4.0},
+  {0.0, -0.5},
+  2.0,
+  {0.0, 2.0}
+},
+    // origin to the positive X axis, on the circle
+    linearInterpolationParam{
+  {2.0, 0.0},
+  {0.0, 0.0},
+  2.0,
+  {0.0, 0.0}
+},
+    // origin to the positive Y axis, on the circle
+    linearInterpolationParam{
+  {0.0, 0.0},
+  {0.0, 2.0},
+  2.0,
+  {0.0, 2.0}
+},
+    // origin to the upper-right quadrant (3-4-5 triangle)
+    linearInterpolationParam{
+  {0.0, 0.0},
+  {6.0, 8.0},
+  5.0,
+  {3.0, 4.0}
+},
+    // origin to the lower-left quadrant (3-4-5 triangle)
+    linearInterpolationParam{
+  {0.0, 0.0},
+  {-6.0, -8.0},
+  5.0,
+  {-3.0, -4.0}
+},
+    // origin to the upper-left quadrant (3-4-5 triangle)
+    linearInterpolationParam{
+  {0.0, 0.0},
+  {-6.0, 8.0},
+  5.0,
+  {-3.0, 4.0}
+},
+    // origin to the lower-right quadrant (3-4-5 triangle)
+    linearInterpolationParam{
+  {0.0, 0.0},
+  {6.0, -8.0},
+  5.0,
+  {3.0, -4.0}
+}
+));
+
 TEST(InterpolationUtils, lookaheadInterpolation)
 {
   // test Lookahead Point Interpolation
@@ -185,7 +309,7 @@ TEST(InterpolationUtils, lookaheadExtrapolation)
   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.x, 12.0, EPSILON);
   EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
 
   // 2 poses at 45°
@@ -196,8 +320,8 @@ TEST(InterpolationUtils, lookaheadExtrapolation)
   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);
+  EXPECT_NEAR(pt.pose.position.x, 2.0 + cos(45.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 2.0 + sin(45.0 * M_PI / 180) * 10.0, EPSILON);
 
   // 2 poses at 90°
   path.poses.clear();
@@ -207,8 +331,8 @@ TEST(InterpolationUtils, lookaheadExtrapolation)
   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);
+  EXPECT_NEAR(pt.pose.position.x, 0.0 + cos(90.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 2.0 + sin(90.0 * M_PI / 180) * 10.0, EPSILON);
 
   // 2 poses at 135°
   path.poses.clear();
@@ -218,16 +342,16 @@ TEST(InterpolationUtils, lookaheadExtrapolation)
   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);
+  EXPECT_NEAR(pt.pose.position.x, -2.0 + cos(135.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 2.0 + 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.x, -12.0, EPSILON);
   EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
 
   // 2 poses at -135°
@@ -238,8 +362,8 @@ TEST(InterpolationUtils, lookaheadExtrapolation)
   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);
+  EXPECT_NEAR(pt.pose.position.x, -2.0 + cos(-135.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, -2.0 + sin(-135.0 * M_PI / 180) * 10.0, EPSILON);
 
   // 2 poses at -90°
   path.poses.clear();
@@ -249,8 +373,8 @@ TEST(InterpolationUtils, lookaheadExtrapolation)
   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);
+  EXPECT_NEAR(pt.pose.position.x, 0.0 + cos(-90.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, -2.0 + sin(-90.0 * M_PI / 180) * 10.0, EPSILON);
 
   // 2 poses at -45°
   path.poses.clear();
@@ -260,6 +384,6 @@ TEST(InterpolationUtils, lookaheadExtrapolation)
   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);
+  EXPECT_NEAR(pt.pose.position.x, 2.0 + cos(-45.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, -2.0 + sin(-45.0 * M_PI / 180) * 10.0, EPSILON);
 }