Nav2 Docking: Add Option for Reversing into Dock

[SteveMacenski]

Currently, we detect the dock and use the vision control loop to drive into the dock. If you have sensors in the back, you can set the staging pose to reversed & dock_backwards to dock. If you do not have sensors in back, there should be an option to dock_backwards but stage forward, thus needs:

• Detect dock
• Rotate to 180 deg heading
• Reverse into the dock (without updates to detected pose)

We'd just need to add the option for that 180 maneuver

[SantoshGovindaraj]

Hello @SteveMacenski, I would like to contribute to this issue as my first step in open-source.
I see that #4406 would be good starting point for me to understand the premise.

Let me know some additional reference that would be helpful to better understand docking.

[SantoshGovindaraj]

Hi @SteveMacenski, I wanted to kindly follow-up. I am really interested in contributing and would love to help move this forward if it's still open or if there are any updates.

Please let me know if there's anything I can assist with or if the issue is now closed. Looking forward to hearing from you.
Thank you in advance. 🙌

[SteveMacenski]

Hi,

We'd love the contribution!

This issue is still open and describes the creation of a behavior that allows the robot to detect the dock while pointed forward towards the dock (where a sensor may exist), rotate 180 to be in reverse (where a sensor may not exist) and then back into the dock based on that single-point detection.

This is likely less accurate than having a sensor, but if you're hardware constrained or do not need very fine accuracies, this is a valuable feature addition. Essentially, we want a parameter that enacts this behavior. After we detect the dock, we check if we should rotate before reversing into the dock. We should also check when we grab the parameters if its properly configured (i.e. this parameter is only valid when dock_backward = true)

Does that make sense? I think that should be enough to get started! You can use the Nav2 Rviz docking panel as well for testing of this feature to make it really easy :-)

[YoulunPeng]

@SteveMacenski I will take this one :D

https://github.com/YoulunPeng/navigation2/tree/feat/add_option_for_reversing_into_dock

[SteveMacenski]

Ok great! Its probably worth reviewing the current thread in #4749 where this was started but didn't get completed. You can probably either start from that and read the review comments or take some inspiration from it!

[padhupradheep]

@YoulunPeng do you have any updates here?

[ajtudela]

@padhupradheep I'm finishing this up. I'll submit the PR in a couple of days. I'm just writing the final tests.