Open Navigation’s Nav2 Docking Framework

This package contains an automatic robot docking framework & auxiliary tools. It uses plugin dock implementations for a particular platform to enable the framework to generalize to robots of many different kinematic models, charging methods, sensor modalities, and so on. It can also handle a database of many different docking locations and dock models to handle a heterogeneous environment. This task server is designed be called by an application BT or autonomy application to dock once completed with tasks or battery is low – not within the navigate-to-pose action itself (though undock may be called from inside navigate actions!).

This work is sponsored by NVIDIA and created by Open Navigation LLC.

This is split into 4 packages

• opennav_docking: Contains the main docking framework
• opennav_docking_msgs: Contains the action interfaces for docking and undocking
• opennav_docking_core: Contains the dock plugin header file to be implemented for each dock type
• opennav_docking_bt: Contains behavior tree nodes and example XML files using the docking task server
• nova_carter_docking: Contains an implementation using the Docking system with the Nvidia Nova Carter Robot platform and dock.

NOTE: This capability has been migrated within Nav2 itself as of June 2024 and nova_carter_docking was migrated to Nova Carter’s GitHub in August 2024.

⚠️ Need ROS 2, Nav2 help or support? Contact Open Navigation! ⚠️

Click on the image above to see an extended video of docking in action.

Architecture

The Docking Framework has 5 main components:

• DockingServer: The main action server and logic for performing the docking/undocking actions
• Navigator: A NavigateToPose action client to navigate the robot to the dock’s staging pose if not with the prestaging tolerances
• DockDatabase: A database of dock instances in an environment and their associated interfaces for transacting with each type. An arbitrary number of types are supported.
• Controller: A spiral-based graceful controller to use for the vision-control loop for docking
• ChargingDock: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find this plugin header in the opennav_docking_core package.

The ChargingDock plugins are the heart of the customizability of the framework to support any type of charging dock for any kind of robot. The DockDatabase is how you describe where these docks exist in your environment to interact with and any of them may be used in your docking request.

The docking procedure is as follows:

1. Take action request and obtain the dock’s plugin and its pose
2. If the robot is not within the prestaging tolerance of the dock’s staging pose, navigate to the staging pose
3. Use the dock’s plugin to initially detect the dock and return the docking pose
4. Enter a vision-control loop where the robot attempts to reach the docking pose while its actively being refined by the vision system
5. Exit the vision-control loop once contact has been detected or charging has started
6. Wait until charging starts and return success.

If anywhere this procedure is unsuccessful, N retries may be made by driving back to the dock’s staging pose and trying again. If still unsuccessful, it will return a failure code to indicate what kind of failure occurred to the client.

Undocking works more simply:

1. If previously docked, use the known dock information to get the dock type. If not, use the undock action request’s indicated dock type
2. Find the staging pose for this dock and back out to that pose
3. Check if successfully backed out to the pose and charging has stopped

Interfaces

Docking Action

The docking action can either operate on a dock in the DockDatabase or from a dock specified in the docking request. This second option is useful for testing or when dock’s locales are not necessarily known in advance. If use_dock_id = true, it uses the dock_id field to specify which dock in the database to use. Else, you must populate the dock_pose and dock_type fields.

If you wish for the docking server to stage your robot at the the dock’s staging pose for you, navigate_to_staging_pose must be true. Else, you can send your robot to this pose and it will be skipped as long as the robot is within the prestaging tolerances. You may set the maximum time for navigation using max_staging_time.

In return, you obtain the num_retries, for the number of attempted retries of the action; success, if the action worked and the robot is successfully charging; and errro_code to return a semantically meaningful error code about what kind of error occurred, if any. See DockRobot.action for more details.

While the action is performing, you can obtain feedback about the current state of docking, how much time docking_time has elapsed, and the current number of retries attempted.

Undocking Action

Undocking is similarly laid out. The action request contains the dock_type which is optional if the docking server previously docked the robot at its current dock. Else, it is required so that the undocking action can obtain the staging pose to back out to if there are multiple dock plugins specified (else, will use the default). There is also a maximum undocking time as a timeout for failures to uncouple itself from the dock, max_undocking_time.

The result similarly contains success and semantic error_code with no feedback.

Reload Database Service

This service exists to potentially reload the dock server’s known dock database with a new file of docks after it is loaded. Simply provide the filepath to your new set of docks and it shall be done!

Dock Specification

There are two unique elements to consider in specifying docks: dock instances and dock plugins. Dock instances are instances of a particular dock in the map, as the database may contain many known docks (and you can specify which by name you’d like to dock at). Dock plugins are the model of dock that each is an instance of. The plugins contain the capabilities to generically detect and connect to a particular dock model. This separation allows us to efficiently enable many individual docking locations of potentially several different revisions with different attributes.

The dock plugins are specified in the parameter file as shown below. If you’re familiar with plugins in other Nav2 servers, this should look like a familiar design pattern. Note that there is no specific information about the dock’s pose or instances. These are generic attributes about the dock revision (such as staging pose, enable charging command, detection method, etc). You can add additional parameters in the dock’s namespace as you choose (for example timeout).

dock_plugins: ["dockv1", "dockv3"]
dockv1:
  plugin: "my_custom_dock_ns::Dockv1"
dockv3:
  plugin: "my_custom_dock_ns::Dockv3"
  timeout: 10.0

There are two ways to populate the database of dock instances in your environment: through the parameter file or an external file. If you’d like to embed your dock information in your Docking Server config file (if you only have a couple of docks), you may use a similar method as defining the dock plugins, specifying the docks in the docks parameter. Note that we specify the plugin type and the dock’s location [x, y, theta] in a particular frame.

``` docks: [‘dock1’, ‘dock2’] dock1: type: “dockv3” frame: map

File truncated at 100 lines see the full file