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 87.64045% with 11 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...v2_graceful_controller/src/graceful_controller.cpp	57.69%	11 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.39% <100.00%> (-2.79%)`	⬇️
nav2_util/src/controller_utils.cpp	`100.00% <100.00%> (ø)`
...v2_graceful_controller/src/graceful_controller.cpp	`81.60% <57.69%> (-8.09%)`	⬇️

... and 4 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.

mikeferguson · 2025-04-11T13:06:11Z

nav2_graceful_controller/src/graceful_controller.cpp

@@ -195,21 +195,19 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
    // Else, fall through and see if we should follow control law longer
  }

-  // Precompute distance to candidate poses
-  std::vector<double> target_distances;
-  computeDistanceAlongPath(transformed_plan.poses, target_distances);


I believe this will cause a minor regression - the getLookaheadPoint() function simply looks at the distance between the target point and the robot - not the distance ALONG the path. In some instances, this can lead to the controller massively shortcutting the desired path.

I would suggest making getLookaheadPoint use distance along path instead of hypot().

Right!
I can make this change in this PR.
Or another issue/PR would be appropriate for this?

I think having it done here is important since that is a regression from the current behavior in this branch 😄

Also, it is a nice feature which RPP would also share in since they share the implementation! I know that's actually been a topic of conversion in other threads / tickets to add that feature, so I think that's wonderful to add in

@SakshayMahna I believe this item is resolved, correct? @mikeferguson can you take another look?

nav2_util/src/interpolation_utils.cpp

nav2_util/include/nav2_util/interpolation_utils.hpp

SteveMacenski · 2025-04-15T14:34:15Z

nav2_graceful_controller/src/graceful_controller.cpp

@@ -195,21 +195,19 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
    // Else, fall through and see if we should follow control law longer
  }

-  // Precompute distance to candidate poses
-  std::vector<double> target_distances;
-  computeDistanceAlongPath(transformed_plan.poses, target_distances);


I think having it done here is important since that is a regression from the current behavior in this branch 😄

Also, it is a nice feature which RPP would also share in since they share the implementation! I know that's actually been a topic of conversion in other threads / tickets to add that feature, so I think that's wonderful to add in

nav2_util/include/nav2_util/interpolation_utils.hpp

SteveMacenski · 2025-05-02T00:04:56Z

nav2_graceful_controller/src/graceful_controller.cpp

@@ -195,21 +195,19 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
    // Else, fall through and see if we should follow control law longer
  }

-  // Precompute distance to candidate poses
-  std::vector<double> target_distances;
-  computeDistanceAlongPath(transformed_plan.poses, target_distances);


@SakshayMahna I believe this item is resolved, correct? @mikeferguson can you take another look?

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

SteveMacenski · 2025-06-02T15:14:08Z

nav2_util/src/controller_utils.cpp

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


Simplify: I think you can just set goal_pose_it = poses.begin() + i; each iteration and when d >= lookahead_dist only do the break. That would let you remove the pose_found logic and the later goal_pose_it = poses.end();.

@SakshayMahna

I gave the suggested implementation quite some tries. But for the suggested one, the robot gets assigned a goal pose beyond the end point automatically (even with interpolate_after_goal=false). And the behaviour just doesn't feel right.

However, with the current implementation everything seems perfectly fine.

I don't understand the reason, but I'm sure I maybe missing something.

Can you make a branch off this one and send a link here? I'll take a look

nav2_graceful_controller/src/graceful_controller.cpp

SteveMacenski · 2025-06-02T15:29:34Z

nav2_graceful_controller/src/graceful_controller.cpp

+  geometry_msgs::msg::PoseStamped target_pose = nav2_util::getLookAheadPoint(
+    dist_to_target, transformed_plan, params_->interpolate_after_goal);
+
+  valid_target_pose_found = validateTargetPose(


What if getLookAheadPoint returns transformed_plan.poses.back() because !pose_found? Then dist_to_target may not be params_->max_lookahead for the input

The loop in getLookAheadPoint iterates from each pose calculating distance along path. The loop for it only breaks when distance found is greater than max_lookahead. The only case, we get !pose_found is when the distance is not able to add up to max_lookahead.

This is fine, because the logic inside validateTargetPose only rejects the point if it is at a distance greater than max_lookahead or less than min_lookahead. Otherwise, the code logic continues to run the same.

I'm saying that you ahve this block of code:

dist_to_target = params_->max_lookahead; target_pose = nav2_util::getLookAheadPoint( dist_to_target, transformed_plan, params_->interpolate_after_goal);

If the target_pose is not exactly max_lookahead away because !pose_found, then the inputs to validateTargetPose aren't consistent. The target_pose is not actually dist_to_target away. dist_to_target is not updated based on the actual return of getLookAheadPoint. dist_to_target == params_->max_lookahead always in this case even when target_pose is the last pose instead.

I think something like the following is necessary:

if (target_pose == transformed_plan.poses.back() { // update dist_to_target }

Yes, understood now. It makes sense to change the lookahead distance if the path is clipped to end at the last pose.

@SakshayMahna OK! Let me know when its implemented :-)

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?

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

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.

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

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

SteveMacenski · 2025-06-09T22:14:52Z

nav2_util/src/controller_utils.cpp

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


@SakshayMahna

nav2_graceful_controller/src/graceful_controller.cpp

SteveMacenski · 2025-06-09T22:17:12Z

nav2_graceful_controller/src/graceful_controller.cpp

-    } else if (dist_to_target < params_->min_lookahead) {
-      // Make sure target is far enough away to avoid instability
-      break;
+  for (int i = transformed_plan.poses.size(); i >= 0; --i) {


If you make this an unsigned int instead of an int, then you don't need the static_cast below and nothing else needs to change

On making it unsigned int, i>=0 condition will always remain valid.
And I am getting a compiler error for the same.

@SakshayMahna unsigned int i = transformed_plan.poses.size(); i >= 0; --i then if (i == transformed_plan.poses.size())

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

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).

SteveMacenski · 2025-06-16T21:02:09Z

@SakshayMahna please rebase on main to make CI turn over. There was a message filter API change that we resolved in main

SteveMacenski

Super close now! Take a look at the remaining open items from previous reviews (should be 3x) :-) Thanks for the iterations - this is much improved!

SteveMacenski · 2025-06-16T21:03:02Z

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -35,6 +35,7 @@ struct Parameters
  double transform_tolerance;
  double min_lookahead;
  double max_lookahead;
+  bool interpolate_after_goal;


Remove since now unused :-)

SteveMacenski · 2025-06-16T21:04:36Z

nav2_graceful_controller/src/graceful_controller.cpp

-    } else if (dist_to_target < params_->min_lookahead) {
-      // Make sure target is far enough away to avoid instability
-      break;
+  for (int i = transformed_plan.poses.size(); i >= 0; --i) {


@SakshayMahna unsigned int i = transformed_plan.poses.size(); i >= 0; --i then if (i == transformed_plan.poses.size())

SteveMacenski · 2025-06-16T21:06:23Z

nav2_util/src/controller_utils.cpp

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


Can you make a branch off this one and send a link here? I'll take a look

SteveMacenski · 2025-06-16T21:08:19Z

nav2_graceful_controller/src/graceful_controller.cpp

+  geometry_msgs::msg::PoseStamped target_pose = nav2_util::getLookAheadPoint(
+    dist_to_target, transformed_plan, params_->interpolate_after_goal);
+
+  valid_target_pose_found = validateTargetPose(


@SakshayMahna OK! Let me know when its implemented :-)

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

SteveMacenski reviewed Apr 15, 2025

View reviewed changes

SteveMacenski reviewed May 2, 2025

View reviewed changes

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

Recover previous changes

57d71ae

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

SakshayMahna reopened this Jun 8, 2025

Fix linting issues

22e8e22

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

Fix test issue

8242788

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

SteveMacenski requested changes Jun 9, 2025

View reviewed changes

SakshayMahna added 3 commits June 16, 2025 09:01

Update lookahead distance if end of path about to reach

936c7da

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

Remove orientation interpolation

96d3433

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

Fix test and linting issues

fa0535b

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

SteveMacenski requested changes Jun 16, 2025

View reviewed changes

Calculate next path point using lookahead for Graceful Controller #5003

Are you sure you want to change the base?

Calculate next path point using lookahead for Graceful Controller #5003

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!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Apr 15, 2025 • 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!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

SteveMacenski Apr 15, 2025 • 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!

Choose a reason for hiding this comment

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!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

codecov bot commented Mar 29, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski Apr 15, 2025 •

edited

Loading

SteveMacenski Apr 15, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski Jun 9, 2025 •

edited

Loading

SakshayMahna commented Jun 8, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading