Add GoToZone feature

rmf_fleet_adapter/CMakeLists.txt

@@ -40,6 +40,7 @@ set(dep_pkgs
   rmf_task
   rmf_task_sequence
   rmf_reservation_msgs
+  rmf_zone_msgs
   std_msgs
   rmf_api_msgs
   rmf_websocket
@@ -100,6 +101,7 @@ target_link_libraries(rmf_fleet_adapter
     ${rmf_ingestor_msgs_TARGETS}
     ${rmf_lift_msgs_TARGETS}
     ${rmf_reservation_msgs_TARGETS}
+    ${rmf_zone_msgs_TARGETS}
     ${rmf_task_msgs_TARGETS}
     yaml-cpp::yaml-cpp
   PRIVATE
@@ -157,6 +159,7 @@ if (BUILD_TESTING)
       ${rmf_ingestor_msgs_TARGETS}
       ${rmf_lift_msgs_TARGETS}
       ${rmf_reservation_msgs_TARGETS}
+      ${rmf_zone_msgs_TARGETS}
       ${rmf_task_msgs_TARGETS}
       rmf_websocket::rmf_websocket
       ${std_msgs_TARGETS}
@@ -257,6 +260,27 @@ target_include_directories(lift_supervisor
 
 # -----------------------------------------------------------------------------
 
+add_executable(zone_supervisor
+  src/zone_supervisor/main.cpp
+  src/zone_supervisor/Node.cpp
+)
+
+target_link_libraries(zone_supervisor
+  PRIVATE
+    rmf_fleet_adapter
+    ${rclcpp_LIBRARIES}
+    ${rmf_zone_msgs_TARGETS}
+    ${rmf_building_map_msgs_TARGETS}
+    ${rmf_fleet_msgs_TARGETS}
+)
+
+target_include_directories(zone_supervisor
+  PRIVATE
+    ${rclcpp_INCLUDE_DIRS}
+)
+
+# -----------------------------------------------------------------------------
+
 add_executable(mutex_group_supervisor
   src/mutex_group_supervisor/main.cpp
 )
@@ -512,6 +536,7 @@ install(
     mock_traffic_light
     full_control
     lift_supervisor
+    zone_supervisor
     mutex_group_supervisor
     experimental_lift_watchdog
     door_supervisor

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -86,6 +86,11 @@ const std::string ReservationAllocationTopicName =
   "rmf/reservations/allocation";
 const std::string ReservationReleaseTopicName = "rmf/reservations/release";
 
+const std::string ZoneRequestTopicName = "zone_requests";
+const std::string ZoneStateTopicName = "zone_states";
+const std::string ZoneManualReleaseTopicName = "zone_manual_release";
+const std::string ZoneBookingRevokedTopicName = "zone_booking_revoked";
+
 const std::string DynamicEventBeginTopicBase = "rmf/dynamic_event/begin";
 const std::string DynamicEventStatusTopicBase = "rmf/dynamic_event/status";
 const std::string DynamicEventActionName = "rmf/dynamic_event/command";

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

@@ -191,6 +191,19 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///   robots before triggering this callback.
   FleetUpdateHandle& consider_composed_requests(ConsiderRequest consider);
 
+  /// Allow this fleet adapter to consider zone requests.
+  ///
+  /// Pass in a nullptr to disable zone requests.
+  ///
+  /// By default, zone requests are always accepted.
+  ///
+  /// \param[in] consider
+  ///   Decide whether to accept a zone request. The description will satisfy
+  ///   the event_description__zone.json schema of rmf_fleet_adapter. The
+  ///   FleetUpdateHandle will ensure that the request is feasible for the
+  ///   robots before triggering this callback.
+  FleetUpdateHandle& consider_zone_requests(ConsiderRequest consider);
+
   /// Allow this fleet adapter to execute a PerformAction activity of specified
   /// category which may be present in sequence event.
   ///

rmf_fleet_adapter/package.xml

@@ -28,6 +28,7 @@
   <depend>rmf_fleet_msgs</depend>
   <depend>rmf_ingestor_msgs</depend>
   <depend>rmf_lift_msgs</depend>
+  <depend>rmf_zone_msgs</depend>
   <depend>rmf_task_msgs</depend>
   <depend>rmf_task_ros2</depend>
   <depend>rmf_task_sequence</depend>

rmf_fleet_adapter/schemas/event_description__zone.json

@@ -0,0 +1,48 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description__zone.json",
+  "title": "Zone Event Description",
+  "description": "Send a robot to a named zone. Optional modifiers influence zone waypoint selection.",
+  "type": "object",
+  "properties": {
+    "zone_name": {
+      "type": "string",
+      "description": "The unique name of the target zone."
+    },
+    "modifiers": {
+      "description": "Optional modifiers that influence waypoint selection within the zone. If present, at least one modifier field must be specified.",
+      "type": "object",
+      "properties": {
+        "waypoint_preference": { "$ref": "#/$defs/waypoint_preference" }
+      },
+      "minProperties": 1,
+      "additionalProperties": false
+    }
+  },
+  "required": ["zone_name"],
+  "$defs": {
+    "waypoint_preference": {
+      "type": "object",
+      "description": "Hints that influence which waypoint the Zone Supervisor assigns. All fields are optional, but at least one must be present.",
+      "properties": {
+        "group": {
+          "type": "string",
+          "description": "Group label used to filter candidate waypoints (e.g. 'left', 'right', 'A', 'B'). Groups are defined per vertex in the Traffic Editor."
+        },
+        "orientation": {
+          "type": "number",
+          "description": "Desired robot heading in radians at the assigned waypoint. Follows the same convention as the orientation field in place.json."
+        },
+        "preferred_waypoints": {
+          "type": "array",
+          "description": "Ordered list of preferred waypoints. Each item follows the place.json schema and may use either the full vertex name or the short alias shown in the Traffic Editor. The Zone Supervisor attempts these in order and falls back to priority-based selection if none are available.",
+          "items": { "$ref": "place.json" },
+          "minItems": 1,
+          "uniqueItems": true
+        }
+      },
+      "minProperties": 1,
+      "additionalProperties": false
+    }
+  }
+}

rmf_fleet_adapter/schemas/task_description__zone.json

@@ -0,0 +1,7 @@
+{
+  "$schema": "https://json-schema.org/draft/2020-12/schema",
+  "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description__zone.json",
+  "title": "Zone Task Description",
+  "description": "Send a robot to a named zone, with optional behavioral modifiers.",
+  "$ref": "event_description__zone.json"
+}

rmf_fleet_adapter/src/full_control/main.cpp

@@ -311,6 +311,8 @@ class FleetDriverRobotCommandHandle
     void execute(const LiftMove&) final {}
     void execute(const LiftDoorOpen&) final {}
     void execute(const LiftSessionEnd&) final {}
+    void execute(const ZoneEntry&) final {}
+    void execute(const ZoneExit&) final {}
 
   private:
     const std::string& _dock_name;

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp

@@ -220,6 +220,8 @@ class DuplicateDockFinder : public rmf_traffic::agv::Graph::Lane::Executor
   void execute(const LiftSessionEnd&) override {}
   void execute(const LiftMove&) override {}
   void execute(const Wait&) override {}
+  void execute(const ZoneEntry&) override {}
+  void execute(const ZoneExit&) override {}
   void execute(const Dock& dock) override
   {
     if (!visited_docks.insert(dock.dock_name()).second)
@@ -342,6 +344,15 @@ std::shared_ptr<EasyFullControl> Adapter::add_easy_fleet(
         config.fleet_name().c_str());
     }
 
+    if (task == "zone" && consider)
+    {
+      fleet_handle->consider_zone_requests(consider);
+      RCLCPP_INFO(
+        this->node()->get_logger(),
+        "Fleet [%s] is configured to perform zone tasks",
+        config.fleet_name().c_str());
+    }
+
     if (task == "clean" && consider)
     {
       fleet_handle->consider_cleaning_requests(consider);

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp

@@ -1625,6 +1625,8 @@ class DockFinder : public rmf_traffic::agv::Graph::Lane::Executor
   void execute(const LiftSessionEnd&) override {}
   void execute(const LiftMove&) override {}
   void execute(const Wait&) override {}
+  void execute(const ZoneEntry&) override {}
+  void execute(const ZoneExit&) override {}
   void execute(const Dock& dock) override
   {
     if (looking_for == dock.dock_name())

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -33,7 +33,9 @@
 #include "../tasks/Clean.hpp"
 #include "../tasks/ChargeBattery.hpp"
 #include "../tasks/Compose.hpp"
+#include "../tasks/Zone.hpp"
 #include "../events/GoToPlace.hpp"
+#include "../events/GoToZone.hpp"
 #include "../events/ResponsiveWait.hpp"
 #include "../events/PerformAction.hpp"
 #include "../events/DynamicEvent.hpp"
@@ -1453,6 +1455,8 @@ class EmergencyLaneCloser : public rmf_traffic::agv::Graph::Lane::Executor
   void execute(const LiftMove&) override {}
   void execute(const Wait&) override {}
   void execute(const Dock& /*dock*/) override {}
+  void execute(const ZoneEntry&) override {}
+  void execute(const ZoneExit&) override {}
   void execute(const LiftSessionBegin& info) override
   {
     lift = info.lift_name();
@@ -1666,6 +1670,135 @@ PlaceDeserializer make_place_deserializer(
       return {place, {}};
     };
 }
+
+ZoneDeserializer make_zone_deserializer(
+  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>>
+  planner)
+{
+  return [planner = std::move(planner)](const nlohmann::json& msg)
+    -> DeserializedZone
+    {
+      std::optional<std::string> zone_name;
+      const auto& graph = (*planner)->get_configuration().graph();
+      const auto zone_property = graph.find_known_zone(msg.get<std::string>());
+      if (!zone_property)
+      {
+        return {
+          std::nullopt,
+          {"zone name [" + msg.get<std::string>() + "] cannot be "
+            "found in the navigation graph"}
+        };
+      }
+      zone_name = zone_property->name();
+      return {zone_name, {}};
+    };
+}
+
+ZoneWaypointDeserializer make_zone_waypoint_deserializer(
+  std::shared_ptr<const std::shared_ptr<const rmf_traffic::agv::Planner>>
+  planner)
+{
+  return [planner = std::move(planner)](
+      const std::string& zone_name,
+      const nlohmann::json& msg) -> DeserializedZoneWaypoint
+    {
+      const auto& graph = (*planner)->get_configuration().graph();
+      const auto zone_props = graph.find_known_zone(zone_name);
+      if (!zone_props)
+      {
+        return {
+          std::nullopt,
+          {"zone name [" + zone_name + "] cannot be found in the navigation "
+            "graph"}
+        };
+      }
+
+      // Resolve the JSON value to either a name or a graph waypoint index.
+      std::optional<std::string> name_in;
+      std::optional<std::size_t> index_in;
+      if (msg.is_string())
+      {
+        name_in = msg.get<std::string>();
+      }
+      else if (msg.is_number_integer())
+      {
+        index_in = msg.get<std::size_t>();
+      }
+      else if (msg.is_object() && msg.contains("waypoint"))
+      {
+        const auto& wp_field = msg.at("waypoint");
+        if (wp_field.is_string())
+          name_in = wp_field.get<std::string>();
+        else if (wp_field.is_number_integer())
+          index_in = wp_field.get<std::size_t>();
+      }
+
+      if (!name_in.has_value() && !index_in.has_value())
+      {
+        return {
+          std::nullopt,
+          {"zone waypoint must be a string, integer, or "
+            "{\"waypoint\": <string|integer>}"}
+        };
+      }
+
+      if (index_in.has_value() && *index_in >= graph.num_waypoints())
+      {
+        return {
+          std::nullopt,
+          {"waypoint index [" + std::to_string(*index_in)
+            + "] exceeds the navigation graph size ["
+            + std::to_string(graph.num_waypoints()) + "]"}
+        };
+      }
+
+      for (const auto& iv : zone_props->internal_vertices())
+      {
+        if (name_in.has_value())
+        {
+          // Exact match on full RMF vertex name
+          if (iv.name() == *name_in)
+            return {iv.name(), {}};
+        }
+        else
+        {
+          const auto* wp = graph.find_waypoint(iv.name());
+          if (wp && wp->index() == *index_in)
+            return {iv.name(), {}};
+        }
+      }
+
+      // Fallback: match by traffic editor short name.
+      // Full vertex names follow {zone}#{group}#p{priority}#{zone_wp_name},
+      // e.g. "zoneA#L#p1#waypoint_A". Match by the trailing short name
+      // segment, and also require the zone prefix matches.
+      if (name_in.has_value())
+      {
+        const std::string wp_suffix = "#" + *name_in;
+        const std::string zone_prefix = zone_name + "#";
+        for (const auto& iv : zone_props->internal_vertices())
+        {
+          const auto& full = iv.name();
+          // check if full vertex name ends with the suffix
+          // and starts with the zone prefix
+          if (full.size() > wp_suffix.size()
+            && full.compare(full.size() - wp_suffix.size(),
+                wp_suffix.size(), wp_suffix) == 0
+            && full.compare(0, zone_prefix.size(), zone_prefix) == 0)
+            return {iv.name(), {}};
+        }
+      }
+
+      const std::string unknown_wp = name_in.has_value()
+        ? ("[" + *name_in + "]")
+        : ("index [" + std::to_string(*index_in) + "]");
+      return {
+        std::nullopt,
+        {"waypoint " + unknown_wp + " is not an internal vertex of zone ["
+          + zone_name + "]"}
+      };
+    };
+}
 } // anonymous namespace
 
 //==============================================================================
@@ -1682,8 +1815,11 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     *activation.phase, activation.event);
 
   events::GoToPlace::add(*activation.event);
+  events::GoToZone::add(*activation.event);
   events::PerformAction::add(*activation.event);
   deserialization.place = make_place_deserializer(planner);
+  deserialization.zone = make_zone_deserializer(planner);
+  deserialization.zone_waypoint = make_zone_waypoint_deserializer(planner);
   deserialization.add_schema(schemas::place);
 
   events::ResponsiveWait::add(*activation.event);
@@ -1716,6 +1852,9 @@ void FleetUpdateHandle::Implementation::add_standard_tasks()
     activation,
     node->clock());
 
+  tasks::add_zone(
+    deserialization);
+
   events::DynamicEvent::add(deserialization, activation.event);
 }
 
@@ -2060,6 +2199,14 @@ FleetUpdateHandle& FleetUpdateHandle::consider_composed_requests(
   *_pimpl->deserialization.consider_composed = std::move(consider);
   return *this;
 }
+
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::consider_zone_requests(
+  ConsiderRequest consider)
+{
+  *_pimpl->deserialization.consider_zone = std::move(consider);
+  return *this;
+}
 
 //==============================================================================
 void FleetUpdateHandle::close_lanes(std::vector<std::size_t> lane_indices)