From f628f1b9bc26dc8d479592eb8d960326164f32aa Mon Sep 17 00:00:00 2001 From: Sakshay Mahna Date: Wed, 17 Sep 2025 20:51:30 +0530 Subject: [PATCH 1/4] Fix graceful controller lookahead bug Signed-off-by: Sakshay Mahna --- nav2_util/src/controller_utils.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp index 18f2ae42dd3..ec3341862c8 100644 --- a/nav2_util/src/controller_utils.cpp +++ b/nav2_util/src/controller_utils.cpp @@ -129,6 +129,27 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( point.y - prev_pose_it->pose.position.y, point.x - prev_pose_it->pose.position.x); + // Path tangent vector (prev -> goal) + double tx = goal_pose_it->pose.position.x - prev_pose_it->pose.position.x; + double ty = goal_pose_it->pose.position.y - prev_pose_it->pose.position.y; + double tangent_len = std::hypot(tx, ty); + tx /= tangent_len; + ty /= tangent_len; + + // Forward vector from raw_yaw + double fx = std::cos(yaw); + double fy = std::sin(yaw); + + // If forward vector points opposite the tangent, flip by PI + double dot = fx * tx + fy * ty; + if (dot < 0.0) { + yaw += M_PI; + } + + // Normalize yaw into [-pi, pi) + while (yaw <= -M_PI) {yaw += 2.0 * M_PI;} + while (yaw > M_PI) {yaw -= 2.0 * M_PI;} + geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = prev_pose_it->header.frame_id; pose.header.stamp = goal_pose_it->header.stamp; From 8ceba021499aa378689227dc1a3d330ed8847d38 Mon Sep 17 00:00:00 2001 From: Sakshay Mahna Date: Thu, 25 Sep 2025 18:59:13 +0530 Subject: [PATCH 2/4] Shorten logic with goal_pose Signed-off-by: Sakshay Mahna --- nav2_util/src/controller_utils.cpp | 25 ++----------------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp index ec3341862c8..42790571d8f 100644 --- a/nav2_util/src/controller_utils.cpp +++ b/nav2_util/src/controller_utils.cpp @@ -126,29 +126,8 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( // 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); - - // Path tangent vector (prev -> goal) - double tx = goal_pose_it->pose.position.x - prev_pose_it->pose.position.x; - double ty = goal_pose_it->pose.position.y - prev_pose_it->pose.position.y; - double tangent_len = std::hypot(tx, ty); - tx /= tangent_len; - ty /= tangent_len; - - // Forward vector from raw_yaw - double fx = std::cos(yaw); - double fy = std::sin(yaw); - - // If forward vector points opposite the tangent, flip by PI - double dot = fx * tx + fy * ty; - if (dot < 0.0) { - yaw += M_PI; - } - - // Normalize yaw into [-pi, pi) - while (yaw <= -M_PI) {yaw += 2.0 * M_PI;} - while (yaw > M_PI) {yaw -= 2.0 * M_PI;} + goal_pose_it->pose.position.y - prev_pose_it->pose.position.y, + goal_pose_it->pose.position.x - prev_pose_it->pose.position.x); geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = prev_pose_it->header.frame_id; From 1205f7dbbf7160c5a00635cfec566f9fa27b8c63 Mon Sep 17 00:00:00 2001 From: Sakshay Mahna Date: Sat, 4 Oct 2025 17:27:15 +0530 Subject: [PATCH 3/4] Add Linear Interpolation Fix Signed-off-by: Sakshay Mahna --- .../include/nav2_util/controller_utils.hpp | 13 ++ nav2_util/src/controller_utils.cpp | 42 +++-- nav2_util/test/test_controller_utils.cpp | 152 ++++++++++++++++-- 3 files changed, 183 insertions(+), 24 deletions(-) diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp index dbe04b1f2f3..4950fa7a7c8 100644 --- a/nav2_util/include/nav2_util/controller_utils.hpp +++ b/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, + double target_dist); + /** * @brief Get lookahead point * @param lookahead_dist Optimal lookahead distance diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp index 42790571d8f..60ea7cc0152 100644 --- a/nav2_util/src/controller_utils.cpp +++ b/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, + 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(); } @@ -100,8 +122,8 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( // 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); + 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; @@ -109,7 +131,7 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( 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()) { @@ -119,15 +141,15 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( // 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( + 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 double yaw = atan2( - goal_pose_it->pose.position.y - prev_pose_it->pose.position.y, - goal_pose_it->pose.position.x - prev_pose_it->pose.position.x); + point.y - prev_pose_it->pose.position.y, + point.x - prev_pose_it->pose.position.x); geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = prev_pose_it->header.frame_id; diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp index 3c13d8e9bbc..e6117db8e11 100644 --- a/nav2_util/test/test_controller_utils.cpp +++ b/nav2_util/test/test_controller_utils.cpp @@ -142,6 +142,130 @@ INSTANTIATE_TEST_SUITE_P( } )); +using linearInterpolationParam = std::tuple< + std::pair, + std::pair, + double, + std::pair +>; + +class linearInterpolationTest + : public ::testing::TestWithParam +{}; + +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 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,8 +342,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 back path.poses.clear(); @@ -227,7 +351,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 -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); } From c1faecadca34b0df1133d78a14a547410d3eb67f Mon Sep 17 00:00:00 2001 From: Sakshay Mahna Date: Tue, 7 Oct 2025 19:16:54 +0530 Subject: [PATCH 4/4] Update const double and comments Signed-off-by: Sakshay Mahna --- nav2_util/include/nav2_util/controller_utils.hpp | 2 +- nav2_util/src/controller_utils.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp index 4950fa7a7c8..be8c88c0013 100644 --- a/nav2_util/include/nav2_util/controller_utils.hpp +++ b/nav2_util/include/nav2_util/controller_utils.hpp @@ -48,7 +48,7 @@ geometry_msgs::msg::Point circleSegmentIntersection( geometry_msgs::msg::Point linearInterpolation( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - double target_dist); + const double target_dist); /** * @brief Get lookahead point diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp index 60ea7cc0152..c15d3009ba0 100644 --- a/nav2_util/src/controller_utils.cpp +++ b/nav2_util/src/controller_utils.cpp @@ -57,7 +57,7 @@ geometry_msgs::msg::Point circleSegmentIntersection( geometry_msgs::msg::Point linearInterpolation( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - double target_dist) + const double target_dist) { geometry_msgs::msg::Point result; @@ -120,7 +120,7 @@ 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 = linearInterpolation( last_pose_it->pose.position, projected_position, interpolation_dist); @@ -135,11 +135,11 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( 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 = linearInterpolation( prev_pose_it->pose.position,