Calculate next path point using lookahead for Graceful Controller #5003

SakshayMahna · 2025-03-22T07:00:53Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4865
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo Simulation of Turtlebot3
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Added default parameter files for Regulated Pure Pursuit Controller and Graceful Controller
Added 2 new parameters for Graceful Controller (lookahead_resolution and interpolate_after_goal)
Added lookahead logic to calculate next path point for Graceful Controller

Additional Information

Graceful Controller now calculates the next point on path using lookahead logic, it starts from a maximum lookahead value and searches for a path using lookahead distance in intervals of lookahead_resolution.
The controller also calculates interpolated point after goal. This can be switched on and off using the parameter interpolate_after_goal.

Description of documentation updates required from your changes

Added new parameter, so need to add that to default configs and documentation page.

Description of how this change was tested

Same unit tests and tested in simulation.

Future work that may be required in bullet points

The interpolation after goal logic seems a bit problematic and may need refinement.

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

mergify · 2025-03-22T07:04:40Z

@SakshayMahna, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-03-23T06:21:35Z

@SakshayMahna, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-03-23T06:37:50Z

@SakshayMahna, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

codecov · 2025-03-29T06:13:43Z

Codecov Report

Attention: Patch coverage is 94.59459% with 4 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...v2_graceful_controller/src/graceful_controller.cpp	73.33%	4 Missing ⚠️

Files with missing lines	Coverage Δ
...e/nav2_graceful_controller/graceful_controller.hpp	`100.00% <ø> (ø)`
...ude/nav2_graceful_controller/parameter_handler.hpp	`100.00% <ø> (ø)`
nav2_graceful_controller/src/parameter_handler.cpp	`100.00% <100.00%> (ø)`
...t_controller/regulated_pure_pursuit_controller.hpp	`100.00% <ø> (ø)`
...ntroller/src/regulated_pure_pursuit_controller.cpp	`85.63% <100.00%> (-2.71%)`	⬇️
nav2_util/src/controller_utils.cpp	`100.00% <100.00%> (ø)`
...v2_graceful_controller/src/graceful_controller.cpp	`89.34% <73.33%> (-0.35%)`	⬇️

... and 5 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

SteveMacenski

Thinking aloud a bit:

This bit of code looks like we iterate through the path, try to find the max_lookahead distance away and back up closer and closer to the robot until we find a solution that is collision free.

Is that case, we should attempt to find the interpolated lookahead point for the max_lookahead away to start off with, that gives us the best and smoothest value.

After that point, its a bit arbitrary the distance selected, isn't it? Selecting a lookahead_resolution versus marching through the path points I don't think improves the smoothness, does it? Either way there's a discontinuity now on the distance away to use based on the collision state (unless we set the search resolution to be very fine, which is probably not computationally worthwhile).

For example, the resolution you set was 0.1, but a normal path from a planning plugin probably does it closer to 0.05 for the costmap resolution when that resolution is 5cm. So wouldn't this case actually be more coarse?

Did you test this without the lookahead_resolution feature and generate those charts using the normal path point iteration? I suspect the improvement in smoothness you see is when the max_lookahead is used due to lack of collision requiring backing up the path to finding the first valid trajectory. My intuition says that when we do the backing-up process, we own that discontinuity either way, given we're backing up by 5cm or 10cm, respectively.

nav2_graceful_controller/params/graceful_controller_params.yaml

nav2_regulated_pure_pursuit_controller/params/regulated_pure_pursuit_params.yaml

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

SakshayMahna · 2025-04-03T16:37:03Z

This bit of code looks like we iterate through the path, try to find the max_lookahead distance away and back up closer and closer to the robot until we find a solution that is collision free.

Yes, this is exactly what is being done. The reasoning behind this was, in case max_lookahead distance has a collision, there is atleast some solution (found by backing up closer to robot) that can be followed.

Is that case, we should attempt to find the interpolated lookahead point for the max_lookahead away to start off with, that gives us the best and smoothest value.

After that point, its a bit arbitrary the distance selected, isn't it? Selecting a lookahead_resolution versus marching through the path points I don't think improves the smoothness, does it? Either way there's a discontinuity now on the distance away to use based on the collision state (unless we set the search resolution to be very fine, which is probably not computationally worthwhile).

For example, the resolution you set was 0.1, but a normal path from a planning plugin probably does it closer to 0.05 for the costmap resolution when that resolution is 5cm. So wouldn't this case actually be more coarse?

Makes sense, the discontinuity is introduced once we move to another lookahead point. That's why thought of putting lookahead_resolution to adjust what may work best for the user.

Did you test this without the lookahead_resolution feature and generate those charts using the normal path point iteration? I suspect the improvement in smoothness you see is when the max_lookahead is used due to lack of collision requiring backing up the path to finding the first valid trajectory. My intuition says that when we do the backing-up process, we own that discontinuity either way, given we're backing up by 5cm or 10cm, respectively.

Velocity plot for using points on path (not using lookahead_resolution) seems almost similar:

As long as the lookahead distance remains the same, the path is as smooth as it can be. It's only in case of collision we may need a strategy to generate a different solution for the robot. In this case the strategy is to reduce the lookahead distance, which introduces discontinuity. Possibly a different strategy may minimize this discontinuity.

SteveMacenski · 2025-04-08T17:22:57Z

That's why thought of putting lookahead_resolution to adjust what may work best for the user.

Do you see an improvement using the lookahead_resolution instead of marching through the path as originally done in the back up procedure?

By default you have it set to 0.1, but the path resolution is ~0.05. If you lower that to 0.05 or even lower like 0.02, is it smoother than before?

If so, I think that's a neat feature and we can keep it, users can tune to their liking and we can set it default to the costmap resolution.

If not, I think maybe we should revert that bit and only use the interpolation when using the full lookahead distance. Else, just use the path points since there's a jump anyway.

SakshayMahna · 2025-04-11T07:15:06Z

No, I don't see any improvement while using a smaller lookahead resolution.

The plots for the 3 cases (smaller resolution, larger resolution, using points) all look similar.
I'll just use the path points then.

nav2_graceful_controller/src/graceful_controller.cpp

nav2_util/src/interpolation_utils.cpp

nav2_util/include/nav2_util/interpolation_utils.hpp

nav2_graceful_controller/src/graceful_controller.cpp

nav2_util/include/nav2_util/interpolation_utils.hpp

nav2_graceful_controller/src/graceful_controller.cpp

SteveMacenski

There's one superficial refactor that would in my opinion make this easier to read / understand:

  // Calculate target pose through lookahead interpolation
  double dist_to_target = params_->max_lookahead;
  geometry_msgs::msg::PoseStamped target_pose = nav2_util::getLookAheadPoint(
    dist_to_target, transformed_plan, params_->interpolate_after_goal);
  bool valid_target_pose_found = validateTargetPose(target_pose, dist_to_target, dist_to_goal, local_plan, costmap_transform, cmd_vel);

  if (!valid_target_pose_found) {
    ...
    ...

    valid_target_pose_found = validateTargetPose(target_pose, dist_to_target, dist_to_goal, local_plan, costmap_transform, cmd_vel);
    if (valid_target_pose_found) {
      break;
    }
  }

This would remove the else statement and explicitly setting the valid_target_pose_found instead of just capturing the validate method's return.

nav2_graceful_controller/src/graceful_controller.cpp

SteveMacenski · 2025-05-16T18:48:53Z

@SakshayMahna can you work on these last couple of updates?

SakshayMahna · 2025-05-20T00:19:56Z

Hello Steve,
Yes, missed the last few notifications and emails. Working on it.

mergify · 2025-05-28T11:46:40Z

This pull request is in conflict. Could you fix it @SakshayMahna?

SakshayMahna · 2025-06-02T11:39:10Z

Hi @SteveMacenski
I did make the requested changes. However, I am not able to think of a way to get the code coverage to target (90.72%), I am stuck at 87.64%.

What I understand it is because of the fallback code that was added, that in case Lookahead Point is not found, fallback to default behaviour of checking the poses one by one.

The only way I think this can be tested is by adding an obstacle in the cost map. That way, the Lookahead Point will become invalid because of collision check by simulate_trajectory, and the execution would go to fallback code. But still I have doubts on how to make this work, because there is a set number of max iterations simulate_trajectory works for.

What do you think about this? Any ideas to improve the code coverage?

SteveMacenski

I think some of these changes in refactoring will resolve the testing gap! :-)

nav2_graceful_controller/src/parameter_handler.cpp

nav2_util/src/controller_utils.cpp

nav2_graceful_controller/src/graceful_controller.cpp

mergify · 2025-06-08T06:19:30Z

This pull request is in conflict. Could you fix it @SakshayMahna?

mergify · 2025-06-08T06:28:42Z

@SakshayMahna, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-06-08T10:03:12Z

This pull request is in conflict. Could you fix it @SakshayMahna?

SakshayMahna · 2025-06-08T10:38:05Z

Hi @SteveMacenski
I made a mistake while syncing the main branch. And managed to get all my commits deleted.
However, I had a backup, and was able to recover the relevant changes.
And I tested the changes running on simulation as well.

Sorry for the mess.

SteveMacenski

You have a few open comments above, but overall doesn't this look simpler and cleaner! Great work 😄

nav2_graceful_controller/src/parameter_handler.cpp

nav2_util/src/controller_utils.cpp

nav2_graceful_controller/src/graceful_controller.cpp

mergify · 2025-06-16T06:45:12Z

@SakshayMahna, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-08-08T16:15:49Z

This pull request is in conflict. Could you fix it @SakshayMahna?

mergify · 2025-08-08T16:46:47Z

@SakshayMahna, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Merge latest changes

SakshayMahna · 2025-08-08T16:55:50Z

Reverted the extra commits and pulled from main.

SteveMacenski · 2025-08-08T18:10:37Z

Some of the tests are failing from these changes:

/opt/overlay_ws/src/navigation2/nav2_graceful_controller/test/test_graceful_controller.cpp:295
Expected equality of these values:
  node->get_parameter("test.transform_tolerance").as_double()
    Which is: 0.1
  1.0
    Which is: 1

/opt/overlay_ws/src/navigation2/nav2_graceful_controller/test/test_graceful_controller.cpp:295
Expected equality of these values:
  node->get_parameter("test.transform_tolerance").as_double()
    Which is: 0.1
  1.0
    Which is: 1/opt/overlay_ws/src/navigation2/nav2_graceful_controller/test/test_graceful_controller.cpp:296
Expected equality of these values:
  node->get_parameter("test.min_lookahead").as_double()
    Which is: 0.25
  1.0
    Which is: 1

/opt/overlay_ws/src/navigation2/nav2_graceful_controller/test/test_graceful_controller.cpp:296
Expected equality of these values:
  node->get_parameter("test.min_lookahead").as_double()
    Which is: 0.25
  1.0
    Which is: 1/opt/overlay_ws/src/navigation2/nav2_graceful_controller/test/test_graceful_controller.cpp:297
Expected equality of these values:
  node->get_parameter("test.max_lookahead").as_double()
    Which is: 1
  2.0
    Which is: 2

/opt/overlay_ws/src/navigation2/nav2_graceful_controller/test/test_graceful_controller.cpp:297
Expected equality of these values:
  node->get_parameter("test.max_lookahead").as_double()
    Which is: 1
  2.0
    Which is: 2unknown file
C++ exception with description "test.interpolate_after_goal" thrown in the test body.

unknown file
C++ exception with description "test.interpolate_after_goal" thrown in the test body.

SteveMacenski · 2025-08-08T18:11:38Z

C++ exception with description "test.interpolate_after_goal" thrown in the test body.

This seems to be the issue, it doesn't update since that doesn't exist as a valid dynamic parameter so the other tests fail due to those not being updated from the failure

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

SteveMacenski · 2025-08-11T17:43:45Z

@SakshayMahna thank you for the diligence! I know that took awhile! This is a really great update though to the controller and in architecture so we don't have that duplication - thanks so much!

SakshayMahna · 2025-08-12T03:58:17Z

Thank you for pushing through as well Steve!
Happy to see it finally merged.

mini-1235 · 2025-09-09T07:03:45Z

After this PR, I often observe that the motion target heading is opposite to the robot's movement direction, is this expected?

SteveMacenski · 2025-09-11T16:09:44Z

Can you expand on that? I'm not sure @SakshayMahna and I fully grokk the picture's meaning.

SteveMacenski · 2025-09-11T16:14:12Z

nav2_util/src/controller_utils.cpp

+    // 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);


@SakshayMahna this block is new compared to the version in RPP. Maybe this shuld be scrutinized?

Yes, this is the problem. There is a discontinuity taking place because of which the angle comes out to be reversed (added / subtracted by pi).

mini-1235 · 2025-09-11T16:16:38Z

Can you expand on that? I'm not sure @SakshayMahna and I fully grokk the picture's meaning.

The red arrow is the motion target published by the graceful controller, while the blue path is the transformed plan. I thought the head of the red arrow should be the same direction with the transformed plan

If that's unclear, I will record a video tomorrow

SteveMacenski · 2025-09-11T16:17:40Z

I agree it probably should be. Did you do an A-B test that this is definitely the PR that changes that behavior?

mini-1235 · 2025-09-11T16:22:35Z

I agree it probably should be. Did you do an A-B test that this is definitely the PR that changes that behavior?

I did, after reverting this commit, I saw the heading is always the same as the transformed plan

SteveMacenski · 2025-09-11T16:27:44Z

@SakshayMahna does that make sense? Can you take a look :-)

@mini-1235 do you have a reproduction example on the Tb3/4 sims that he can use as a reference to test?

mini-1235 · 2025-09-11T16:34:14Z

@mini-1235 do you have a reproduction example on the Tb3/4 sims that he can use as a reference to test?

It is very late in my time zone, I will write it down here tomorrow:)

mini-1235 · 2025-09-12T13:29:28Z

@mini-1235 do you have a reproduction example on the Tb3/4 sims that he can use as a reference to test?

I replace the controller plugin in nav2_params and left everything else as default:

FollowPath:
      plugin: "nav2_graceful_controller::GracefulController"

Then launch the tb3 demo

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=0

I will show my result before and after this PR:

Before

2025-09-12.21-21-12.mp4

After

2025-09-12.21-14-55.mp4

SakshayMahna · 2025-09-15T10:25:06Z

Hi @mini-1235 ,
Thank you for raising this. I understand the problem. I'll have a look.

SakshayMahna · 2025-09-15T16:20:57Z

As pointed out by Steve above, the problem is in the yaw interpolation. There is a discontinuity taking place because of which the angle comes out to be reversed (added / subtracted by pi).

I think I have a fix for it. Keeping the yaw value in a way to keep it in the direction of motion of robot.

mini-1235 · 2025-09-17T13:18:22Z

As pointed out by Steve above, the problem is in the yaw interpolation. There is a discontinuity taking place because of which the angle comes out to be reversed (added / subtracted by pi).

I think I have a fix for it. Keeping the yaw value in a way to keep it in the direction of motion of robot.

@SakshayMahna Feel free to ping me when you open the PR, I can test it :)

SakshayMahna · 2025-09-17T15:24:53Z

Thank you!
@mini-1235 Could you please have a look? I have created this PR: #5530

SteveMacenski · 2025-09-17T18:41:19Z

Love it guys - please take a look and I will too!

…s-navigation#5003) * Add controller utils Signed-off-by: Sakshay Mahna <sakshum19@gmail.com> * Fix linting issues Signed-off-by: Sakshay Mahna <sakshum19@gmail.com> * Update regulated pure pursuit Signed-off-by: Sakshay Mahna <sakshum19@gmail.com> * Update graceful controller Signed-off-by: Sakshay Mahna <sakshum19@gmail.com> * Remove interpolate after goal & Use orientationAroundZAxis Signed-off-by: Sakshay Mahna <sakshum19@gmail.com> * Update nav2_util/src/controller_utils.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_util/src/controller_utils.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_util/src/controller_utils.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Remove interpolate_after_goal parameter from test Signed-off-by: Sakshay Mahna <sakshum19@gmail.com> --------- Signed-off-by: Sakshay Mahna <sakshum19@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

SakshayMahna force-pushed the main branch from 38a8bc3 to 532caff Compare March 23, 2025 06:18

SakshayMahna mentioned this pull request Mar 29, 2025

[Graceful Controller] Additional Controller Techniques - Experimentation #4865

Open

9 tasks

SteveMacenski reviewed Apr 1, 2025

View reviewed changes

mikeferguson reviewed Apr 11, 2025

View reviewed changes

nav2_graceful_controller/src/graceful_controller.cpp Show resolved Hide resolved

SteveMacenski reviewed Apr 15, 2025

View reviewed changes

nav2_util/src/interpolation_utils.cpp Outdated Show resolved Hide resolved

nav2_util/include/nav2_util/interpolation_utils.hpp Outdated Show resolved Hide resolved

nav2_graceful_controller/src/graceful_controller.cpp Show resolved Hide resolved

SteveMacenski reviewed May 2, 2025

View reviewed changes

nav2_util/include/nav2_util/interpolation_utils.hpp Show resolved Hide resolved

nav2_graceful_controller/src/graceful_controller.cpp Show resolved Hide resolved

SteveMacenski requested changes May 2, 2025

View reviewed changes

nav2_graceful_controller/src/graceful_controller.cpp Outdated Show resolved Hide resolved

nav2_graceful_controller/src/graceful_controller.cpp Outdated Show resolved Hide resolved

nav2_graceful_controller/src/graceful_controller.cpp Outdated Show resolved Hide resolved

SteveMacenski reviewed Jun 2, 2025

View reviewed changes

SakshayMahna force-pushed the main branch from 0ae4e27 to 74631ae Compare June 8, 2025 06:18

SakshayMahna force-pushed the main branch from 408107c to 686bc65 Compare June 8, 2025 10:02

SakshayMahna closed this Jun 8, 2025

SakshayMahna force-pushed the main branch from 686bc65 to 7f561b0 Compare June 8, 2025 10:05

SakshayMahna reopened this Jun 8, 2025

SteveMacenski requested changes Jun 9, 2025

View reviewed changes

SakshayMahna force-pushed the main branch from 61b6375 to 8a9412d Compare August 8, 2025 16:46

Merge pull request #2 from ros-navigation/main

a26850b

Merge latest changes

Remove interpolate_after_goal parameter from test

719d711

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

SteveMacenski approved these changes Aug 11, 2025

View reviewed changes

SteveMacenski merged commit bab2a86 into ros-navigation:main Aug 11, 2025
12 of 15 checks passed

SteveMacenski reviewed Sep 11, 2025

View reviewed changes

mini-1235 mentioned this pull request Sep 16, 2025

Add exact path following in rpp for feasible plans #5496

Closed

8 tasks

SakshayMahna mentioned this pull request Sep 17, 2025

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

Closed

mini-1235 mentioned this pull request Sep 26, 2025

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

Merged

8 tasks

Calculate next path point using lookahead for Graceful Controller #5003

Calculate next path point using lookahead for Graceful Controller #5003

Uh oh!

Conversation

SakshayMahna commented Mar 22, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

mergify bot commented Mar 22, 2025

Uh oh!

mergify bot commented Mar 23, 2025

Uh oh!

mergify bot commented Mar 23, 2025

Uh oh!

codecov bot commented Mar 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SakshayMahna commented Apr 3, 2025

Uh oh!

SteveMacenski commented Apr 8, 2025

Uh oh!

SakshayMahna commented Apr 11, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented May 16, 2025

Uh oh!

SakshayMahna commented May 20, 2025

Uh oh!

mergify bot commented May 28, 2025

Uh oh!

SakshayMahna commented Jun 2, 2025

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Jun 8, 2025

Uh oh!

mergify bot commented Jun 8, 2025

Uh oh!

mergify bot commented Jun 8, 2025

Uh oh!

SakshayMahna commented Jun 8, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

codecov bot commented Mar 29, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SakshayMahna commented Jun 8, 2025 •

edited

Loading

SteveMacenski commented Sep 11, 2025 •

edited

Loading