Adds a simple parking spot management system. (#325)

Signed-off-by: Arjo Chakravarty <[email protected]>

rmf_fleet_adapter/CMakeLists.txt

@@ -38,6 +38,7 @@ set(dep_pkgs
   rmf_battery
   rmf_task
   rmf_task_sequence
+  rmf_reservation_msgs
   std_msgs
   rmf_api_msgs
   rmf_websocket
@@ -88,6 +89,7 @@ ament_target_dependencies(rmf_fleet_adapter
     rmf_dispenser_msgs
     rmf_ingestor_msgs
     rmf_building_map_msgs
+    rmf_reservation_msgs
     rclcpp
 )
 
@@ -115,6 +117,7 @@ target_include_directories(rmf_fleet_adapter
     ${rmf_dispenser_msgs_INCLUDE_DIRS}
     ${rmf_ingestor_msgs_INCLUDE_DIRS}
     ${rmf_api_msgs_INCLUDE_DIRS}
+    ${rmf_reservation_msgs_INCLUDE_DIRS}
     ${rmf_websocket_INCLUDE_DIR}
     ${rmf_traffic_ros2_INCLUDE_DIRS}
     ${rmf_building_map_msgs_INCLUDE_DIRS}
@@ -152,6 +155,7 @@ if (BUILD_TESTING)
       std_msgs
       rmf_building_map_msgs
       rmf_websocket
+      rmf_reservation_msgs
   )
   target_include_directories(test_rmf_fleet_adapter
     PRIVATE

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -80,6 +80,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/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp

@@ -99,6 +99,14 @@ class RobotUpdateHandle
   /// for it.
   void update_position(rmf_traffic::agv::Plan::StartSet position);
 
+  /// Set whether this robot uses the parking reservation system. By default this
+  /// is false in order to keep the system behavior backwards compatible, but it
+  /// is recommended that you turn this on.
+  ///
+  /// If you are using the EasyFullControl API then you can set this in your
+  /// fleet configuration.
+  RobotUpdateHandle& use_parking_reservation_system(bool use);
+
   /// Set the waypoint where the charger for this robot is located.
   /// If not specified, the nearest waypoint in the graph with the is_charger()
   /// property will be assumed as the charger for this robot.

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