Docking helper

Overview

Dock Helper is a ROS node providing services and actions to assist with docking the robot to charging stations. It manages:

• Finding the nearest docking station
• Docking to a specific docking station by ID
• Docking to the nearest available docking station

The node obtains docking station information from the map and uses the Nav2 docking capability to execute the actual docking operation.

IMPORTANT

The node requires transform information between the robot's base_link and charging_port to calculate the correct approach position for docking.

Docking Station Information

Docking stations are received from the /mowing_map topic and stored internally. Each docking station includes:

• ID
• Name
• Pose (position and orientation)

Services

Find Nearest Docking Station /find_nearest_docking_station

This service finds the docking station closest to the robot's current position and returns its details.

---
# Result
uint16 CODE_SUCCESS = 0
uint16 CODE_NOT_FOUND = 1
uint16 CODE_UNKNOWN_ERROR = 99

DockingStation docking_station
uint16 code                # 0 is success, otherwise is error code
string message             # Information message

Actions

Dock Robot to Nearest Station /dock_robot_nearest

# goal definition
---
# Result codes
uint16 CODE_SUCCESS = 0
uint16 CODE_DOCK_NOT_IN_DB = 901
uint16 CODE_DOCK_NOT_VALID = 902
uint16 CODE_FAILED_TO_STAGE = 903
uint16 CODE_FAILED_TO_DETECT_DOCK = 904
uint16 CODE_FAILED_TO_CONTROL = 905
uint16 CODE_FAILED_TO_CHARGE = 906
uint16 CODE_UNKNOWN = 999

DockingStation chosen_docking_station
uint16 code                # 0 is success, otherwise is error code
string message             # Information message
uint16 num_retries         # Number of retries attempted
---
# Feedback
# Status codes
uint16 STATUS_NONE = 0
uint16 STATUS_NAV_TO_STAGING_POSE = 1
uint16 STATUS_INITIAL_PERCEPTION = 2
uint16 STATUS_CONTROLLING = 3
uint16 STATUS_WAIT_FOR_CHARGE = 4
uint16 STATUS_RETRY = 5

DockingStation chosen_docking_station
uint16 status              # Current status of docking process
string message             # Information message
uint16 num_retries         # Number of retries attempted
builtin_interfaces/Duration docking_time  # Docking time elapsed

Dock Robot to Specific Station /dock_robot_to

# goal definition
string dock_id
---
# Result codes
uint16 CODE_SUCCESS = 0
uint16 CODE_DOCK_NOT_IN_DB = 901
uint16 CODE_DOCK_NOT_VALID = 902
uint16 CODE_FAILED_TO_STAGE = 903
uint16 CODE_FAILED_TO_DETECT_DOCK = 904
uint16 CODE_FAILED_TO_CONTROL = 905
uint16 CODE_FAILED_TO_CHARGE = 906
uint16 CODE_UNKNOWN = 999

DockingStation chosen_docking_station
uint16 code                # 0 is success, otherwise is error code
string message             # Information message
uint16 num_retries         # Number of retries attempted
---
# Feedback
# Status codes
uint16 STATUS_NONE = 0
uint16 STATUS_NAV_TO_STAGING_POSE = 1
uint16 STATUS_INITIAL_PERCEPTION = 2
uint16 STATUS_CONTROLLING = 3
uint16 STATUS_WAIT_FOR_CHARGE = 4
uint16 STATUS_RETRY = 5

DockingStation chosen_docking_station
uint16 status              # Current status of docking process
string message             # Information message
uint16 num_retries         # Number of retries attempted
builtin_interfaces/Duration docking_time  # Docking time elapsed

Implementation Details

The docking operation utilizes Nav2's /dock_robot action to perform the actual docking. The Docking Helper node:

1. Identifies the target docking station (nearest or specific)
2. Calculates the appropriate docking pose by accounting for the offset between the robot's base_link and charging_port
3. Monitors docking progress and reports status through action feedback
4. Reports success or failure when docking completes

The docking pose is calculated by:

• Taking the docking station's pose
• Rotating it 180 degrees (to face the docking station)
• Adding an offset based on the distance between base_link and charging_port