Adds a simple parking spot management system.

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -76,6 +76,14 @@ const std::string MutexGroupStatesTopicName = "mutex_group_states";
 const std::string MutexGroupManualReleaseTopicName =
   "mutex_group_manual_release";
 
+const std::string ReservationRequestTopicName = "rmf/reservations/request";
+const std::string ReservationResponseTopicName = "rmf/reservations/tickets";
+const std::string ReservationClaimTopicName = "rmf/reservations/claim";
+const std::string ReservationAllocationTopicName =
+  "rmf/reservations/allocation";
+const std::string ReservationReleaseTopicName = "rmf/reservations/release";
+const std::string ReservationCancelTopicName = "rmf/reservations/cancel";
+
 const uint64_t Unclaimed = (uint64_t)(-1);
 
 } // namespace rmf_fleet_adapter

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp

@@ -776,6 +776,13 @@ class EasyFullControl::FleetConfiguration
   /// Should robots in this fleet have responsive wait enabled by default?
   bool default_responsive_wait() const;
 
+  /// Should robots use the parking reservation system.
+  bool using_parking_reservation_system() const;
+
+  /// Set whether this fleet uses the parking reservation system.
+  void use_parking_reservation_system(
+    const bool use);
+
   /// Set whether robots in this fleet should have responsive wait enabled by
   /// default.
   void set_default_responsive_wait(bool enable);

rmf_fleet_adapter/package.xml

@@ -21,6 +21,7 @@
   <depend>rmf_api_msgs</depend>
   <depend>rmf_battery</depend>
   <depend>rmf_building_map_msgs</depend>
+  <depend>rmf_reservation_msgs</depend>
   <depend>rmf_dispenser_msgs</depend>
   <depend>rmf_door_msgs</depend>
   <depend>rmf_fleet_msgs</depend>

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp

@@ -383,7 +383,8 @@ std::shared_ptr<EasyFullControl> Adapter::add_easy_fleet(
     config.default_responsive_wait(),
     config.default_max_merge_waypoint_distance(),
     config.default_max_merge_lane_distance(),
-    config.default_min_lane_length());
+    config.default_min_lane_length(),
+    config.using_parking_reservation_system());
 }
 
 //==============================================================================

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp

@@ -2007,6 +2007,7 @@ class EasyFullControl::FleetConfiguration::Implementation
   double default_min_lane_length;
   std::unordered_map<std::string, std::string> lift_emergency_levels;
   std::unordered_set<std::size_t> strict_lanes;
+  bool use_parking_reservation;
 };
 
 //==============================================================================
@@ -2063,7 +2064,8 @@ EasyFullControl::FleetConfiguration::FleetConfiguration(
         std::move(default_max_merge_lane_distance),
         std::move(default_min_lane_length),
         {},
-        {}
+        {},
+        false // Parking reservation system
       }))
 {
   // Do nothing
@@ -2474,6 +2476,14 @@ EasyFullControl::FleetConfiguration::from_config_files(
     default_responsive_wait = default_responsive_wait_yaml.as<bool>();
   }
 
+  bool use_simple_parking_reservation_system = false;
+  const YAML::Node& parking_reservation = rmf_fleet["use_parking_reservations"];
+  if (parking_reservation)
+  {
+    use_simple_parking_reservation_system =
+      parking_reservation.as<bool>();
+  }
+
   double default_max_merge_waypoint_distance = 1e-3;
   const YAML::Node& default_max_merge_waypoint_distance_yaml =
     rmf_fleet["max_merge_waypoint_distance"];
@@ -2743,6 +2753,7 @@ EasyFullControl::FleetConfiguration::from_config_files(
     default_min_lane_length);
   config.change_lift_emergency_levels() = lift_emergency_levels;
   config.set_retreat_to_charger_interval(retreat_to_charger_interval);
+  config.use_parking_reservation_system(use_simple_parking_reservation_system);
   config.change_strict_lanes() = std::move(strict_lanes);
   return config;
 }
@@ -3056,6 +3067,20 @@ bool EasyFullControl::FleetConfiguration::default_responsive_wait() const
   return _pimpl->default_responsive_wait;
 }
 
+//==============================================================================
+bool EasyFullControl::FleetConfiguration::using_parking_reservation_system()
+  const
+{
+  return _pimpl->use_parking_reservation;
+}
+
+//==============================================================================
+void EasyFullControl::FleetConfiguration::use_parking_reservation_system(
+  const bool use)
+{
+  _pimpl->use_parking_reservation = use;
+}
+
 //==============================================================================
 void EasyFullControl::FleetConfiguration::set_default_responsive_wait(
   bool enable)
@@ -3362,6 +3387,7 @@ auto EasyFullControl::add_robot(
       localization = std::move(localization),
       nav_params = robot_nav_params,
       enable_responsive_wait,
+      use_parking_reservation = _pimpl->use_parking_reservation,
       finishing_request
     ](const RobotUpdateHandlePtr& updater)
     {
@@ -3382,6 +3408,7 @@ auto EasyFullControl::add_robot(
           context,
           nav_params,
           enable_responsive_wait,
+          use_parking_reservation,
           finishing_request
         ](const auto&)
         {
@@ -3403,6 +3430,8 @@ auto EasyFullControl::add_robot(
             context->robot_finishing_request(true);
           }
 
+          context->_set_parking_spot_manager(use_parking_reservation);
+
           RCLCPP_INFO(
             node->get_logger(),
             "Successfully added robot [%s] to the fleet [%s].",

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp

@@ -119,6 +119,30 @@ std::shared_ptr<Node> Node::make(
     node->create_observable<MutexGroupStates>(
     MutexGroupStatesTopicName, transient_local_qos);
 
+  node->_reservation_request_pub =
+    node->create_publisher<ReservationRequest>(
+    ReservationRequestTopicName, transient_local_qos);
+
+  node->_reservation_ticket_obs =
+    node->create_observable<ReservationTicket>(
+    ReservationResponseTopicName, transient_local_qos);
+
+  node->_reservation_claim_pub =
+    node->create_publisher<ReservationClaim>(
+    ReservationClaimTopicName, transient_local_qos);
+
+  node->_reservation_alloc_obs =
+    node->create_observable<ReservationAllocation>(
+    ReservationAllocationTopicName, transient_local_qos);
+
+  node->_reservation_release_pub =
+    node->create_publisher<ReservationRelease>(
+    ReservationReleaseTopicName, transient_local_qos);
+
+  node->_reservation_cancel_pub =
+    node->create_publisher<ReservationCancel>(
+    ReservationCancelTopicName, transient_local_qos);
+
   return node;
 }
 
@@ -260,5 +284,40 @@ auto Node::mutex_group_states() const -> const MutexGroupStatesObs&
   return _mutex_group_states_obs->observe();
 }
 
+//==============================================================================
+auto Node::location_requester() const -> const ReservationRequestPub&
+{
+  return _reservation_request_pub;
+}
+
+//==============================================================================
+auto Node::claim_location_ticket() const -> const ReservationClaimPub&
+{
+  return _reservation_claim_pub;
+}
+
+//==============================================================================
+auto Node::location_ticket_obs() const -> const ReservationTicketObs&
+{
+  return _reservation_ticket_obs->observe();
+}
+
+//==============================================================================
+auto Node::allocated_claims_obs() const -> const ReservationAllocationObs&
+{
+  return _reservation_alloc_obs->observe();
+}
+
+//==============================================================================
+auto Node::release_location() const -> const ReservationReleasePub&
+{
+  return _reservation_release_pub;
+}
+
+//==============================================================================
+auto Node::cancel_reservation() const -> const ReservationCancelPub&
+{
+  return _reservation_cancel_pub;
+}
 } // namespace agv
 } // namespace rmf_fleet_adapter

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp

@@ -18,6 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP
 #define SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP
 
+
 #include <rmf_rxcpp/Transport.hpp>
 
 #include <rmf_dispenser_msgs/msg/dispenser_request.hpp>
@@ -32,6 +33,13 @@
 #include <rmf_lift_msgs/msg/lift_request.hpp>
 #include <rmf_lift_msgs/msg/lift_state.hpp>
 #include <rmf_task_msgs/msg/task_summary.hpp>
+#include <rmf_reservation_msgs/msg/claim_request.hpp>
+#include <rmf_reservation_msgs/msg/flexible_time_request.hpp>
+#include <rmf_reservation_msgs/msg/free_parking_spots.hpp>
+#include <rmf_reservation_msgs/msg/release_request.hpp>
+#include <rmf_reservation_msgs/msg/reservation_allocation.hpp>
+#include <rmf_reservation_msgs/msg/ticket.hpp>
+
 #include <std_msgs/msg/bool.hpp>
 
 #include <rmf_fleet_msgs/msg/fleet_state.hpp>
@@ -135,6 +143,35 @@ class Node : public rmf_rxcpp::Transport
   using MutexGroupStatesObs = rxcpp::observable<MutexGroupStates::SharedPtr>;
   const MutexGroupStatesObs& mutex_group_states() const;
 
+  using ReservationRequest = rmf_reservation_msgs::msg::FlexibleTimeRequest;
+  using ReservationRequestPub =
+    rclcpp::Publisher<ReservationRequest>::SharedPtr;
+  const ReservationRequestPub& location_requester() const;
+
+  using ReservationTicket = rmf_reservation_msgs::msg::Ticket;
+  using ReservationTicketObs = rxcpp::observable<ReservationTicket::SharedPtr>;
+  const ReservationTicketObs& location_ticket_obs() const;
+
+  using ReservationClaim = rmf_reservation_msgs::msg::ClaimRequest;
+  using ReservationClaimPub = rclcpp::Publisher<ReservationClaim>::SharedPtr;
+  const ReservationClaimPub& claim_location_ticket() const;
+
+  using ReservationAllocation = rmf_reservation_msgs::msg::ReservationAllocation;
+  using ReservationAllocationObs =
+    rxcpp::observable<ReservationAllocation::SharedPtr>;
+  const ReservationAllocationObs& allocated_claims_obs() const;
+
+  using ReservationRelease = rmf_reservation_msgs::msg::ReleaseRequest;
+  using ReservationReleasePub =
+    rclcpp::Publisher<ReservationRelease>::SharedPtr;
+  const ReservationReleasePub& release_location() const;
+
+  using ReservationCancel = rmf_reservation_msgs::msg::ReleaseRequest;
+  using ReservationCancelPub =
+    rclcpp::Publisher<ReservationCancel>::SharedPtr;
+  const ReservationCancelPub& cancel_reservation() const;
+
+
   template<typename DurationRepT, typename DurationT, typename CallbackT>
   rclcpp::TimerBase::SharedPtr try_create_wall_timer(
     std::chrono::duration<DurationRepT, DurationT> period,
@@ -195,6 +232,12 @@ class Node : public rmf_rxcpp::Transport
   MutexGroupRequestPub _mutex_group_request_pub;
   Bridge<MutexGroupRequest> _mutex_group_request_obs;
   Bridge<MutexGroupStates> _mutex_group_states_obs;
+  ReservationRequestPub _reservation_request_pub;
+  Bridge<ReservationTicket> _reservation_ticket_obs;
+  ReservationClaimPub _reservation_claim_pub;
+  Bridge<ReservationAllocation> _reservation_alloc_obs;
+  ReservationReleasePub _reservation_release_pub;
+  ReservationCancelPub _reservation_cancel_pub;
 };
 
 } // namespace agv

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp

@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "ReservationManager.hpp"
+#include "RobotContext.hpp"
+
+using namespace rmf_fleet_adapter::agv;
+
+//==============================================================================
+void ReservationManager::replace_ticket(
+  const rmf_reservation_msgs::msg::ReservationAllocation new_allocation)
+{
+  if (has_ticket())
+  {
+    if (new_allocation.ticket.ticket_id != _allocation->ticket.ticket_id)
+    {
+      RCLCPP_INFO(
+        _context->node()->get_logger(),
+        "Releasing waypoint for ticket %lu as new ticket has become available",
+        _allocation->ticket.ticket_id);
+      rmf_reservation_msgs::msg::ReleaseRequest msg;
+      msg.ticket = _allocation->ticket;
+      _context->node()->release_location()->publish(msg);
+    }
+  }
+  _allocation = new_allocation;
+}
+
+//==============================================================================
+void ReservationManager::cancel()
+{
+  if (has_ticket())
+    return;
+
+  RCLCPP_INFO(
+    _context->node()->get_logger(),
+    "Cancelling ticket %lu",
+    _allocation->ticket.ticket_id);
+  rmf_reservation_msgs::msg::ReleaseRequest msg;
+  msg.ticket = _allocation->ticket;
+  _context->node()->cancel_reservation()->publish(msg);
+  _allocation = std::nullopt;
+}
+
+//==============================================================================
+std::string ReservationManager::get_reserved_location() const
+{
+if (has_ticket())
+    return _allocation->resource;
+
+return "";
+}
+
+//==============================================================================
+bool ReservationManager::has_ticket() const
+{
+return _allocation.has_value();
+}