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.

[mxgrey]

Thanks for all your patience while I sought time to review this PR.

Before getting too much into line-by-line feedback, I have some high-level methodology recommendations. First some observations:

• The current implementation leans heavily on RobotContext to track whether the current task is related to going into a zone. Shoving data into RobotContext is an anti-pattern that we've increasingly resorted to for lack of better options, but it deserves heavy scrutiny before accepting it. I think this is a case where it's not needed, and I'll explain why later.
• The current implementation contaminates GoToPlace with implementation logic that's specific to GoToZone. This suggests a leaky abstraction and needs to be scrutinized.
• The execution of a ZoneEntry or ZoneExit is currently based on the nav graph topology and on what kind of task is being executed. This is another indication of a leaky abstraction.

I believe all of the above issues can be resolved in a relatively simple way: Instead of pushing zone-related logic down into GoToPlace, you should implement GoToZone with a sequence, much like GoToPlace is. That sequence would look like this:

GoToZone
├─ GoToPlace (Entry Point)
├─ BookZoneWaypoint
├─ ReleaseReservation (Entry Point)
└─ EnterZone

Some of the effects this has:

• We no longer need to modify GoToPlace at all. It can continue to use the reservation system as normal, and redirect robots that are trying to approach the same zone entry point at the same time. This has a bonus effect of fixing the pileup problem you mentioned, so you will no longer need to implement a go-to-waitpoint capability.
• The ZoneEntry lane event doesn't need to do anything for now. Its purpose will be taken over by BookZoneWaypoint -> EnterZone in the proposed GoToZone sequence. Maybe in the future the ZoneEntry lane event might be used to check if the zone is locked out, or publish some kind of message, but for now it can simply do nothing.

[mxgrey]

I don't think this warning is needed since zones are an optional feature. There are many navigation graphs out there that predate the existence of the zone feature, and there's no harm in the fact that they don't contain zone information. A warning like this might scare users into regenerating their nav graphs when it's not really necessary.

[mxgrey]

This isn't a very robust approach to tracking whether a zone task is happening. If there's any subtle fluke that leads to the task ending without this callback being triggered (for example, I think there are some cases where kill can do this) then the is_zone_task flag could get stuck as true.

If we really need to track this, we should consider using a similar pattern to _lift_destination where the GoToZone event would hold onto a std::shared_ptr<void>. As long as the std::shared_ptr<void> has a count, then we know a zone event is happening. After the GoToZone event destructs, the count will automatically drop.

[mxgrey]

Is there any particular reason you decided to implement ZoneEntry and ZoneExit as legacy phases instead of Events? The legacy phase concept only exists so that we don't need to port all the old implementations of phases into the new task system. New task actions are meant to be implemented using the Event API.

[mxgrey]

At the community forum today we talked about reimplementing GoToZone as a dynamically generated sequence. The key to making that work can be seen here. You can break it down like this:

using StandbyPtr = rmf_task_sequence::Event::StandbyPtr;

// The UpdateFn is related to the update callback:
// https://github.com/open-rmf/rmf_task/blob/main/rmf_task_sequence/include/rmf_task_sequence/Event.hpp#L180-L185
using UpdateFn = std::function<void()>;

// A builder function that takes in an update callback and produces an Event Standby.
using MakeStandby = std::function<StandbyPtr(UpdateFn)>;

std::vector<MakeStandby> standbys;
// TODO: Add Event Standby builders for:
// - GoToPlace (entry waypoint)
// - BookZoneWaypoint
// - ReleaseReservation (Entry Point)
// - EnterZone

auto standby = rmf_task_sequence::events::Bundle::standby(
    rmf_task_sequence::events::Bundle::Type::Sequence,
    standbys, 
    state, 
    std::move(update)
);

auto execution = standby->begin([]() {}, std::move(finished));

If you want to do conditional branching, you can tweak the last line:

auto execution = standby->begin(
    []() {},
    [w = this->weak_from_this()]() {
        const auto self = w.lock();
        if (!self)
            return;

        if (self->something_failed())
            self->do_fallback();
        else
            self->do_next_thing();
    }
);

The second callback determines what happens when the sequence is finished executing. You can have that callback trigger a new sequence generated by the GoToZone event.

[kjchee]

The key to making that work can be seen here. You can break it down like this

@mxgrey, thanks for your detailed explanation & example on the implementation of the dynamically generated sequence!

Regarding the events, after a few rounds of internal discussion (together with @lttey), we're thinking of a new sequence and would appreciate your guidance:

explanation what each event does:

sequence + fallback:

We think a 'single-authority' method is cleaner:
Zone Supervisor just advise (zone wp candidate list + a cost per candidate), and the reservation_node performs the final wp selection using those cost + rmf-waypoint-availability.

This only needs 2 small changes at fleet adapter library side:

1. let the caller supply per-candidate costs into GoToPlace's one_of (similar to the cost-bias you described for next-gen) & eventually pass it to reservation_node
2. a per-event override of the fleet's use_parking_reservationat the GoToPlace event (using it to force ON for the enter zone step)

Bit of context on why this took us a while:
the main thing tripping us up was having 2 authorities in the original design (zone supervisor and reservation manager both booking the interior waypoints). We wanted to just lean on the reservation manager, but it's optional at the fleet config, so we force reservation ON for the enter-zone step. This sequence is basically us trying to get around both. It also feels like it lines up better with where next-gen is heading, since we'd be feeding a cost bias into the reservation manager to pick the final waypoint.

What are your thoughts on all of this?

[mxgrey]

What are your thoughts on all of this?

This sounds great overall, and I appreciate the diagram that you've drawn.

One detail that I think is worth pointing out: If you GoToPlace to one of the zone entry waypoints before selecting the location within the zone, you might go to an entry waypoint that does not connect to zone waypoint that is selected later. That could require the robot to wastefully back out of the entry waypoint that it originally arrived at. To prevent this you should probably constrain your inner zone waypoint to be one of the waypoints connected to the entry waypoint.

Related to that, in my experience, one_of tends to send you to the same waypoint most of the time because there will usually be one waypoint that tends to be most convenient from most starting points. I think this workflow, as specified here, will tend to create a heavy bias towards a single zone entry waypoint, and that bias will not be based on the inner zone waypoint costs that the user has set. You could consider selecting the inner zone waypoint before sending the robot to an entry waypoint to get behavior that better reflects the user's cost assignments.