search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.3k stars 1.2k forks source link

Nav2 Docking: Send a navigation goal while the robot is docking or is already docked #4503

Closed jcarlosgm30 closed 4 days ago

jcarlosgm30 commented 4 days ago

Hi @SteveMacenski ,

I have found two undesired behaviors (in my opinion) in the docking framework that I believe are due to the decoupling between navigation and docking.

Is there a plan to improve this, or should it be addressed through the use of behavior trees?

Bug report

Required Info:

Steps to reproduce issue

Behavior 1: Send a navigation goal while the robot is executing the docking maneuver
1. Execute the `/dock_robot` action.
2. Send a navigation goal while the robot executes the docking maneuver.
3. Then, the robot navigates to the new navigation goal. When the robot reaches the goal, it tries to dock because the docking action wasn't aborted. 
Behavior 2: Send a navigation goal while the robot is docked
1. Send a navigation goal while the robot is already docked.
2. The navigation system tries to navigate to the goal but the robot is in collision, so the recovery behaviors can be executed. This is dangerous if there is a recovery behavior that doesn't check if the robot is in collision.

Expected behavior

Actual behavior

Additional information

Feature request

Feature description

Implementation considerations

ajtudela commented 4 days ago

Hi Carlos,

For the first behavior, if I'm not mistaken, successive calls to the NavigateToPose will replace the goal to navigate to. On the other hand, once the robot is sent to the staging pose, the navigator doesn't check if it's already there before making the initial perception.

https://github.com/ros-navigation/navigation2/blob/77425546565df0df072bc1e5606d26aa23573bf4/nav2_docking/opennav_docking/src/docking_server.cpp#L249-L259

This can be solved by checking that the robot is back in the staging pose before attempting to dock.

For the second behaviour, as it shows in the application_example, you should add an UnDock BT node in your navigator main tree to check if the robot is docked before the start.

image

SteveMacenski commented 4 days ago

Behavior 1

That's on your autonomy system to be tracking state appropriately and cancel previous commands before issuing a new one. /navigate_to_pose is not /dock_robot so these actions are not related to each other, there's not an easy / clean way for them to know about each other - and moreso - /dock_robot will even call /navigate_to_pose internally if desired to get to the staging pose, so making them mutually exclusive isn't possible.

Honestly speaking having built / seen alot of robotics products: I think this is bad application design that should be solved at your application layer to cancel any on-going robot command before issuing a new one. This isn't unique for docking and navigation, imagine you had some assisted remote teleoperation or higher level robot behavior outside of navigation. Only one action that results in cmd_vels to the base should be operating at a time and that's on your application design coordinating many such actions to manage. You could do this a number of ways from trivially simple to very complicated but very powerful and it depends on your needs.

Behavior 2

Its really easy to just add an undock-robot command at the start of your Nav2 BT so that no matter what, if you're docked, you'll just undock first. That should outright solve your second behavior trivially. Though, another option is to keep proper handle on the robot's state and explicitly undock only when needed, but its up to you about which direction makes most sense for your solution.

tl;dr: Just add an if docked -> undock at the start of your BT and that should just be handled for you automatically. Alternatively, if you're managing your robot state, you should know if docked to undock at the autonomy level, but both are perfectly acceptable.

Conclusion

I think this should be closed since there's no actionable changes to Nav2 to be made here. Simple changes are needed by @jcarlosgm30

jcarlosgm30 commented 1 day ago

thanks so much for the replies guys! @SteveMacenski @ajtudela

Controlling it at the application level using BT is the way I was thinking, but I wasn't sure if it was the way you had in mind when it was designed. It's all clear!

have a good navigation! ;)