Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Oct 8, 2025
Merged

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

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f628f1b
Fix graceful controller lookahead bug
SakshayMahna Sep 17, 2025
8ceba02
Shorten logic with goal_pose
SakshayMahna Sep 25, 2025
2cce476
Merge branch 'ros-navigation:main' into graceful-lookahead-bug
SakshayMahna Oct 4, 2025
1205f7d
Add Linear Interpolation Fix
SakshayMahna Oct 4, 2025
c1faeca
Update const double and comments
SakshayMahna Oct 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 13 additions & 0 deletions nav2_util/include/nav2_util/controller_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
38 changes: 30 additions & 8 deletions nav2_util/src/controller_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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();
}
Expand All @@ -100,16 +122,16 @@ 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;
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()) {
Expand All @@ -119,9 +141,9 @@ 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
Expand Down
152 changes: 138 additions & 14 deletions nav2_util/test/test_controller_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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°
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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°
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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);
}
Loading