Cannot undock after docking backwards

[ffilosomi]

Hi,

I was able to integrate the Docking Server into my application (ROS2 Jazzy with Nav2 1.4.2 built and sourced as an overlay) and dock my robot backwards. It worked perfectly!

However, I was surprised when I tried to undock: the robot didn’t move forward to the staging pose and instead rotated slowly in place. It didn’t leave the dock, and the undock action failed.

I have the impression that the undocking action does not take the backward docking direction into account, and it tries to back out to the staging pose even when the robot was docked backwards and should undock forwards.

Any comments would be highly appreciated.

Federico Filosomi

[SteveMacenski]

Can you try the version on main (Nav2 main can compile on Jazzy) and see if you find the same issue? We did some work w.r.t. backwards docking a bit ago and it may just be that some part of it wasn't backported to Jazzy. Before trying to debug the error, it would be good to know if the error's already been resolved

[ffilosomi]

I’ll give it a try ASAP.
By the way, the part of the code causing the issue is this one (line 696 in docking_server.cpp):

// Make sure that the staging pose is pointing in the same direction when moving backwards
if (dock_backward) {
dock_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
tf2::getYaw(dock_pose.pose.orientation) + M_PI);
}

This is also present in main.
Replacing M_PI with 0 immediately solved my problem.

If the robot docks backwards, I want it to move forwards when it leaves the base.
It’s not necessary—and it doesn’t seem logical (at least to me)—to rotate it by 180° while it’s docked and then has to leave the base.

Frankly speaking, I’m not an expert, but I can tell you that changing that line solved my issue, and now everything works perfectly!

[SteveMacenski]

That’s a similar part of the code we’ve messed with in the main branch recently related to backwards docking, so I think the main branch would fix you. Then it’s just about finding the right PR to back port to Jazzy