Add GoToZone feature

[chart-singapore]

New feature implementation

Implemented feature

This is the main PR for GoToZone Feature. Tracked in Stage A of open-rmf/rmf#726.

Introduces GoToZone task/event (input a zone name instead of a waypoint) & a zone booking system that lets robots claim, hold, and release specific waypoints inside named zones (e.g., consultation rooms, parking zone, ward bedside parking) under the arbitration of a new standalone Zone Supervisor node. Fleet adapters interact with the zone supervisor via new rmf_zone_msgs topics, and zone entry/exit (request booking/release booking) are wired into the plan execution pipeline via nav-graph lane events.

This PR is the deep-dive for an 8-repo feature.

Merge order	Repo	PR	Scope
1	rmf_internal_msgs	pr#91	rmf_zone_msgs package (7 msgs)
1	rmf_building_map_msgs	PR#10	GraphZone, ZoneVertex, ZoneTransitionLane
1	rmf_traffic	PR#133	Zone concepts on the graph
1	rmf_traffic_editor	PR#544	Zone editing UI + YAML serialization
2	rmf_task	PR#136	GoToZone task-sequence event + ZoneGuardModel
3	rmf_ros2 (this)	this	Zone supervisor + fleet adapter integration
4	rmf_visualization	PR#91	Zone rendering in RViz
4	rmf_demos	PR#350	Chart_lab demo + dispatch_zone.py

Merge sequence: 1 --> 2 --> 3 --> 4.

---

Implementation description

A. Overview: End-to-end flow

1. Dispatch. A user submits a zone task either as a top-level task (category: "zone") or as a GoToZone event nested inside a larger task sequence. The request may carry optional modifiers.waypoint_preference hints that influence waypoint selection inside the zone:

• group — a label the user assigns to subsets of zone vertices in the Traffic Editor (e.g. left, right, center). The supervisor prefers candidates in the matching group.
• orientation — the robot's desired heading (radians) at the final selected waypoint.
• preferred_waypoints — an ordered list of candidate waypoint names; each entry may be the fully qualified vertex name (e.g. consultation#R#p1#wait_2) or the short alias (wait_2) shown in Traffic Editor / RViz.

Future stages will extend this list with sensor-driven options.

2. Deserialization. The JSON request is validated and deserialized into a GoToZone::Description by rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Zone.cpp, producing an event-sequence description for the task planner.

3. Bidding (ZoneGuardModel). During task bidding, the task planner calls ZoneGuardModel::estimate_finish for each candidate robot. If a robot's current waypoint is already one of the zone's internal waypoints, estimate_finish returns std::nullopt and that robot is excluded from bidding on this particular zone task (while remaining eligible for other tasks). This prevents awarding a zone task to a robot that is already parked inside the zone.

4. Execution entry. When a robot wins the bid, its TaskManager dispatches the task and invokes the execution logic at rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToZone.cpp. GoToZone::Active:

• Sets is_zone_task = true and records the caller's modifiers on the RobotContext.
• Performs a defensive re-check (complementing the bidding guard): if the robot is already at one of the zone's internal waypoints, execution is refused. This closes the gap for direct robot_task_request assignments that bypass bidding.
• Collects the zone's internal-vertex indices as the goal list and wraps a GoToPlace::Active built from GoToPlace::Description::make_for_one_of(goals).
• Installs a wrapped_finished callback so that on completion/cancel/kill, is_zone_task, zone_task_modifiers, and booked_zone_goal are all cleared on the RobotContext.

5. Initial planning. Inside GoToPlace, the planner selects the lowest-cost goal from the list (initially any internal vertex in the zone) and produces a plan toward it.

6. Zone entry — booking handshake. When the plan routes the robot onto a lane whose entry_event is ZoneEntry (wired by parse_graph.cpp from the nav graph's transition-lane metadata), ExecutePlan inserts a ZoneEntry phase. The phase:

• Publishes a ZoneRequest of type ENTRY on zone_requests, stamped with a fresh request_id of the form {fleet}_{robot}_{zone}_{random_hex}.
• Subscribes to zone_states (transient-local QoS) and waits for a ZoneBooking whose request_id matches exactly.
• On confirmation, calls set_booked_zone_goal(...) and set_booked_zone_waypoint(...) on the RobotContext, then invokes request_replan().

7. Replan to the assigned waypoint. On replan, GoToPlace::_find_plan() reads booked_zone_goal from the RobotContext and uses it as the planner goal in place of "any of the internal vertices". The robot now heads to the specific waypoint the supervisor assigned.

8. In-zone operation. The robot parks at the assigned waypoint. _booked_zone_waypoint remains set, holding an RAII _zone_stubbornness reference that prevents traffic negotiators from displacing the robot while it is occupying its booked slot.

9. Zone exit — release. When a subsequent task (for example, a GoToPlace to a waypoint outside the zone) routes the robot onto a lane whose exit_event is ZoneExit, ExecutePlan inserts a ZoneExit phase. The phase publishes a ZoneRequest of type EXIT, awaits release confirmation from the zone supervisor, then clears booked_zone_waypoint on the RobotContext. The stubbornness reference drops.

10. Recovery paths. Two out-of-band mechanisms handle bookings that are not released cleanly by the normal flow:

• Manual release. An operator can publish a ZoneManualRelease message to the supervisor with the affected (robot, fleet, zone) triple, then zone supervisor drops the booking and publishes a ZoneBookingRevoked message so the affected RobotContext can clean up its zone-state fields.
• Automatic stale-booking detection. The zone supervisor runs a periodic timer (default stale_booking_check_interval = 60 s) that compares each booked robot's reported position against the x/y of its booked waypoint. A booking that previously latched has_arrived and is now out of tolerance is marked as suspect. If the suspect state persists for stale_booking_grace_period (default 180 s), the supervisor revokes the booking and publishes a ZoneBookingRevoked to the affected robot.

---

B. New task type (zone) and new event type (go_to_zone)

Two JSON schemas define the external surface:

• schemas/task_description__zone.json — the top-level category: "zone" task. Dispatch with dispatch_task_request.
• schemas/event_description__zone.json — the go_to_zone event for use inside a composed task sequence.

---

C. New fleet-adapter event: events/GoToZone

The event-level handler for go_to_zone. Responsibilities:

• Wraps a GoToPlace::Active that targets all internal vertices of the named zone as a make_for_one_of goal set.
• Applies zone-specific setup on the RobotContext: sets is_zone_task = true and records the caller's zone_task_modifiers.
• Installs a wrapped_finished callback that clears task-level zone state (is_zone_task, zone_task_modifiers, booked_zone_goal) on completion / cancel / kill, preventing state leakage into the next task.
• _booked_zone_waypoint is intentionally not cleared here, it stays set until the robot physically leaves the zone (via ZoneExit or forced ZoneBookingRevoked), so that the stubbornness guard remains in force for the whole in-zone period.

---

D. New phases: ZoneEntry and ZoneExit

Implemented as legacy-style task phases (via LegacyPhaseShim), consistent with existing low-level phases such as DoorOpen and RequestLift. They are inserted by ExecutePlan when the robot traverses a lane whose entry_event / exit_event carries a ZoneEntry or ZoneExit — wired at nav-graph parse time in parse_graph.cpp.

• ZoneEntry publishes an ENTRY ZoneRequest to the zone supervisor, waits for a matching ZoneBooking, stores the assigned waypoint and planner goal on the RobotContext, then calls request_replan() so that the outer GoToPlace (inside events/GoToZone) routes the robot to the granted waypoint.
• ZoneExit publishes an EXIT ZoneRequest to the zone supervisor, waits for release confirmation, then clears the booking on the RobotContext.

Open question for reviewers. These phases follow the older LegacyPhaseShim pattern. We are happy to migrate them to the newer event-based framework used by LockMutexGroup if Open-RMF maintainers prefer that direction.

---

E. New node: zone_supervisor

A standalone ROS 2 node that owns zone-booking authority. All callbacks share a single MutuallyExclusive callback group, so _zone_log, _zones, and the derived indexes are accessed without additional locks.

Responsibilities:

• Maintains the zone registry from fleet-published nav graphs (zones are assumed globally unique across fleets).
• Processes ZoneRequest (ENTRY / EXIT) and allocates waypoints through a 4-stage algorithm:
  1. Availability — drop waypoints already booked by another robot.
  2. Group — if group_hint is set and matches any candidate, narrow to that subset, otherwise keep all candidates.
  3. Preferred — if preferred_waypoints is set, return the first one present in the current candidate set.
  4. Priority — otherwise return the candidate with the lowest priority value.
• Publishes ZoneState snapshots on state change (event-driven).
• Runs periodic stale-booking detection using fleet-state position and an has_arrived latch that prevents false revocations while the robot is still in transit.
• Handles ZoneManualRelease for operator-initiated recovery (e.g. a broken-down robot that needs to be manually pulled out of a zone).

Tunable parameters:

• stale_booking_check_interval (default 60 s)
• stale_booking_distance_threshold (default 1.0 m)
• stale_booking_grace_period (default 180 s)

---

F. RobotContext new fields

Field	Type	Purpose
_is_zone_task	bool	Gate used by ExecutePlan to decide whether a ZoneEntry phase should be inserted when the plan crosses a zone entry lane.
_zone_task_modifiers	ZoneTaskModifiers struct	Modifier hints (group_hint, orientation_hint, preferred_waypoints) carried from the task description into the ZoneRequest payload.
_booked_zone_goal	std::optional<Plan::Goal>	Planner-goal override read by GoToPlace::_find_plan() when non-empty. Cleared when the GoToZone task completes / cancels / kills.
_booked_zone_waypoint	std::string	Booking identity. Held until the robot leaves the zone (via ZoneExit) or the booking is revoked. Also gates _zone_stubbornness.
_zone_stubbornness	std::shared_ptr<void>	Acquired when _booked_zone_waypoint is set, released when cleared.

---

G. Known limitations

1. No transit-zone support. If a user dispatches a plain GoToPlace whose planned path happens to pass through a zone, no ZoneEntry / ZoneExit events fire — zones are only honored when the task / event is explicitly GoToZone. Transit-zone handling is deferred to a later stage (lift cabin zone and lift waiting zone).

2. ZoneExit only fires on zone exit-lane traversal. If a subsequent plan starts inside the zone but the startset resolves to a waypoint outside the zone (an edge case where the robot's startset has already merged onto an external waypoint), the exit lane is never traversed and the booking is not released through the normal path. But the zone supervisor's periodic stale-booking check (default every 60 s) will eventually remove such bookings.

3. Direct GoToPlace to a zone internal vertex is soft-blocked, not hard-blocked. Zone internal vertices are exposed in RViz / Traffic Editor only by their "short name" (e.g. wait_2), the fully qualified form (consultation#R#p1#wait_2) that GoToPlace would need to resolve is intentionally hidden. This discourages, but does not prevent, a user from dispatching GoToPlace directly to a zone waypoint — which would bypass the supervisor and leave the zone in an inconsistent state.

GenAI Use

We follow OSRA's policy on GenAI tools

• I used a GenAI tool in this PR. ( Claude Code Opus 4.6 for code review & edge cases analysis like task cancelled during zoneentry/zoneexit, booked zone wp but haven't entered zone and etc... )
• I did not use GenAI

[kjchee]

Hi @mxgrey, noting down our planned responses to your comments from yesterday's Interoperability Meeting.

Comment 1: Remove the same-vertex entry/exit lane restriction

• Remove the restriction (replace the hard refusal with a warning, as you suggested).
• Users could implement their own mutex group outside the zone to control access to the entry lane.
• Note: after a new round of internal discussion, we found that the original reason for the rule was robot pile-up at the zone entry when internal vertices are fully occupied. We'll solve that root cause separately. For Stage A, users proceed without the restriction. In a future stage, we plan to add a "go-to-waitpoint" mechanism that redirects to a wait point when the zone is full at task dispatch.

Comment 2: Remove the zone guard model

• Remove the guard. A robot already in the zone can be picked for a new GoToZone targeting the same zone. The assigned waypoint depends on the new modifier the user provides.

Comment 3: Opt-in / opt-out of zone stubbornness

• Add the option at the task level (optional parameter on GoToZone), defaulting to on.