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.

controller.use_collision_detection

Type	Default
bool	true

Description: Whether to use collision detection to avoid obstacles.

controller.costmap_topic

Type	Default
string	“local_costmap/costmap_raw”

Description: Raw costmap topic for collision checking.

controller.footprint_topic

Type	Default
string	“local_costmap/published_footprint”

Description: Topic for footprint in the costmap frame.

controller.transform_tolerance

Type	Default
double	0.1

Description: Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.

controller.projection_time

Type	Default
double	1.0

Description: Time to look ahead for collisions (s).

controller.simulation_time_step

Type	Default
double	0.1

Description: Time step for projections (s).

controller.dock_collision_threshold

Type	Default
double	0.3

Description: Distance (m) from the dock pose to ignore collisions, i.e. the robot will not check for collisions within this distance from the dock pose, as the robot will make contact with the dock. Set to 0.0 when physical contact is not made with a dock.

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