Fix - #5094 rotation shim stateful goal checking

[PrabhavSaxena]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #5094 )
Primary OS tested on	(Ubuntu)
Robotic platform tested on	(gazebo simulation of Tally)
Does this PR contain AI generated software?	(No)

---

Description of contribution in a few bullet points

• Added a new plugin called position_goal_checker, which checks only the xy tolerance and ignores the yaw tolerance. Used the same plugin internally in Rotation Shim Controller.

Description of documentation updates required from your changes

• Should I add position_goal_checker in the documentation, as it is a new plugin?

Description of how this change was tested

• Validated RotationShimController in turtlebot simulation.

---

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

[mergify]

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

[mergify]

@PrabhavSaxena, all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @humble, but it must be in main
to have these changes reflected into new distributions.

[SteveMacenski]

I don't think this works from the discussion in #5094 (comment) can you comment on that?

[PrabhavSaxena]

I don't think this works from the discussion in #5094 (comment) can you comment on that?

The issue occurs because the rotation shim controller uses position-only tolerance checking to decide when to switch to rotation mode. However, due to localization shifts, if the robot briefly moves outside the XY tolerance during rotation, it switches back to the primary controller, causing oscillation.

The change I've implemented uses the existing stateful behavior of goal_checker->isGoalReached().

1. First check if the robot is within both XY and angular tolerances
2. Remember when the robot has entered the goal region
3. Continue to return true for subsequent calls, even with minor position shifts

This "memory" is exactly what's needed to solve the oscillation problem - once the controller enters rotation mode, it will stay there despite minor localization shifts.

One potential concern might be that isGoalReached() also checks orientation. For a more complete solution, we might need to:

1. Either implement a truly position-only stateful check
2. Or modify the withinPositionGoalTolerance to maintain its own state once position tolerance is met

I'd appreciate your guidance on which approach would better align with the architecture of Nav2.

[SteveMacenski]

Or modify the withinPositionGoalTolerance to maintain its own state once position tolerance is met

I think this is what we should do ultimately. The goal checker that is given to the controller from the server can be any plugin considering any number of criteria. We can't rely on its implementation for internal controller behavior. We could essentially create a "stateful position goal checker" style object (or even that literal new plugin that we directly instantiate and use internally) to use in the controller to meet this need.

[PrabhavSaxena]

Or modify the withinPositionGoalTolerance to maintain its own state once position tolerance is met

I think this is what we should do ultimately. The goal checker that is given to the controller from the server can be any plugin considering any number of criteria. We can't rely on its implementation for internal controller behavior. We could essentially create a "stateful position goal checker" style object (or even that literal new plugin that we directly instantiate and use internally) to use in the controller to meet this need.

I have added the plugin to check the position only. I have also added rotation_shim to internally use the same. Let me know if this works

[SteveMacenski]

Last thing is that we need this PR to also target main so that Rolling, Jazzy, etc can have these changes also reflected. That way when you eventually upgrade, this is also included. All new features must be in main.

[SteveMacenski]

Needs to be added in docs to this list with its configuration page: https://docs.nav2.org/configuration/packages/configuring-controller-server.html#provided-plugins. Here as well: https://docs.nav2.org/plugins/index.html.

[PrabhavSaxena]

Hi, I will resolve the comments right now, I had few questions about the same.

1. Should I direct this PR to main branch (which requires some effort because I made this PR out of humble branch) or should I merge this PR (with humble) and create a new one which targets main branch. Or if there is some easier way to make my changes target the main branch.
2. Should I remove the tools/utils.hpp file in nav2_rotation_shim_controller as we are no longer using the withinPositionGoalTolerance function?

[SteveMacenski]

For main we should also have the goal checker reset when we reset the controller (which happens at the end of a planning request https://github.com/ros-navigation/navigation2/blob/main/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp#L401-L405)

[PrabhavSaxena]

done

[SteveMacenski]

1. Leave this PR and lets get this approved first. Then once its done, you should apply this to main (with a tweak I mention in a comment above) and we can merge all of them together
2. Yes please unless you think it has reusability elsewhere :-)

[PrabhavSaxena]

1. Leave this PR and lets get this approved first. Then once its done, you should apply this to main (with a tweak I mention in a comment above) and we can merge all of them together

   2. Yes please unless you think it has reusability elsewhere :-)

cool, got it :)

[SteveMacenski]

I look forward to the main PR :-)

[PrabhavSaxena]

I look forward to the main PR :-)

I created a PR for the same (Pull request 5162)