Codecov Report

❌ Patch coverage is 98.29545% with 3 lines in your changes missing coverage. Please review.

... and 6 files with indirect coverage changes

My vote would go towards AdaptiveToleranceGoalChecker! As soon as we agree on the name (and the implementation specifics) I will rename it and look towards implementing the doc changes

AdaptiveToleranceGoalChecker is it!

P.S. Not necessarily asking you to make this change yet, but something related to this is this function: https://github.com/ros-navigation/navigation2/blob/main/nav2_route/src/route_tracker.cpp#L54

In this, I check if we pass a tangental line at the goal pose orthogonal to the goal orientation. We call the node achieved not only when we're within tolerance, but when we actually pass that tangential line marking the goal-aligned pose has been passed (with whatever latitudinal error).

We may be able to use that kind of logic here as another "we're done" condition. If we are in the coarse window and we have just passed the goal pose "finish line", we should stop.

What do you think?

I'm not 100% sure at a lobbing-out-an-idea view, but that may replace the need for best_distance_sq_ entirely? Maybe good to do both, actually, for the case that it stops before it actually crosses that "finish line"

So I currently just implemented it as simple as possible by taking how the Stopped goal checker works and simply switched the current geometry approach for a velocity approach. This could already be further tested, I think @jkeo180 from the issue showed interest in helping maybe she can test it and maybe also give further ideas into possible improvements.

I would now take a look in the next couple of days ( as I do have some other stuff to do like my job :) ) which kinds of checks can we do for "usefullsness" and take a look at the route tracker implementation of node achieved to see how could we best implement it here. I will try to tackle it over the weekend or latest start of next week. Until than we can also further discuss which solution might fit best here. Of course as always all theoretical and implementation guidelines are very welcome!

I did also redo the tests you can take a look if it looks better.

I thought about the route tracker implementation you mentioned... Just to make sure we are on the same page implementation should run still side by side to the current stagnation implementation but with the extra nice to have of triggering instantly on crossing the "goal line"?

Something like

Now my questions would be:

1. Does this actually make sense as it is possble the robot will trigger this before trying to actually correct in case of overshooting
2. How to tackle if the robot is already approaching from the dot > 0 side due to us using navfn? Do we just track last known dot and if we go from < 0 -> > 0 than trigger but if already come from the side with > 0 just fall through to the stagnation checks?

Also btw my commits might be slow this week as I have quite a busy week. ☹️

Just to make sure we are on the same page implementation should run still side by side to the current stagnation implementation but with the extra nice to have of triggering instantly on crossing the "goal line"?

Yeah, precisely. The stagnation though should consider the change in distance and the velocity though (not as linked, but as 2 things that can both increment the counter).

Does this actually make sense as it is possble the robot will trigger this before trying to actually correct in case of overshooting

I think so? I'm not sure that some small correction will look different from a small velocity for the stagnation checks to get it. I think lets not try to over optimize here, we have enough going on already. I think if stopped / uselessly small velocity / jitter within coarse --> accept. If within coarse but moving away now with a counter --> accept. If in coarse and passed the "finish line" --> accept. If in fine at all --> accept.

Folks can add more in the future form there. I think that handles the realistic coarse-window optimizations that I can think of (stopped already / not making progress / instantly passed the line to bypass the stagnation counter so we don't continue to drive away once passed the tangent line).

How to tackle if the robot is already approaching from the dot > 0 side due to us using navfn? Do we just track last known dot and if we go from < 0 -> > 0 than trigger but if already come from the side with > 0 just fall through to the stagnation checks?

Maybe use the path approach orientation (angle from pose N and N-1 -- though probably not N-1 due to NavFn jitter. Maybe 3-5?) rather than the orientation of the goal pose itself.

Good catch.

Yeah, precisely. The stagnation though should consider the change in distance and the velocity though (not as linked, but as 2 things that can both increment the counter).

Yep that was the last implementation, please check if it is fine at its current state (2 seperate counters 1 for stopped one for distance stagnation).

I think if stopped / uselessly small velocity / jitter within coarse --> accept. If within coarse but moving away now with a counter --> accept. If in coarse and passed the "finish line" --> accept. If in fine at all --> accept.

Maybe use the path approach orientation (angle from pose N and N-1 -- though probably not N-1 due to NavFn jitter. Maybe 3-5?) rather than the orientation of the goal pose itself.

Okay I will take a look into implementing than this extra 3rd success trigger. I would do the N-x x as parameter just so we can also adapt to other planners when needed. I hope to get the commit out in the next couple of days.

Also please take a look at the current set of test. Once I implement what we discussed I will just add tests for the 3rd accept trigger in the same fashion.

The release test keeps failing due to it looking for the old file Could not find '/opt/overlay_ws/src/navigation2/nav2_controller/plugins/progress_goal_checker.cpp' to scan for exclusion markers... should I do something about that?

Okay so I tried to make it planner agnostic so that I take the first position within the coarse zone and use that to compute the finish line. If I am not mistaken this should handle what we want nicely and work with "all" planners. Maybe I skipped some edge case that this does not cover so please advise.

Here is than also a sketch of all 4 success paths possible (might be also good to add into the documentation later)

Okay so I tried to make it planner agnostic so that I take the first position within the coarse zone and use that to compute the finish line. If I am not mistaken this should handle what we want nicely and work with "all" planners. Maybe I skipped some edge case that this does not cover so please advise.

Clever, I like that

Here is than also a sketch of all 4 success paths possible (might be also good to add into the documentation later)

Great thanks! That'll be good for the configuration guide docs on this :-)

Nice thanks for working on that, it looks more complex than what I anticipated when I wrote the issue, but I trust that it's for good reason. I won't be able to review it in detail but will give it a test sometime next week.

What about adding tests? I think the complexity warrants some.

@SteveMacenski what do you think about this becoming the nav2 default?

@tonynajjar I did also add test here https://github.com/ros-navigation/navigation2/pull/6052/changes#diff-4a74fd3e34ade379d7ab0aea85ef84fc620ff36d10d8c1c3389b68e5877a9747R471-R872

Steve did ask about redoing them at the beginning, which I did, after which we did not comment on if they look good or not. The tests are made to cover all 3 success cases (it is very possible that I forgot some edge cases so please check)

apologies, I was blind 😅

@SteveMacenski what do you think about this become the nav2 default?

After you give it a whirl and are happy with it, I'm OK with that! I tentatiely approve pending your review

Hey @DavidG-Develop, I'm checking the logic now. The 'finish line' concept is exactly what I was hoping for to prevent the robot from oscillating at the edge of the goal.

What I did to test:

• Set the fine_xy_goal_tolerance to a reasonable distance and saw from the logs that the goal checker succeeded by using fine_xy_goal_tolerance
• Set the fine_xy_goal_tolerance very small and saw from the logs that the goal checker succeeded by using coarse_xy_goal_tolerance

That's the basic behavior I was looking for 👍 I didn't test every edge case though.

Maybe let's not immediately set this as the default - Once it's merged, I will try to see if we can have it to be the default internally for some time (+ hopefully other people test) and after this we can set it as the default.

@doisyg FYI, let's discuss if this has any drawbacks for us

@DavidG-Develop OK! Final steps:

• We need to add to the migration guide that this new goal checker exists
• We need to add to the configuration guide for the controller server the new goal checker
• Do some final tests in simulation using MPPI and RPP (~20 paths achieved at various angles each) to show that we correctly continue to go to the refined until we stall, achieve the fine bounds, and/or pass the finish line. Use TB3/4 sim from nav2 bringup. I basically just want you to sanity check the course-stopped, fine-achieved, or finish-line crossed behaviors using the full system to make sure there aren't unintended issues that are easily predictable from non-unit testing. For each situation at the end, make sure the right one triggered and the behavior was good with the counters (maybe we want to retune the default values?) - adding some temporary logging in the plugin may help in making this clear.

Then we can merge! Thanks for all this great work and would love your help in more tickets if you're open to it!

@SteveMacenski I will look that I get the docs written sometime next week.

For the testing, I did already do some testing on the tb4 sim by sending goals with poses tight next to obstacles, and I did observe all 4 cases proc at least once. I do also agree we should check and possibly tune the default values. To do that I would most likely build some evaluation script where I can than track the trajectory of the robot as well as I can visualise the finish line to see if all triggers correctly... Any recommendations for initial default values to start testing from?

Then we can merge! Thanks for all this great work and would love your help in more tickets if you're open to it!

Thanks for the kind feedback, I really appreciate it! Also thanks for the support on this one 😄 ! I usually would like to take on 1 ticket per month where I can then slowly work on it whenever I catch some time, but due to a full-time job, an extra part-time job at my Uni and always doing stuff for our Robocup team, the rest free time is mostly not enought for a ticket per month 🤣 . (just went back to check my last merge was sep 2025... ooops haha, so once a month was myb slightly extended 🤣 🤣 )

Great work!
Is it worth adding additional check for fine_xy_goal_tolerance_ > coarse_xy_goal_tolerance_ on updateParametersCallback() ?

@elsayedelsheikh I don't find it necessary... If someone sets fine_xy_goal_tolerance_ > coarse_xy_goal_tolerance_ than the checker would just act as a simple goal checker (maybe it can also be seen as an additional feature that the user can switch it during runtime from adaptive to simple). I could add a warning print to notify the user that that configuration is not intended, but I do not see a reason why it should be considered "wrong". Thoughts @SteveMacenski ?

@elsayedelsheikh I don't find it necessary... If someone sets fine_xy_goal_tolerance_ > coarse_xy_goal_tolerance_ than the checker would just act as a simple goal checker (maybe it can also be seen as an additional feature that the user can switch it during runtime from adaptive to simple). I could add a warning print to notify the user that that configuration is not intended, but I do not see a reason why it should be considered "wrong". Thoughts @SteveMacenski ?

A warning would be nice!

So I added the warn for coarse < fine, but this got me thinking for other params as well...

So currently I followed the setup of other checkers and do check that all doubles >= 0 and all ints >= 1 but only on the params callback and not on init... Now my question would be is it better that I follow the way the others are structured and do not check all params in init (assuming the user will not put in unreasonable values) , or would it better be to actually check them in init as well and either throw or just warn and use default? I made sure the code does not crash even with values which are not intended to be used but it does make it behave not as intended (never succeeding, succeeding instantly, and so on).

Also I noticed we never use result.reason in the validateParameterUpdatesCallback which I find to be sub optimal, but again I went with following the other checkers structure but I believe it would be better if we also populate that field with the reason before returning.

Thoughts? @SteveMacenski @elsayedelsheikh

In my opinion, users often overlook dynamic parameter updates, so a warning is necessary.
On initialization, they’re more likely to follow the documentation/guides.
Let's hear from @SteveMacenski

Okay so I built a script which does some testing and the following are the results:
MPPI default settings:

• Not close to obstacles always reaching fine tolerance
• Close to obstacles - 80% fine, 14% stop triggered, 6% distance triggered

To test the finish line I had to force it by setting params in a way no normal person should but also seems to trigger correctly

Now with RRP the story is much different... To start off I have never used RRP so my knowledge of the control algorithm behind it is 0! With me using default params even without obstacles it would 100% of the times trigger the distance stagnation due to it stopping short...

Now as I am not expirienced with it I would leave it up to you guys to check/decide if this is fine or it should be changed...

From my testes based on TB4 sim the default values are fine, but ofc for larger and slower robots they will have to be adjusted...

@DavidG-Develop please rebase/pull in main in order to have CI turn over.

I think everything was answered in your exchanges so I don't think there's anything for me to add more :-) Just the documentation!

@elsayedelsheikh did you give this a nice test / evaluation? I want to merge this in this week, but I would like ~2-3 outside parties to corroborate that it works well for their applications and gets better results (and no edge cases that we didn't handle)

@elsayedelsheikh did you give this a nice test / evaluation? I want to merge this in this week, but I would like ~2-3 outside parties to corroborate that it works well for their applications and gets better results (and no edge cases that we didn't handle)

I've run a few tests in simulation, and so far, so good. I'll continue tomorrow with more testing on a real robot, and if there's any problem, I'll let you know by the end of the day. :-)

@DavidG-Develop if you get the docs together, I can merge this by Friday!

@SteveMacenski Bruh you merged the docs already 🤣 🤣 I see why you asked on Linked to possibly find some other ppl who can also do the testing, aproving and merging 😆

🙃 sorry... I juggle alot of things and sometimes I forget the state of them

I think just some tests by the community to get a 👍 and I'll merge!

I don’t even know how @SteveMacenski keeps track of all these open tickets. 💪
I can barely remember what I had for breakfast 😅

P.S. My schedule finally works, and I’ve set aside 2 hours daily for Nav2. Feel free to mention me on any tickets.

How'd the hardware testing go @elsayedelsheikh?

@SteveMacenski
Unfortunately, I didn’t get the chance to test it on hardware since some motor driver issues showed up earlier today.

Anyway, this LGTM! I ran more simulation scenarios with different goal headings and also very close goals where making those turns is basically not possible, while keeping the goal checker fine tolerance quite tight so coarse tolerance shows its powers and It’s working really well with MPPI 💯

Maybe let's not immediately set this as the default - Once it's merged, I will try to see if we can have it to be the default internally for some time (+ hopefully other people test) and after this we can set it as the default.

+1

Thanks @DavidG-Develop for your help -- would welcome your assistence on other tickets too (or other things you might want to improve) if you wanted and had the time!

Hey @SteveMacenski I will try to take on an issue whenever I catch enough free time... In the meantime, you can always ping me for input or a quick sim test!