New Goal checker: AxisGoalChecker #5746

doisyg · 2025-12-01T16:36:02Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	Ubuntu
Robotic platform tested on	Dexory bot
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

This pull request adds a new goal checker plugin called AxisGoalChecker to the nav2_controller package. The AxisGoalChecker checks if the robot has reached the goal by evaluating progress along the axis defined by the last segment of the path, with configurable tolerances and an option to allow overshoot.

Description of documentation updates required from your changes

ros-navigation/docs.nav2.org#838

Description of how this change was tested

Multiple days running straight paths ("segments") in a huge warehouses. And then rebased on main.

Future work that may be required in bullet points

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
Should this be backported to current distributions? If so, tag with backport-*.

SteveMacenski

Beyond anything else; I think this configuration guide entry will need a graphic to explain the value / when it should be used. I don't think this is immediately clear and could be easily misunderstood, so I think that element more than most is key.

I agree on naming. How about (and maybe not any better):

segment_axis_goal_tolerance -> Why not just leave as goal_tolerance or whatever it is now? It just has a different meaning for this critic (i.e. computed differently)
AxisGoalChecker -> PathApproachAngleGoalChecker?

nav2_controller/src/controller_server.cpp

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp

nav2_controller/plugins/axis_goal_checker.cpp

doisyg · 2025-12-02T10:01:28Z

segment_axis_goal_tolerance -> Why not just leave as goal_tolerance or whatever it is now? It just has a different meaning for this critic (i.e. computed differently)

Renamed

AxisGoalChecker -> PathApproachAngleGoalChecker?

Still not sure about that one, I will get started with the documentation and see if I get more inspired (I still want to reflect that we check the pregression along an axis or line)

SteveMacenski · 2025-12-02T18:46:56Z

Also: maybe instead of giving it the second-to-last point and modifying the API for just that information, how about giving the path in general to the goal checker so that it can use the full path instead (and this particular one just uses the second to last). It also pushes the changes into the specific new plugin rather than the core server - which I like from a separation of concerns perspective.

... I think there's a PR somewhere that actually was making that change some time ago for the goal checker to check on the path length as part of goal checking to deal with path looping. So this is very much aligned. #5446

SteveMacenski · 2025-12-02T18:47:47Z

PS @mini-1235

doisyg · 2025-12-02T19:11:40Z

Also: maybe instead of giving it the second-to-last point and modifying the API for just that information, how about giving the path in general to the goal checker so that it can use the full path instead (and this particular one just uses the second to last). It also pushes the changes into the specific new plugin rather than the core server - which I like from a separation of concerns perspective.

Oh that would be much better indeed !

... I think there's a PR somewhere that actually was making that change some time ago for the goal checker to check on the path length as part of goal checking to deal with path looping. So this is very much aligned. #5446

Nice! Let me know the progress. And I will pick up this PR once #5446 will be done then. Putting it to draft in the meantime. Also because there may be some trigonometry edge cases.

tonynajjar · 2025-12-16T16:07:24Z

(Please ignore any shenanigans happening on this branch until further notice)

Copilot

Pull request overview

This PR introduces a new goal checker plugin called AxisGoalChecker, designed specifically for validating goals in line path scenarios. The checker validates goal achievement based on the robot's projected distance along the axis formed by the last two poses of the path, rather than using standard Euclidean distance.

Key Changes:

New AxisGoalChecker plugin that projects robot position onto the path's final segment axis
Configurable overshoot behavior via is_overshoot_valid parameter for handling localization jumps
Fallback to standard distance checking for single-pose paths

Reviewed changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated 9 comments.

File	Description
nav2_controller/plugins/axis_goal_checker.cpp	Implementation of the new goal checker with axis-based projection logic
nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp	Header file defining the AxisGoalChecker class interface
nav2_controller/CMakeLists.txt	Build configuration for the new axis_goal_checker library
nav2_controller/plugins.xml	Plugin registration for the AxisGoalChecker

nav2_controller/plugins/axis_goal_checker.cpp

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp

nav2_controller/plugins/axis_goal_checker.cpp

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp

nav2_controller/plugins/axis_goal_checker.cpp

Copilot · 2026-01-05T17:01:38Z

nav2_controller/plugins/axis_goal_checker.cpp

+  pose_tolerance.orientation =
+    nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);


The orientation tolerance is set to M_PI_2 (90 degrees), which doesn't align with the goal checker's design that ignores orientation entirely. For consistency with PositionGoalChecker (which also doesn't check orientation), this should be set to an identity quaternion (x=0, y=0, z=0, w=1) to indicate that orientation is not checked, rather than using an arbitrary angle value.

Suggested change

pose_tolerance.orientation =

nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);

// Identity quaternion indicates that orientation is not checked

pose_tolerance.orientation.x = 0.0;

pose_tolerance.orientation.y = 0.0;

pose_tolerance.orientation.z = 0.0;

pose_tolerance.orientation.w = 1.0;

pose_tolerance.orientation isn't actually used anywhere but conceptually shouldn't it be nav2_util::geometry_utils::orientationAroundZAxis(M_PI) to indicate that orientation is completely unconstrained. @doisyg

nav2_controller/plugins/axis_goal_checker.cpp

codecov · 2026-01-05T17:22:50Z

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_controller/plugins/axis_goal_checker.cpp	`100.00% <100.00%> (ø)`

... and 30 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 · 2026-01-05T21:44:28Z

Send me a note on slack when you want me to take a look at this

tonynajjar · 2026-01-05T22:04:22Z

Today I rebased it on main, adapting it to the centralizing of the path handler. Tomorrow I'll look into it more in details for polishing. You can however already take a look - it was already tested by @doisyg and in use since some months

SteveMacenski · 2026-01-06T01:36:55Z

It generally look fine to me, I just think some illustrations and/or expanded doxygen description (which can be used in the nav2 docs later as well) to explain how / why this is useful are necessary.

SteveMacenski · 2026-01-06T18:11:27Z

Some quick test here would be nice. Otherwise, make sure it still works for your needs after recent changes but LGTM

tonynajjar · 2026-01-06T18:20:40Z

Some quick test here would be nice

Yes, they're written but some fail. Probably tomorrow

SteveMacenski · 2026-01-06T18:25:54Z

Fix DCO as well :-) Then we can merge the pair

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar · 2026-01-07T09:58:24Z

@doisyg ready to take a look

Copilot

Pull request overview

Copilot reviewed 6 out of 6 changed files in this pull request and generated 12 comments.

nav2_controller/plugins/axis_goal_checker.cpp

nav2_controller/plugins/test/axis_goal_checker.cpp

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp

nav2_controller/plugins/axis_goal_checker.cpp

nav2_controller/plugins/test/axis_goal_checker.cpp

nav2_controller/plugins.xml

nav2_controller/plugins/axis_goal_checker.cpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Copilot

Pull request overview

Copilot reviewed 6 out of 6 changed files in this pull request and generated 7 comments.

nav2_controller/plugins/axis_goal_checker.cpp

Copilot · 2026-01-07T10:43:19Z

nav2_controller/plugins/axis_goal_checker.cpp

+    double cross_track_distance = distance_to_goal * sin(projection_angle);
+
+    if (is_overshoot_valid_) {
+      return along_path_distance < along_path_tolerance_ &&


The overshoot validation logic appears to have a bug. When is_overshoot_valid_ is true, the condition along_path_distance < along_path_tolerance_ will always be true for any overshoot (negative along_path_distance), regardless of how far past the goal the robot is. This means the test at line 350-353 which expects any overshoot to be valid is actually revealing a bug rather than correct behavior. The condition should likely be along_path_distance < along_path_tolerance_ && along_path_distance > -along_path_tolerance_ to constrain overshoots within the tolerance range, or simply remove the absolute value check but keep a reasonable bounds check.

Suggested change

return along_path_distance < along_path_tolerance_ &&

return along_path_distance < along_path_tolerance_ &&

along_path_distance > -along_path_tolerance_ &&

intended behavior but FYI @doisyg

nav2_controller/plugins/test/axis_goal_checker.cpp

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp

nav2_controller/plugins/axis_goal_checker.cpp

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Copilot

Pull request overview

Copilot reviewed 6 out of 6 changed files in this pull request and generated 14 comments.

nav2_controller/plugins/test/axis_goal_checker.cpp

nav2_controller/plugins/axis_goal_checker.cpp

nav2_controller/plugins/test/axis_goal_checker.cpp

nav2_controller/plugins/axis_goal_checker.cpp

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp

nav2_controller/plugins/axis_goal_checker.cpp

* Add axis goal checker Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * Apply suggestion from @Copilot Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * PR comments Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * revert Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * rename + add param Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * rename Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * simplify Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * remove import Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * add tests Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * Apply suggestions from code review Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add path_length_tolerance as dynamic param Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * Apply suggestion from @Copilot Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * handle case where points are very close Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * handle atan2(0,0) and add tests Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * fix params Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * PR fixes Signed-off-by: Tony Najjar <tony.najjar@dexory.com> --------- Signed-off-by: Tony Najjar <tony.najjar@dexory.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Tony Najjar <tony.najjar@dexory.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Add axis goal checker Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * Apply suggestion from @Copilot Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * PR comments Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * revert Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * rename + add param Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * rename Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * simplify Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * remove import Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * add tests Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * Apply suggestions from code review Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add path_length_tolerance as dynamic param Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * Apply suggestion from @Copilot Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * handle case where points are very close Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * handle atan2(0,0) and add tests Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * fix params Signed-off-by: Tony Najjar <tony.najjar@dexory.com> * PR fixes Signed-off-by: Tony Najjar <tony.najjar@dexory.com> --------- Signed-off-by: Tony Najjar <tony.najjar@dexory.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Tony Najjar <tony.najjar@dexory.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: lotusymt <mengtiy5@uci.edu>

SteveMacenski reviewed Dec 1, 2025

View reviewed changes

nav2_controller/src/controller_server.cpp Outdated Show resolved Hide resolved

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp Show resolved Hide resolved

nav2_controller/plugins/axis_goal_checker.cpp Outdated Show resolved Hide resolved

doisyg marked this pull request as draft December 2, 2025 19:11

tonynajjar mentioned this pull request Dec 22, 2025

Centralize path handler logic in controller server #5446

Merged

tonynajjar force-pushed the axis_goal_checker branch from 60a16b6 to 257100a Compare January 5, 2026 16:47

tonynajjar requested a review from Copilot January 5, 2026 16:56

Copilot started reviewing on behalf of tonynajjar January 5, 2026 16:57 View session

Copilot AI reviewed Jan 5, 2026

View reviewed changes

tonynajjar marked this pull request as ready for review January 6, 2026 17:57

tonynajjar mentioned this pull request Jan 6, 2026

Axis goal checker docs ros-navigation/docs.nav2.org#838

Merged

tonynajjar and others added 9 commits January 7, 2026 10:56

Add axis goal checker

ed714bf

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Apply suggestion from @Copilot

1e8fd8f

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

PR comments

4929d27

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

revert

db40de8

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

rename + add param

2d232ba

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

rename

fd6bd95

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

simplify

ae77ed4

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

remove import

b264a0b

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

add tests

e4b4352

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar force-pushed the axis_goal_checker branch from e5791f9 to e4b4352 Compare January 7, 2026 09:56

tonynajjar requested review from SteveMacenski, Copilot and tonynajjar and removed request for tonynajjar January 7, 2026 09:57

Copilot started reviewing on behalf of tonynajjar January 7, 2026 09:58 View session

Copilot AI reviewed Jan 7, 2026

View reviewed changes

tonynajjar and others added 5 commits January 7, 2026 11:10

Apply suggestions from code review

16dc722

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

add path_length_tolerance as dynamic param

413e6ba

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Apply suggestion from @Copilot

1947d86

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

handle case where points are very close

69fb133

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

handle atan2(0,0) and add tests

777c7bd

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar requested a review from Copilot January 7, 2026 10:38

Copilot started reviewing on behalf of tonynajjar January 7, 2026 10:38 View session

Copilot AI reviewed Jan 7, 2026

View reviewed changes

fix params

b69bc4f

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

SteveMacenski reviewed Jan 7, 2026

View reviewed changes

PR fixes

a7d0d34

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

tonynajjar requested a review from Copilot January 8, 2026 11:58

Copilot started reviewing on behalf of tonynajjar January 8, 2026 11:58 View session

Copilot AI reviewed Jan 8, 2026

View reviewed changes

tonynajjar requested a review from SteveMacenski January 8, 2026 13:41

SteveMacenski approved these changes Jan 8, 2026

View reviewed changes

SteveMacenski merged commit 7b20878 into ros-navigation:main Jan 8, 2026
23 checks passed

		pose_tolerance.orientation =
		nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);

-  pose_tolerance.orientation =
-    nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);
+  // Identity quaternion indicates that orientation is not checked
+  pose_tolerance.orientation.x = 0.0;
+  pose_tolerance.orientation.y = 0.0;
+  pose_tolerance.orientation.z = 0.0;
+  pose_tolerance.orientation.w = 1.0;

	return along_path_distance < along_path_tolerance_ &&
	return along_path_distance < along_path_tolerance_ &&
	along_path_distance > -along_path_tolerance_ &&

New Goal checker: AxisGoalChecker #5746

New Goal checker: AxisGoalChecker #5746

Uh oh!

Conversation

doisyg commented Dec 1, 2025 • edited by tonynajjar Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

doisyg commented Dec 2, 2025

Uh oh!

SteveMacenski commented Dec 2, 2025

Uh oh!

SteveMacenski commented Dec 2, 2025

Uh oh!

doisyg commented Dec 2, 2025

Uh oh!

tonynajjar commented Dec 16, 2025

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI Jan 5, 2026

Choose a reason for hiding this comment

Uh oh!

tonynajjar Jan 6, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

codecov bot commented Jan 5, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

SteveMacenski commented Jan 5, 2026

Uh oh!

tonynajjar commented Jan 5, 2026

Uh oh!

SteveMacenski commented Jan 6, 2026

Uh oh!

SteveMacenski commented Jan 6, 2026

Uh oh!

tonynajjar commented Jan 6, 2026

Uh oh!

SteveMacenski commented Jan 6, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

tonynajjar commented Jan 7, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

doisyg commented Dec 1, 2025 •

edited by tonynajjar

Loading

codecov bot commented Jan 5, 2026 •

edited

Loading

SteveMacenski commented Jan 6, 2026 •

edited

Loading

tonynajjar Jan 7, 2026 •

edited

Loading