Rotate to goal heading when reaching goal

[gennartan]

Feature request

Rotate to goal heading when reaching goal

Feature description

When reaching the goal, the rotation shim controller could take back control of the robot to rotate to the goal heading.

Implementation considerations

The current implementation does not take it into account. It is the responsibility of the primary controller to reach the correct heading.

Here is a first idea of the implementation: https://github.com/open-navigation/navigation2/commit/94d3a9d8610d367088ad48fa066d378f31844601 Note: I could not test it yet, probably it is not fully working as is :smile:

[SteveMacenski]

I believe RPP/DWB already do this internally. I had planned to implement something like this but paused since it was already baked in and I didn't want them fighting over control to do it.

But, hey, not a bad idea and to be fair others have their own controller plugins. That seems like a logical contribution. I'd just make it a parameter and when we describe it in documentation, explain that some controllers do this automatically so it doesn't need to be enabled.

Also another good PR contribution, thanks @gennartan !

[gennartan]

Hello,

I updated my code (and tested it). You can find it here: https://github.com/gennartan/navigation2/commit/2587f63fd65908a7e9fa64f7b34847d3b11cb61c . I have a few questions before I can make a pull request. Maybe you can help ^^

• A path is a vector of ´PoseStamped´. Why is it not a vector of ´Pose´ ? Since I am performing a frame transform on the goal path, I have an TF error because the stamp of the pose is not valid anymore. And that force me to change the stamp of my goal point (which does not make really sense since this point should not have a relation to time)

• I am performing 2 transforms. One on the robot pose, and one on the goal pose. I am sure that it could be more efficient to only do one transform (either from the robot pose frame to the goal pose frame, or the other way around). Are the frames of each of these pose fixed ? Or does it vary depending on the integration ?

• I approximately copied the ´withinPositionGoalTolerance´ function from another file in nav2. Would it make sense to add this in nav2_utils ? If yes, would it make sense to do it with this feature or in a separate ticket ?

Thank you in advance for your responses !

[SteveMacenski]

A path is a vector of ´PoseStamped´. Why is it not a vector of ´Pose´ ?

Future proofing so that paths can be time series for kinodynamic planning. That's not how they're used currently, so you should use the timestamp in the Path message, no each individual pose's timestamp.

Or does it vary depending on the integration ?

In general assume nothing. Make it generalized so that you get that data from either parameters or from other content that has that parameter so it can be changed or adjusted for someone's situation. In this case, you have the costmap, so you have the reference frames that you're interested in as API calls (for example https://github.com/gennartan/navigation2/blob/main/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp#L205)

Would it make sense to add this in nav2_utils

Sure! Do in this PR.

[gennartan]

The PR is now open.

Finally I did not add the function to check if the goal is within tolerance to ´nav2_util´ because of a circular dependency issue. The goal checker is defined in the header file of ´nav2_core´, which depends on ´nav2_utils´. So I updated the code to have a similar structure than the one for the MPPI controller.