Docking - Unexpected behavior when trying to dock while already docked or undock while already not docked

[jcarlosgm30]

Hi @SteveMacenski ,

I think there are two non-optimal behaviors in the docking server:

• If you send a goal to dock the robot with navigate_to_staging_pose set to true but the robot is already docked, then it tries to navigate to the docking pose. (video)
• If you send a goal to undock when the robot is not docked, then it will undock. I think this is dangerous because the robot navigates without avoiding obstacles. (video)

what do you think?

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Humble
• Version or commit hash:
  • 635880d3d6ad1f23eb75d793409189c653797f58
• DDS implementation:
  • Cyclone DDS

Steps to reproduce issue

Dock behavior:

1. Dock the robot
2. Send another goal into /dock_robot with navigate_to_staging_pose set to true

Undock behavior:

1. Send a goal into ' /undock_robot' while the robot is not docked

Expected behavior

Dock behavior:

• Abort the action goal and log that the robot is already docked

Undock behavior:

• Abort the action goal and log that the robot is not docked

Actual behavior

Dock behavior

• It tries to navigate to the staging pose but the navigator is not able to manage it because the robot is "in collision" with the dock.

Undock behavior:

• It executes the undocking maneuver.

Additional information

---

Feature request

I think it would be nice if the docking server checks if the robot is docked or not before the execution of dock / undock maneuvers.

Feature description

Implementation considerations

[jcarlosgm30]

Here you can find a video with the behavior I would expect in these cases

[ajtudela]

Hi Carlos,

Yes, I faced the same behavior while testing the docking_server and is one of the things I'm improving. I was planning on fixing it in later PRs but if you don't want to wait, here is the fix.

[jcarlosgm30]

Hi @ajtudela ,

Good catch!

I was preparing a PR pretty similar to yours. Here there is a video with the behavior after the fix I did.

Since you have opened the PR, I'll save myself from opening it. Let's wait for the review then!

[SteveMacenski]

https://github.com/ros-navigation/navigation2/pull/4448 addresses