Makes sure that the reservation system always assigns nearest goal during Emergency Pullover (open-rmf#484)

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

Author: arjo129

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp

@@ -58,4 +58,4 @@ std::string ReservationManager::get_reserved_location() const
 bool ReservationManager::has_ticket() const
 {
   return _allocation.has_value();
-}
+}

rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp

@@ -173,7 +173,7 @@ auto EmergencyPullover::Active::make(
       active->_context->_find_and_sort_parking_spots(true);
 
     RCLCPP_INFO(active->_context->node()->get_logger(),
-      "Creating reservation negotiator");
+      "Creating reservation negotiator for emergency pullover");
     active->_reservation_client = reservation::ReservationNodeNegotiator::make(
       active->_context,
       potential_waitpoints,
@@ -194,7 +194,8 @@ auto EmergencyPullover::Active::make(
 
         self->_chosen_goal = goal;
         self->_find_plan();
-      }
+      },
+      true // Always re-plan to find the nearest spot.
     );
   }
   return active;

rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp

@@ -25,27 +25,42 @@
 namespace rmf_fleet_adapter {
 namespace reservation {
 
-/// This class implements the protocol for negotiating a spot with the "reservation"
-/// node. The reservation node maintains a list of spots which are free.
+/// This class implements the protocol for negotiating a spot with the
+/// "reservation" node. The reservation node maintains a list of spots which
+/// are free.
 class ReservationNodeNegotiator :
   public std::enable_shared_from_this<ReservationNodeNegotiator>
 {
 public:
+  //============================================================================
+  /// Creates a ReservationNegotiator to handle the reservation system protocol
+  /// \param[in] context the RobotContext with all of the current robot's
+  /// context.
+  /// \param[in] goals a list of potential spots.
+  /// \param[in] same_map Is the goal on the same map
+  /// \param[in] selected_final_destination_cb A callback which is triggered
+  /// when the final destination is selected.
+  /// \param[in] always_recalculate_nearest_goal Always find the nearest goal
   static std::shared_ptr<ReservationNodeNegotiator> make(
     std::shared_ptr<agv::RobotContext> context,
     const std::vector<rmf_traffic::agv::Plan::Goal> goals,
     const bool same_map,
     const std::function<void(const rmf_traffic::agv::Plan::Goal&)>
     selected_final_destination_cb,
-    const std::function<void(const rmf_traffic::agv::Plan::Goal&)> selected_waitpoint_cb)
+    const std::function<void(const rmf_traffic::agv::Plan::Goal&)>
+    selected_waitpoint_cb,
+    bool always_recalculate_nearest_goal = false)
   {
     auto negotiator = std::shared_ptr<ReservationNodeNegotiator>(
       new ReservationNodeNegotiator(
-        context, goals, selected_final_destination_cb, selected_waitpoint_cb));
+        context,
+        goals,
+        selected_final_destination_cb,
+        selected_waitpoint_cb));
 
     negotiator->_reservation_ticket =
-      context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker(
-          context->worker()))
+      context->node()->location_ticket_obs().observe_on(
+        rxcpp::identity_same_worker(context->worker()))
       .subscribe([ptr = negotiator->weak_from_this()](
         const std::shared_ptr<rmf_reservation_msgs::msg::Ticket>
         & msg)
@@ -169,22 +184,24 @@ class ReservationNodeNegotiator :
           }
         });
 
-
-    for (std::size_t i = 0; i < negotiator->_goals.size(); ++i)
+    if (!always_recalculate_nearest_goal)
     {
-      if (events::wp_name(*context.get(), negotiator->_goals[i]) == context->_get_reserved_location())
+      for (std::size_t i = 0; i < negotiator->_goals.size(); ++i)
       {
-        RCLCPP_INFO(context->node()->get_logger(),
-          "%s: Already at goal no need to engage reservation system\n",
-          context->requester_id().c_str());
-        context->worker().schedule([
-            cb = negotiator->_selected_final_destination_cb,
-            wp = negotiator->_goals[i]
-          ](const auto&)
-          {
-            cb(wp);
-          });
-        return negotiator;
+        if (events::wp_name(*context.get(), negotiator->_goals[i]) == context->_get_reserved_location())
+        {
+          RCLCPP_INFO(context->node()->get_logger(),
+            "%s: Already have a goal no need to engage reservation system\n",
+            context->requester_id().c_str());
+          context->worker().schedule([
+              cb = negotiator->_selected_final_destination_cb,
+              wp = negotiator->_goals[i]
+            ](const auto&)
+            {
+              cb(wp);
+            });
+          return negotiator;
+        }
       }
     }
     RCLCPP_INFO(negotiator->_context->node()->get_logger(),
@@ -205,13 +222,14 @@ class ReservationNodeNegotiator :
   }
 
 private:
-
+  //============================================================================
   ReservationNodeNegotiator(
     std::shared_ptr<agv::RobotContext> context,
     const std::vector<rmf_traffic::agv::Plan::Goal> goals,
     const std::function<void(const rmf_traffic::agv::Plan::Goal&)>
-    selected_final_destination_cb,
-    const std::function<void(const rmf_traffic::agv::Plan::Goal&)> selected_waitpoint_cb)
+      selected_final_destination_cb,
+    const std::function<void(const rmf_traffic::agv::Plan::Goal&)>
+      selected_waitpoint_cb)
   {
     _context = context;
     _goals = goals;
@@ -220,6 +238,7 @@ class ReservationNodeNegotiator :
     _reservation_id = _context->last_reservation_request_id();
   }
 
+  //============================================================================
   enum class ReservationState
   {
     Pending=0,
@@ -228,7 +247,7 @@ class ReservationNodeNegotiator :
     ReceivedResponseProceedImmediate=3
   };
 
-
+  //============================================================================
   void make_request(bool only_same_map)
   {
     auto current_location = _context->location();

rmf_reservation_node/src/main.cpp

@@ -27,7 +27,6 @@
 #include <rmf_reservation_msgs/msg/claim_request.hpp>
 #include <rmf_fleet_adapter/StandardNames.hpp>
 
-#include <iostream>
 #include <sstream>
 #include <unordered_map>
 #include <vector>