Feature request

Currently, the docking server has the following behavior:

• Navigate to the staging pose
• Approach the dock
• Once at the dock, wait for the docking to be confirmed (charging, stall, ...)

Feature description

I would like to add an additional step while waiting for the dock to be confirmed. At this stage, we know that the robot is at the dock location with a tolerance. So it would be nice to be able to configure small steps that should be performed in order to actually get the dock connected, or improve the connection.

It should consist in small rotations or small linear movement. I am mainly thinking about charging dock, sometimes it is a matter of a few centimeters to get the charger connected, and the approach control does not perform those adjustement steps with enough robustness.

Implementation considerations

• It should be part of the dock plugin (each dock has its own mechanical constraints)
• It should take into account the robot position and the refined dock position
• It should take into account the quality of the docking pose. For example, for a wireless charger, at 3cm we can already charge, but it is better to be even closer.