Docking Server

Source code on Github.

The Docking Server in opennav_docking implements a server for docking and undocking a robot. This can be from Charging stations (i.e. docks) or non-charging docking locations such as the end of a conveyor belt or a pallet. 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, charging-type, 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!).

Thanks to NVIDIA for sponsoring this Docking Server package!

Parameters

controller_frequency

Type

Default

double

50.0

Description

Control frequency (Hz) for vision-control loop.

initial_perception_timeout

Type

Default

double

5.0

Description

Timeout (s) to wait to obtain initial perception of the dock.

wait_charge_timeout

Type

Default

double

5.0

Description

Timeout (s) to wait to see if charging starts after docking.

dock_approach_timeout

Type

Default

double

30.0

Description

Timeout (s) to attempt vision-control approach loop.

undock_linear_tolerance

Type

Default

double

0.05

Description

Tolerance (m) to exit the undocking control loop at staging pose.

undock_angular_tolerance

Type

Default

double

0.05

Description

Angular Tolerance (rad) to exist undocking loop at staging pose.

max_retries

Type

Default

int

3

Description

Maximum number of retries to attempt.

base_frame

Type

Default

string

“base_link”

Description

Robot’s base frame for control law.

fixed_frame

Type

Default

string

“odom”

Description

Fixed frame to use, recommended to be a smooth odometry frame not map.

dock_backwards

Type

Default

bool

false

Description

Whether the robot is docking with the dock forward or backward in motion.

dock_prestaging_tolerance

Type

Default

double

0.5

Description

L2 distance in X,Y,Theta from the staging pose to bypass navigation.

dock_plugins

Type

Default

vector<string>

N/A

Description

A set of dock plugins to load.

dock_database

Type

Default

string

N/A

Description

The filepath to the dock database to use for this environment. Use docks or this param.

docks

Type

Default

vector<string>

N/A

Description

Instead of dock_database, the set of docks specified in the params file itself. Use dock_database or this param.

navigator_bt_xml

Type

Default

string

“”

Description

BT XML to use for Navigator, if non-default.

controller.k_phi

Type

Default

double

3.0

Description

Ratio of the rate of change of angle relative to distance from the target. Much be > 0.

controller.k_delta

Type

Default

double

2.0

Description

Higher values result in converging to the target more quickly.

controller.beta

Type

Default

double

0.4

Description

Parameter to reduce linear velocity proportional to path curvature. Increasing this linearly reduces the velocity (v(t) = v_max / (1 + beta * |curv|^lambda)).

controller.lambda

Type

Default

double

2.0

Description

Parameter to reduce linear velocity proportional to path curvature. Increasing this exponentially reduces the velocity (v(t) = v_max / (1 + beta * |curv|^lambda)).

controller.v_linear_min

Type

Default

double

0.1

Description

Minimum velocity for approaching dock.

controller.v_linear_max

Type

Default

double

0.24

Description

Maximum velocity for approaching dock.

controller.v_angular_max

Type

Default

double

0.75

Description

Maximum angular velocity for approaching dock.

controller.slowdown_radius

Type

Default

double

0.25

Description

Radius to end goal to commense slow down.

Note: dock_plugins and either docks or dock_database are required.

SimpleChargingDock Parameters

Simple Charging Dock is a provided charging dock plugin that can handle many docks and common techniques.

<dock_name>.staging_x_offset

Type

Default

double

-0.7

Description

Staging pose offset forward (negative) of dock pose (m).

<dock_name>.staging_yaw_offset

Type

Default

double

0.0

Description

Staging pose angle relative to dock pose (rad).

<dock_name>.use_battery_status

Type

Default

bool

true

Description

Whether to use the battery state message or isDocked() for isCharging().

<dock_name>.use_external_detection_pose

Type

Default

bool

false

Description

Whether to use external detection topic for dock or use the databases’ pose.

<dock_name>.external_detection_timeout

Type

Default

double

1.0

Description

Timeout (s) at which if the newest detection update does not meet to fail.

<dock_name>.external_detection_translation_x

Type

Default

double

-0.20

Description

X offset from detected pose for docking pose (m).

<dock_name>.external_detection_translation_y

Type

Default

double

0.0

Description

Y offset from detected pose for docking pose (m).

<dock_name>.external_detection_rotation_yaw

Type

Default

double

0.0

Description

Yaw offset from detected pose for docking pose (rad).

<dock_name>.external_detection_rotation_pitch

Type

Default

double

1.57

Description

Pitch offset from detected pose for docking pose (rad). Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in image_proc and isaac_ros.

<dock_name>.external_detection_rotation_roll

Type

Default

double

-1.57

Description

Roll offset from detected pose for docking pose (rad). Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in image_proc and isaac_ros.

<dock_name>.filter_coef

Type

Default

double

0.1

Description

Dock external detection method filtering algorithm coefficient.

<dock_name>.charging_threshold

Type

Default

double

0.5

Description

Threshold of current in battery state above which isCharging() = true.

<dock_name>.use_stall_detection

Type

Default

bool

false

Description

Whether or not to use stall detection for isDocked() or positional threshold.

<dock_name>.stall_joint_names

Type

Default

vector<string>

N/A

Description

Names in joint_states topic of joints to track.

<dock_name>.stall_velocity_threshold

Type

Default

double

1.0

Description

The joint velocity below which to trigger isDocked() = true.

<dock_name>.stall_effort_threshold

Type

Default

double

1.0

Description

Current or motor effort in joint state to trigger isDocked() = true.

<dock_name>.docking_threshold

Type

Default

double

0.05

Description

If not using stall detection, the pose threshold to the docking pose where isDocked() = true.

Example

docking_server:
  ros__parameters:
    controller_frequency: 50.0
    initial_perception_timeout: 5.0
    wait_charge_timeout: 5.0
    dock_approach_timeout: 30.0
    undock_linear_tolerance: 0.05
    undock_angular_tolerance: 0.1
    max_retries: 3
    base_frame: "base_link"
    fixed_frame: "odom"
    dock_backwards: false
    dock_prestaging_tolerance: 0.5

    # Types of docks
    dock_plugins: ['nova_carter_dock']
    nova_carter_dock:
      plugin: 'opennav_docking::SimpleChargingDock'  # Also 'opennav_docking::SimpleNonChargingDock'
      docking_threshold: 0.05
      staging_x_offset: -0.7
      use_external_detection_pose: true
      use_battery_status: false # true
      use_stall_detection: false

      external_detection_timeout: 1.0
      external_detection_translation_x: -0.18
      external_detection_translation_y: 0.0
      external_detection_rotation_roll: -1.57
      external_detection_rotation_pitch: -1.57
      external_detection_rotation_yaw: 0.0
      filter_coef: 0.1

    # Dock instances
    docks: ['home_dock']
    home_dock:
      type: 'nova_carter_dock'
      frame: map
      pose: [0.0, 0.0, 0.0]
      id: 'c67f50cb-e152-4720-85cc-5eb20bd85ce8'

    controller:
      k_phi: 3.0
      k_delta: 2.0
      v_linear_min: 0.15
      v_linear_max: 0.15