Adds a simple parking spot management system.

This commit adds a stupidly simple parking spot system. It ensures the
next location a robot goes to is not occupied. It also publishes
currently free parking spots. When combined with #308 it allows a robot
to move to the nearest free spot instead of the nearest spot only.

note: This PR still needs more testing

Depends on:
- open-rmf/rmf_internal_msgs#63

Known issues:
- When the robots start in simulation not all robots seem to be running
  the idle task, hence some robot dont end up claiming parking spots.

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

rmf_chope_node/CMakeLists.txt

@@ -9,13 +9,13 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rmf_chope_msgs REQUIRED)
-find_package(rmf_fleet_adapter REQUIRED)
+find_package(rmf_building_map_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
 add_executable(queue_manager src/main.cpp)
-ament_target_dependencies(queue_manager rclcpp rmf_chope_msgs)
+ament_target_dependencies(queue_manager rclcpp rmf_chope_msgs rmf_building_map_msgs)
 
 install(TARGETS
   queue_manager

rmf_chope_node/package.xml

@@ -12,8 +12,8 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>rmf_building_map_msgs</depend>
   <depend>rmf_chope_msgs</depend>
-  <depend>rmf_fleet_adapter</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rmf_chope_node/src/main.cpp

@@ -3,17 +3,17 @@
 #include "rclcpp/rclcpp.hpp"
 #include <cstddef>
 #include <cstdint>
-#include <openssl/x509v3.h>
+#include <mutex>
 #include <optional>
-#include <random>
 #include <rclcpp/logging.hpp>
-#include <rmf_chope_msgs/msg/detail/request_header__struct.hpp>
-#include <rmf_chope_msgs/msg/detail/reservation_allocation__struct.hpp>
-#include <rmf_chope_msgs/msg/detail/ticket__struct.hpp>
+
+#include <rmf_building_map_msgs/msg/graph.hpp>
+#include <rmf_chope_msgs/msg/detail/free_parking_spots__struct.hpp>
 #include <rmf_chope_msgs/msg/request_header.hpp>
 #include <rmf_chope_msgs/msg/release_request.hpp>
 #include <rmf_chope_msgs/msg/reservation_allocation.hpp>
 #include <rmf_chope_msgs/msg/ticket.hpp>
+#include <rmf_chope_msgs/msg/free_parking_spots.hpp>
 #include <unordered_map>
 #include <vector>
 
@@ -122,13 +122,13 @@ struct LocationReq {
   }
 };
 
-
 /// Implements a simple Mutex. Only one robot can claim a location at a time.
 /// The current implementation is relatively simplistic and basically checks if a location is occupied or not.
 /// A queuing system is in the works. Note: It is possible for the current system to get deadlocked.
 class CurrentState {
 public:  
   std::vector<std::string> free_locations() {
+    std::lock_guard<std::mutex> lock(_mtx);
     std::vector<std::string> locations;
     for (auto &[loc, state] : _current_location_reservations) {
       if (!state.ticket.has_value()) {
@@ -137,20 +137,35 @@ class CurrentState {
     }
     return locations;
   }
+
+  void add_location(std::string location) {
+    std::lock_guard<std::mutex> lock(_mtx);
+    if (_current_location_reservations.count(location) == 0)
+    {
+      _current_location_reservations.emplace(location, LocationState {std::nullopt});
+    }
+  }
 
   std::optional<std::string> allocate_lowest_cost_free_spot(const std::vector<LocationReq>& incoming_requests, const std::size_t ticket_id)
   {
+    if (_ticket_to_location.count(ticket_id) != 0)
+    {
+      // Ticket has been allocated. Probably some DDS-ism causing the issue
+      return std::nullopt;
+    }
+
     auto requests = incoming_requests;
     std::sort(requests.begin(), requests.end());
+    std::lock_guard<std::mutex> lock(_mtx);
     for (std::size_t i = 0; i < requests.size(); i++) {
       auto parking = _current_location_reservations.find(requests[i].location);
       if (parking == _current_location_reservations.end()) {
-        _current_location_reservations.emplace(requests[i].location, LocationState {ticket_id});
+        _current_location_reservations[requests[i].location] = LocationState {ticket_id};
         _ticket_to_location.emplace(ticket_id, requests[i].location);
         return requests[i].location;
       }
-      else if (!parking->second.ticket.has_value()){
-        _current_location_reservations.emplace(parking->first, LocationState {ticket_id});
+      else if (!parking->second.ticket.has_value()) {
+        _current_location_reservations[requests[i].location].ticket = ticket_id;
         _ticket_to_location.emplace(ticket_id, requests[i].location);
         return parking->first;
       }
@@ -161,6 +176,7 @@ class CurrentState {
 
   bool release(const std::size_t ticket_id)
   {
+    std::lock_guard<std::mutex> lock(_mtx);
     auto _ticket = _ticket_to_location.find(ticket_id);
     if (_ticket == _ticket_to_location.end())
     {
@@ -172,10 +188,13 @@ class CurrentState {
   }
 
 private:
+  std::mutex _mtx;
   std::unordered_map<std::string, LocationState> _current_location_reservations;
   std::unordered_map<std::size_t, std::string> _ticket_to_location;
 };
 
+using namespace std::chrono_literals;
+
 class SimpleQueueSystemNode : public rclcpp::Node {
 public:
   SimpleQueueSystemNode()  : Node("rmf_chope_node") {
@@ -191,12 +210,34 @@ class SimpleQueueSystemNode : public rclcpp::Node {
       ReservationClaimTopicName, qos, std::bind(&SimpleQueueSystemNode::claim_request, this, std::placeholders::_1));
     release_subscription_ = this->create_subscription<rmf_chope_msgs::msg::ReleaseRequest>(
       ReservationReleaseTopicName, qos, std::bind(&SimpleQueueSystemNode::release, this, std::placeholders::_1));
+    graph_subscription_ = this->create_subscription<rmf_building_map_msgs::msg::Graph>("/nav_graphs", qos, std::bind(&SimpleQueueSystemNode::recieved_graph, this, std::placeholders::_1));
 
     ticket_pub_ = this->create_publisher<rmf_chope_msgs::msg::Ticket>(ReservationResponseTopicName, qos);
     allocation_pub_ = this->create_publisher<rmf_chope_msgs::msg::ReservationAllocation>(ReservationAllocationTopicName, qos);
+    free_spot_pub_ = this->create_publisher<rmf_chope_msgs::msg::FreeParkingSpots>("/rmf/reservations/free_parking_spot", qos);
+
+    timer_ = this->create_wall_timer(500ms, std::bind(&SimpleQueueSystemNode::publish_free_spots, this));
   }
 
 private:
+  void recieved_graph(const rmf_building_map_msgs::msg::Graph::ConstSharedPtr &graph_msg) {
+    RCLCPP_INFO(this->get_logger(), "Got graph");
+    for (std::size_t i = 0; i < graph_msg->vertices.size(); i++)
+    {
+      for(auto &param: graph_msg->vertices[i].params) {
+
+        //TODO(arjo) make this configure-able
+        if (param.name == "is_parking_spot" && param.value_bool)
+        {
+          std::stringstream name;
+          name << i;
+          std::string topic;
+          name >> topic;
+          current_state_.add_location(topic);
+        }
+      }
+    }
+  }
   void on_request(const rmf_chope_msgs::msg::FlexibleTimeRequest::ConstSharedPtr &request) {
 
     std::vector<LocationReq> requests;
@@ -249,12 +290,12 @@ class SimpleQueueSystemNode : public rclcpp::Node {
           allocation.satisfies_alternative = i;          
         }
       }
-
+      RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", result.value().c_str(), request->ticket.ticket_id);
       allocation_pub_->publish(allocation);
     }
     else
     {
-       RCLCPP_ERROR(this->get_logger(), "Could not allocate resource for ticket %lu.", request->ticket.ticket_id);
+      RCLCPP_INFO(this->get_logger(), "Could not allocate resource for ticket %lu.", request->ticket.ticket_id);
     }
   }
 
@@ -267,16 +308,28 @@ class SimpleQueueSystemNode : public rclcpp::Node {
     }
   }
 
+
+  void publish_free_spots() {
+    rmf_chope_msgs::msg::FreeParkingSpots spots;
+    spots.spots = current_state_.free_locations();
+
+    free_spot_pub_->publish(spots);
+  }
+
   rclcpp::Subscription<rmf_chope_msgs::msg::FlexibleTimeRequest>::SharedPtr request_subscription_;
   rclcpp::Subscription<rmf_chope_msgs::msg::ClaimRequest>::SharedPtr claim_subscription_;
   rclcpp::Subscription<rmf_chope_msgs::msg::ReleaseRequest>::SharedPtr release_subscription_;
+  rclcpp::Subscription<rmf_building_map_msgs::msg::Graph>::SharedPtr graph_subscription_;
 
   rclcpp::Publisher<rmf_chope_msgs::msg::Ticket>::SharedPtr ticket_pub_;
   rclcpp::Publisher<rmf_chope_msgs::msg::ReservationAllocation>::SharedPtr allocation_pub_;
+  rclcpp::Publisher<rmf_chope_msgs::msg::FreeParkingSpots>::SharedPtr free_spot_pub_;
 
   std::unordered_map<std::size_t, std::vector<LocationReq>> requests_;
   TicketStore ticket_store_;
   CurrentState current_state_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
 };
 
 int main (int argc, const char** argv) {

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -78,6 +78,7 @@ 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 ReservationFreeSpotsTopicName = "/rmf/reservations/free_parking_spot";
 
 const uint64_t Unclaimed = (uint64_t)(-1);
 

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp

@@ -139,6 +139,10 @@ std::shared_ptr<Node> Node::make(
   node->_reservation_release_pub =
     node->create_publisher<ReservationRelease>(
       ReservationReleaseTopicName, transient_local_qos);
+
+  node->_reservation_free_spot_obs =
+    node->create_observable<ReservationFreeSpotStatus>(
+    ReservationFreeSpotsTopicName, transient_local_qos);
   return node;
 }
 
@@ -310,5 +314,10 @@ auto Node::release_location() const -> const ReservationReleasePub&
   return _reservation_release_pub;
 }
 
+//==============================================================================
+auto Node::freespots_obs() const -> const ReservationFreeSpotObs&
+{
+  return _reservation_free_spot_obs->observe();
+}
 } // namespace agv
 } // namespace rmf_fleet_adapter

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp

@@ -18,8 +18,7 @@
 #ifndef SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP
 #define SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP
 
-#include <rmf_chope_msgs/msg/detail/claim_request__struct.hpp>
-#include <rmf_chope_msgs/msg/detail/flexible_time_request__struct.hpp>
+
 #include <rmf_rxcpp/Transport.hpp>
 
 #include <rmf_dispenser_msgs/msg/dispenser_request.hpp>
@@ -39,6 +38,7 @@
 #include <rmf_chope_msgs/msg/ticket.hpp>
 #include <rmf_chope_msgs/msg/reservation_allocation.hpp>
 #include <rmf_chope_msgs/msg/release_request.hpp>
+#include <rmf_chope_msgs/msg/free_parking_spots.hpp>
 
 #include <std_msgs/msg/bool.hpp>
 
@@ -163,6 +163,10 @@ class Node : public rmf_rxcpp::Transport
   using ReservationReleasePub = rclcpp::Publisher<ReservationRelease>::SharedPtr; 
   const ReservationReleasePub& release_location() const;
 
+  using ReservationFreeSpotStatus = rmf_chope_msgs::msg::FreeParkingSpots;
+  using ReservationFreeSpotObs =  rxcpp::observable<ReservationFreeSpotStatus::SharedPtr>;
+  const ReservationFreeSpotObs& freespots_obs() const; 
+
   template<typename DurationRepT, typename DurationT, typename CallbackT>
   rclcpp::TimerBase::SharedPtr try_create_wall_timer(
     std::chrono::duration<DurationRepT, DurationT> period,
@@ -228,6 +232,7 @@ class Node : public rmf_rxcpp::Transport
   ReservationClaimPub _reservation_claim_pub;
   Bridge<ReservationAllocation> _reservation_alloc_obs;
   ReservationReleasePub _reservation_release_pub;
+  Bridge<ReservationFreeSpotStatus> _reservation_free_spot_obs;
 };
 
 } // namespace agv

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp

@@ -38,14 +38,20 @@ class ReservationManager {
     }
 
     std::optional<rmf_chope_msgs::msg::ReservationAllocation> release_ticket() {
-        if (allocations.size() == 0) {
+        if (allocations.size() <= 1) {
+            // For safety every robot must have at least one reservation at any point in time.
             return std::nullopt;
         }
         auto temp = allocations.back();
         allocations.pop_back();
         return temp;
     }
 
+    bool has_ticket() const
+    {
+        return allocations.size() != 0;
+    }
+
     std::deque<rmf_chope_msgs::msg::ReservationAllocation> allocations;
 };
 }

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp

@@ -1614,5 +1614,10 @@ std::optional<rmf_chope_msgs::msg::ReservationAllocation>
   return _reservation_mgr.release_ticket();
 }
 
+//==============================================================================
+bool RobotContext::_has_ticket() const
+{
+  return _reservation_mgr.has_ticket();
+}
 } // namespace agv
 } // namespace rmf_fleet_adapter

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp

@@ -720,6 +720,9 @@ class RobotContext
 
   std::optional<rmf_chope_msgs::msg::ReservationAllocation> _release_resource();
 
+  /// Has ticket now
+  bool _has_ticket() const;
+
   template<typename... Args>
   static std::shared_ptr<RobotContext> make(Args&&... args)
   {
@@ -770,6 +773,8 @@ class RobotContext
         self->_publish_mutex_group_requests();
       });
 
+    //context->
+
     return context;
   }
 

rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp

@@ -521,7 +521,8 @@ std::optional<rmf_traffic::agv::Plan::Goal> GoToPlace::Active::_choose_goal(
 
   // No need to use reservation system if we are already there.
   if (_description.one_of().size() == 1
-  && _description.one_of()[0].waypoint() == current_location[0].waypoint())
+  && _description.one_of()[0].waypoint() == current_location[0].waypoint()
+  && _context->_has_ticket())
   {
     return _description.one_of()[0];
   }