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: double Default: 50.0: Control frequency (Hz) for vision-control loop.

`initial_perception_timeout`

Type: double Default: 5.0: Timeout (s) to wait to obtain initial perception of the dock.

`wait_charge_timeout`

Type: double Default: 5.0: Timeout (s) to wait to see if charging starts after docking.

`dock_approach_timeout`

Type: double Default: 30.0: Timeout (s) to attempt vision-control approach loop.

`undock_linear_tolerance`

Type: double Default: 0.05: Tolerance (m) to exit the undocking control loop at staging pose.

`undock_angular_tolerance`

Type: double Default: 0.05: Angular tolerance (rad) to exit undocking loop at staging pose.

`max_retries`

Type: int Default: 3: Maximum number of retries to attempt.

`base_frame`

Type: string Default: "base_link": Robot's base frame for control law.

`fixed_frame`

Type: string Default: "odom": Fixed frame to use, recommended to be a smooth odometry frame not map.

`dock_backwards`

Type: bool Default: false: Whether the robot is docking with the dock forward or backward in motion.

`dock_prestaging_tolerance`

Type: double Default: 0.5: L2 distance in X,Y,Theta from the staging pose to bypass navigation.

`action_server_result_timeout`

Type: double Default: 10.0 Unit: seconds: The timeout value (in seconds) for action servers to discard a goal handle if a result has not been produced. This used to default to 15 minutes in rcl but was changed to 10 seconds in this PR #1012, which may be less than some actions in Nav2 take to run. For most applications, this should not need to be adjusted as long as the actions within the server do not exceed this deadline. This issue has been raised with OSRF to find another solution to avoid active goal timeouts for bookkeeping, so this is a semi-temporary workaround.

`dock_plugins`

Type: vector<string> Default: N/A: A set of dock plugins to load.

`dock_database`

Type: string Default: N/A: The filepath to the dock database to use for this environment. Use docks or this param.

`docks`

Type: vector<string> Default: N/A: Instead of dock_database, the set of docks specified in the params file itself. Use dock_database or this param.

`navigator_bt_xml`

Type: string Default: "": BT XML to use for Navigator, if non-default.

`controller.k_phi`

Type: double Default: 3.0: Ratio of the rate of change of angle relative to distance from the target. Much be > 0.

`controller.k_delta`

Type: double Default: 2.0: Higher values result in converging to the target more quickly.

`controller.beta`

Type: double Default: 0.4: 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: double Default: 2.0: 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: double Default: 0.1: Minimum velocity for approaching dock.

`controller.v_linear_max`

Type: double Default: 0.24: Maximum velocity for approaching dock.

`controller.v_angular_max`

Type: double Default: 0.75: Maximum angular velocity for approaching dock.

`controller.slowdown_radius`

Type: double Default: 0.25: Radius to end goal to commense slow down.

`controller.rotate_to_heading_angular_vel`

Type: double Default: 1.0: Angular velocity (rad/s) to rotate to the goal heading.

`controller.rotate_to_heading_max_angular_accel`

Type: double Default: 3.2: Maximum angular acceleration (rad/s^2) to rotate to the goal heading.

`controller.use_collision_detection`

Type: bool Default: true: Whether to use collision detection to avoid obstacles.

`controller.costmap_topic`

Type: string Default: "local_costmap/costmap_raw": Raw costmap topic for collision checking.

`controller.footprint_topic`

Type: string Default: "local_costmap/published_footprint": Topic for footprint in the costmap frame.

`controller.transform_tolerance`

Type: double Default: 0.1: 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: double Default: 1.0: Time to look ahead for collisions (s).

`controller.simulation_time_step`

Type: double Default: 0.1: Time step for projections (s).

`controller.dock_collision_threshold`

Type: double Default: 0.3

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: double Default: -0.7: Staging pose offset forward (negative) of dock pose (m).

`<dock_name>.staging_yaw_offset`

Type: double Default: 0.0: Staging pose angle relative to dock pose (rad).

`<dock_name>.use_battery_status`

Type: bool Default: true: Whether to use the battery state message or isDocked() for isCharging().

`<dock_name>.use_external_detection_pose`

Type: bool Default: false: Whether to use external detection topic for dock or use the databases' pose.

`<dock_name>.external_detection_timeout`

Type: double Default: 1.0: Timeout (s) at which if the newest detection update does not meet to fail.

`<dock_name>.external_detection_translation_x`

Type: double Default: -0.2: X offset from detected pose for docking pose (m).

`<dock_name>.external_detection_translation_y`

Type: double Default: 0.0: Y offset from detected pose for docking pose (m).

`<dock_name>.external_detection_rotation_yaw`

Type: double Default: 0.0: Yaw offset from detected pose for docking pose (rad).

`<dock_name>.external_detection_rotation_pitch`

Type: double Default: 1.57

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: double Default: -1.57

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: double Default: 0.1: Dock external detection method filtering algorithm coefficient.

`<dock_name>.charging_threshold`

Type: double Default: 0.5: Threshold of current in battery state above which isCharging() = true.

`<dock_name>.use_stall_detection`

Type: bool Default: false: Whether or not to use stall detection for isDocked() or positional threshold.

`<dock_name>.stall_joint_names`

Type: vector<string> Default: N/A: Names in joint_states topic of joints to track.

`<dock_name>.stall_velocity_threshold`

Type: double Default: 1.0: The joint velocity below which to trigger isDocked() = true.

`<dock_name>.stall_effort_threshold`

Type: double Default: 1.0: Current or motor effort in joint state to trigger isDocked() = true.

`<dock_name>.docking_threshold`

Type: double Default: 0.05: 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
      v_angular_max: 0.75
      slowdown_radius: 0.25
      use_collision_detection: true
      costmap_topic: "local_costmap/costmap_raw"
      footprint_topic: "local_costmap/published_footprint"
      transform_tolerance: 0.1
      projection_time: 1.0
      simulation_time_step: 0.1
      dock_collision_threshold: 0.3