[Feature request]: Proposal For Docking

This is a design proposal, so start by reading the Path Server, Plan Executor and Fleet Adapter sections and the TargetNode/TargetOrientation sequence diagram, not by editing code. The issue names site.json, DockRegistry, Progress::MODE_ACTION and several linked PRs (#64, #67-#74, #77, #78) that should be read first. Done means the maintainers agree on the docking design, and only then can the work be split into smaller pieces.

Before proceeding, is there an existing issue or discussion for this?

• I have done a search for similar issues and discussions.

Proposed Implementation For Docking in NGP

Docking is the process via which an AMR approaches a waypoint and aligns itself. During this time the AMR switches from localizing against the global frame and uses a local frame of reference to navigate to its own end point. It may need to reserve more space than just what is allocated by default as there may be multiple attempts at docking. Docking in itself is a robot specific
maneuver and each robot may have its own mechanism to perform docking.

Prior Art

Nav2

Docking is already implemented within Nav2 via the docking service. Robot vendors can implement their own docking service via this API. There is also a dock database which stores poses of known docks and the type of docking server that should be used.

How docking is triggered

How docking is triggered is via the target_node field in the destination message.
In the event the target_node is populated,
and the site.json marks the target node as a Dock node, the robot should
perform a dock sequence.

sequenceDiagram
  autonumber
  actor User
  participant DestServer as DestinationServer
  participant PathServer as PathServer
  participant PlanExec as PlanExecutor
  participant FleetAdapter as FleetAdapter
  participant Nav2 as InnerNav2

  User->>DestServer: DestinationGoal with TargetNode dock_north_1
  DestServer->>PathServer: Destination with TargetNode dock_north_1
  activate PathServer
  Note over PathServer: Lookup dock_north_1 in DockRegistry (site.json), stamp TargetOrientation on staging wp k-1 and forward TargetNode on dock wp k
  PathServer->>PlanExec: Plan (wp 0..k-1 -> staging wp k-1 -> dock wp k)
  deactivate PathServer

  activate PlanExec
  Note over PlanExec: Gate PlanRelease at staging wp k-1 (has TargetOrientation)
  PlanExec->>FleetAdapter: SafeZone (target_waypoint k-1, pose -1.0, 1.0, yaw pi/2)
  FleetAdapter->>Nav2: NavigateToPose (-1.0, 1.0, yaw pi/2)
  Nav2-->>FleetAdapter: NavigateToPose Succeeded
  FleetAdapter->>PlanExec: Progress (MODE_NAVIGATING, reached_waypoint k-1)
  FleetAdapter->>PathServer: Progress (MODE_NAVIGATING, reached_waypoint k-1)

  Note over PlanExec: Staging wp k-1 reached and blockers satisfied, release dock wp k
  PlanExec->>FleetAdapter: PlanRelease (waypoint_id k) and SafeZone (target_waypoint k, node dock_north_1)
  Note over FleetAdapter: Resolve TargetNode dock_north_1 in DockRegistry -> DockRobot, hold reached_waypoint k-1 and enter MODE_ACTION
  FleetAdapter->>PlanExec: Progress (MODE_ACTION, reached_waypoint k-1, target_waypoint k)
  FleetAdapter->>PathServer: Progress (MODE_ACTION, reached_waypoint k-1, target_waypoint k)
  Note over PathServer: is_executing_action is true (MODE_ACTION and dock TargetNode), exclude robot from replans and mark staging-to-dock segment occupied
  FleetAdapter->>Nav2: DockRobot (dock_pose -1.0, 2.0, yaw pi/2)
  Nav2-->>FleetAdapter: DockRobot Succeeded
  FleetAdapter->>PlanExec: Progress (MODE_IDLE, reached_waypoint k, target_waypoint k)
  FleetAdapter->>PathServer: Progress (MODE_IDLE, reached_waypoint k, target_waypoint k)
  deactivate PlanExec

An example of dispatching a destination goal with a docking sequence is given in this Python snippet:

import math
from rmf_prototype_msgs.msg import (
    DestinationConstraints,
    DestinationGoal,
    GraphElementKey,
    Region,
    TargetNode,
    TargetOrientation,
    TargetRegion,
)
from unique_identifier_msgs.msg import UUID


def make_dock_destination_goal(
    x: float,
    y: float,
    yaw: float,
    dock_node_name: str,
    session_seed: int = 1,
) -> DestinationGoal:
    """Create a DestinationGoal targeting a dock node defined in the RMF site file."""
    goal = DestinationGoal()
    goal.session = UUID(uuid=[session_seed & 0xFF] + [0] * 15)

    # 1. Specify the spatial target region and docking heading
    region = Region()
    region.hint = Region.HINT_POINT
    region.points = [float(x), float(y)]

    orientation = TargetOrientation()
    orientation.orientation_radians = float(yaw)
    orientation.spread_radians = 0.0
    orientation.tolerance_radians = 0.0

    target_region = TargetRegion()
    target_region.tolerance = 0.25
    target_region.region = region
    target_region.orientations = [orientation]

    # 2. Attach the TargetNode matching the dock location name in the .site.json
    target_node = TargetNode()
    target_node.key = GraphElementKey(name=[dock_node_name])
    target_node.orientations = [orientation]

    constraints = DestinationConstraints()
    constraints.regions = [target_region]
    constraints.nodes = [target_node]

    goal.one_of = [constraints]
    return goal


# Example: Send robot0 to dock into 'dock_north_1' at (-1.0, 2.0) facing North (pi/2)
dock_goal = make_dock_destination_goal(
    x=-1.0,
    y=2.0,
    yaw=math.pi / 2.0,
    dock_node_name='dock_north_1',
    session_seed=1,
)
goal_publisher.publish(dock_goal)

Why this isn't just a crossflow action

As established, docking is a robot-specific maneuver. In a single deployment with
multiple robots, the exact docking procedure must be delegated to each robot's fleet adapter / onboard controller (e.g. opennav_docking's DockRobot and UndockRobot actions in Nav2) rather than executed as a generic task-level Crossflow workflow outside the traffic system. In addition, there are tight interactions with spatio-temporal allocation and the multi-agent path planner that require the planner, executor, and fleet adapter to coordinate throughout the maneuver:

1. Staging Pose & Pre-Dock Orientation Gating:

   • Before a robot can enter a dock alcove using local sensor/dock feedback, it must first navigate to the dock's staging (entry) waypoint and align to the required approach heading (TargetOrientation).
   • If the planner or executor released the dock waypoint immediately as a normal free-space trajectory segment, the robot would attempt to cut the corner into the dock alcove using global localization (NavigateToPose) or report the staging waypoint as cleared before finishing its in-place rotation.
   • Instead, rmf_path_server stamps the dock approach orientation onto the pre-dock staging waypoint (wp[k-1]), and rmf_plan_executor gates PlanRelease at wp[k-1] until ProgressTracker confirms both the staging position and TargetOrientation are satisfied.

2. Blocking Replanning & Reserving Both Staging and Dock Space (MODE_ACTION):

   • Once a robot begins its final docking approach (wp[k-1] -> wp[k]) or undocking departure (wp[0] -> wp[1]), it transitions to closed-loop local control (DockRobot / UndockRobot) and cannot be rerouted mid-maneuver by the global MAPF planner.
   • If another robot requests a new plan while the first robot is mid-docking, rmf_path_server must:
     1. Exclude the docking/undocking robot from MAPF replanning (is_executing_action() == true, detected via Progress::MODE_ACTION or by matching the active waypoint's TargetNode in DockRegistry).
     2. Mark the entire active action segment ([wp[k-1], wp[k]]) as occupied in the planning grid so other robots route around the alcove or wait, while preserving cross-plan TrafficDependency blockers against the docking robot's active plan.
   • Simultaneously, ProgressTracker holds the docking robot's reached_waypoint at the pre-dock staging waypoint (k-1) until DockRobot / UndockRobot completes, ensuring rmf_plan_executor's spatial allocation (SafeZone costmap) keeps both the staging cell and the dock cell reserved for the robot and prevents trailing robots from being released into the staging area prematurely.

⚠️ Some caveats to note: This method using TargetNode does require the fleet-adapter to have
a special case for docking and be aware of what consists of a docking node.

Implementation Details

Path Server (PR #68, PR #71, PR #72)

When a Destination with a TargetNode is received it is mapped back to the relevant site.json file (PR #68, PR #71).
If it is a docking node, the path server will route a path to it using standard mapf techniques. The Plan.msg would have the TargetNode as its final waypoint and the mapf algorithm should route the robot to its pre-dock pose first.

To prevent replanning the PathServer also needs to maintain a PlanRegistry and the current state of the robot (this was already needed to be able to ad-hoc re-plan for robots). When a robot is performing a dock, the entire horizon between the pre-dock and the post-dock pose appears as a static obstacle to all other robots[^1] (PR #71). Traffic dependencies are calculated across multiple mapf sessions (PR #71).

Plan Executor (PR #64, PR #71, PR #72)

The plan executor also needs to maintain access to the PlanRegistry and rely on cross-MAPF session traffic dependencies (PR #71). It should be able to reconstruct the current relevant set of plans via a simple DFS (PR #71). For safe zones we need to use this cross-plan dependency to reserve space (PR #71, PR #72), while gating intermediate waypoints that carry TargetOrientation or TargetNode constraints (PR #64, PR #72).

Fleet Adapter (PR #67, PR #72, PR #73, PR #74, PR #77, PR #78)

The fleet adapter should match TargetNode to known docks (PR #68, PR #78). In the event that the currently released waypoints involve a dock/predock pair the adapter should hold its progress at the pre-dock waypoint and report Progress::MODE_ACTION until it reaches its final destination (PR #67, PR #69, PR #72, PR #78).

[^1]: This is something that may need to change. Ideally we would do two passes of MAPF. First with the docking robot's dock space as a static obstacle, and second with the docking robot at its final destination if dome robots cannot make it because theyre blocked.

---

Necessary Changes Across the NGP Stack

This section is llm generated changelog.

1. Site Configuration & DockRegistry (rmf_path_server, rmf_mock_robot_sim, & rmf_nav2_traffic)

• PRs: PR #68, PR #71, PR #74, PR #78
• Define docks in the RMF site file (*.site.json) as guided navigation locations connected by lanes with "dock": {"dock_name": "...", "duration": ...} properties (PR #68).
• Load DockRegistry from the site_file ROS parameter in rmf_mock_robot_sim (PR #68), rmf_path_server (PR #71), and rmf_nav2_traffic (PR #78) to resolve:
  • Dock location name / graph node key, world coordinates (x, y), entry/staging coordinates (entry_x, entry_y), and approach heading entry_yaw.

2. Message Definitions (rmf_prototype_msgs)

• PRs: PR #64, PR #69, PR #78
• Progress.msg (PR #69, PR #78):
  • Include pose (geometry_msgs/PoseStamped), mode (MODE_IDLE, MODE_NAVIGATING, MODE_ACTION, MODE_WAITING, MODE_ERROR) (PR #69), and safe_zone_id (SafeZoneId[<=1]) (PR #78).
• Waypoint.msg / DestinationConstraints.msg (PR #64):
  • Carry arrival_constraints (TargetRegion with TargetOrientation + TargetNode) on Plan waypoints and SafeZone.incremental_target so the graph node (TargetNode) is the single source of truth for docking targets.

3. Path Server (rmf_path_server)

• PRs: PR #64, PR #71, PR #72
• Dock Waypoint & Orientation Annotation (PlanRegistry::build_session_plans) (PR #64, PR #71, PR #72):
  • When a plan's destination includes a TargetNode matching a known dock in DockRegistry:
    • Stamp TargetOrientation(entry_yaw) onto the pre-dock staging waypoint (wp[k-1]) (PR #64, PR #72).
    • Preserve the destination's TargetNode on the dock waypoint (wp[k].arrival_constraints.nodes) (PR #64, PR #71).
• Action-Aware Replan Protection (PlanServer::take_snapshot & PlanRegistry::is_executing_action) (PR #71):
  • Check whether any robot is currently executing a dock/undock action via:
    1. progress.mode == Progress::MODE_ACTION (PR #69, PR #71), or
    2. a TargetNode on the active target waypoint (wp.arrival_constraints.nodes) or start waypoint that matches a dock in DockRegistry (PR #71).
  • Exclude committed robots from MAPF replanning, mark their occupied segment (occupied_path_of) in the planning Map, and compute cross-plan TrafficDependency blockers against their in-progress plans via DFS (get_mapf_snapshot) (PR #71).

4. Plan Executor (rmf_plan_executor)

• PRs: PR #64, PR #71, PR #72
• Constraint Gating (handle_progress) (PR #64, PR #72):
  • Stop advancing released_wp_idx at any intermediate waypoint that carries arrival_constraints.nodes or a constrained TargetOrientation until the robot reaches that waypoint.
• Constraint Forwarding (SafeZone) (PR #64, PR #72):
  • Forward the target waypoint's TargetOrientation, tolerance, and TargetNodes in SafeZone.incremental_target so the fleet adapter knows both the spatial target and the target graph node.
• Cross-Plan Blocker, Subdivision & Target Occupancy Holding (PR #71, PR #72):
  • Reconstruct multi-session MapfResult via DFS in PlanRegistry (PR #71), support grid_subdivision and blur_radius for corridor/alcove SafeZone allocation, and hold PlanRelease when a peer robot is on an unaligned plan version/session (PR #72).

5. Fleet Adapter (rmf_mock_robot_sim & rmf_nav2_traffic)

• PRs: PR #67, PR #68, PR #69, PR #72, PR #73, PR #74, PR #77, PR #78
• Mock Robot Simulator Progress & Docking (rmf_mock_robot_sim) (PR #67, PR #68, PR #69):
  • Accurately track and publish Progress across waypoints (PR #67), match TargetNode against site_file docks to simulate dock/undock durations (PR #68), and report Progress::MODE_ACTION during dock/undock transitions (PR #69).
• Action Classification (ProgressTracker::resolve_dock_transition) (PR #78):
  • When a SafeZone updates ProgressTracker, inspect the target TargetNodes (and fallback Plan waypoint nodes) against DockRegistry:
    • Approaching a known dock -> InnerNavigationAction::DockRobot { navigate_to_staging_pose }
    • Departing a known dock -> InnerNavigationAction::UndockRobot
    • Standard waypoint -> InnerNavigationAction::NavigateToPose
• Immediate MODE_ACTION & Pre-Waypoint Progress Hold (ProgressTracker) (PR #72, PR #78):
  • For DockRobot and UndockRobot, set hold_at_pre_waypoint = true and immediately publish Progress with mode = Progress::MODE_ACTION and reached_waypoint = last_waypoint as soon as the SafeZone is received (or when a stationary NavigateToPose at the staging pose upgrades in-place to DockRobot) (PR #78).
  • For NavigateToPose with a constrained TargetOrientation, hold reached_waypoint at last_waypoint (hold_final_waypoint_for_yaw = true) until the robot's heading satisfies target_yaw_tolerance (PR #72, PR #78).
• Sequence-Numbered Costmap Handshake & Branching Crossflow Dispatch (InnerNavigationServices) (PR #73, PR #74, PR #77, PR #78):
  • After the sequence-numbered costmap handshake (CostmapUpdateState, PR #73) confirms the robot's SafeZone costmap is applied in Nav2, dispatch InnerNavigationTarget through the one-shot branching Crossflow workflow (PR #77, PR #78):
    • preempt_and_prepare_goal -> forks into execute_dock_robot (/inner/dock_robot), execute_undock_robot (/inner/undock_robot), or execute_navigate_to_pose (/inner/navigate_to_pose) -> reconcile_goal_outcome.
  • When reconcile_goal_outcome receives GoalOutcomeStatus::Succeeded, release the action hold (mark_action_completed / mark_target_reached) and publish Progress with mode = Progress::MODE_IDLE (or MODE_NAVIGATING) and reached_waypoint = target_waypoint (PR #78).
  • Verified end-to-end in Gazebo + Nav2 with opennav_docking and corridor swap/docking scenarios (PR #74, PR #78).