Backward docking stucks on Humble

[Jakubach]

Hello, when using Backward docking mode the robot correctly reverse to the docking pose but very near docking pose it starts doing in-place circles without the end. The problem only occurs it this mode.
The solution I've found:

The SmoothControlLaw method used in docking server is calculateRegularVelocity(pose, backward) (link) and changing it to that one taking also the robot current pose fixes the problem: calculateRegularVelocity(const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current,const bool & backward = false) (link)
I haven't checked that but the problem can also occur in the GracefulController since it's implemented similarly (link).
I also had to delete the lines with backward_projection (428-432) (link) (then I modified it for my case to perform initial in-place rotations).

In summary:

• Used calculateRegularVelocity(pose,current, backward) instead of calculateRegularVelocity(pose, backward)
• Commented out the part with backward_projection(and that part does not exist in GracefulController)

After this changes the robot stops correctly in the end point (with default controller parameters)

Screencast.from.11-07-2024.06_53_45.PM.1.webm

Screencast.from.11-08-2024.04_42_24.PM.1.mp4

I will propose that changes on pull request.

[ajtudela]

Hi, I think I have a clear idea of what's happening to you:

• First, make sure that the dock orientation is facing the wall (not at the robot):

• Second, assuming you have sensors in the back to detect the dock, set the staging pose to be reversed using the staging_yaw_offset.

NOTES FOR ME: Improve the documentation for these cases and implement detection+rotate+reverse ASAP.

[Jakubach]

Hi, thank you for your response. I've tried re-orientating the dock pose (rotation by PI around Z) so if the robot pose is [0,0 0] then dock pose is [0,0, PI].
To the second point, my robot has a sensor to detect the dock in a front of it (and charging connectors are on back). I was playing with staging_yaw_offset but the dock detection starts after reaching the staging pose not during reaching that pose (then it could work).

[ajtudela]

Unfortunately, your case: sensors in the front and docking pads in the back is not currently supported. There is an open issue in nav2 here.

I saw your PR. If you want to implement this feature, you need to do it in the main branch of Nav2 and I'll review it.

[Jakubach]

I made a PR in the main branch of Nav2

[SteveMacenski]

This all sounds good, thanks @ajtudela for responding to @Jakubach and helping to review! Its very appreciated :-)