Priority in Task Allocation (#278)

* Addeed priority to requests

* Modified compute_f()

* Removed redundant earlist_start_time from PendingTask

* Added test for priority

* More tests

* Update valid assignments

* Pruning each solution node. Fix for expand_charger()

* Modified tests

* Check for priority validity across agents

* Check for priority validity only if a high priority task exists among requests

* More tests

* rmf_task::Request is no longer an abstract interface

* Refactor

* Commented out tests

* Continued refactor

* Done refactoring

* Done refactoring

* added Priority msg and updated FleetUpdateHandle

* Move Priority and CostCalculator forward declarations into headers

* Added Description to Request

* Refactor after addition of Request::Description

* Fixed UB in rmf_fleet_adapter::tasks

* Return shared ptrs by const ref

* Moved BinaryPriority BinaryPriorityCostCalculator and InvariantHueristicQueue into new headers

* Allow cost calculator to be nullptr

* BinaryPriority and BinaryPriorityCostCalculator do not use pimpl.Doc for make_low_priority and pure virtual destructor for Priority

* Cleanup

Author: Yadunund

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -176,9 +176,9 @@ void TaskManager::set_queue(
         start = assignments[i-1].state().location();
       start.time(a.deployment_time());
       rmf_task_msgs::msg::TaskType task_type_msg;
-
-      if (const auto request =
-        std::dynamic_pointer_cast<const rmf_task::requests::Clean>(a.request()))
+      const auto request = a.request();
+      if (std::dynamic_pointer_cast<
+        const rmf_task::requests::CleanDescription>(request->description()) != nullptr)
       {
         task_type_msg.type = task_type_msg.TYPE_CLEAN;
         auto task = rmf_fleet_adapter::tasks::make_clean(
@@ -191,9 +191,9 @@ void TaskManager::set_queue(
         _queue.push_back(task);
       }
 
-      else if (const auto request =
-        std::dynamic_pointer_cast<const rmf_task::requests::ChargeBattery>(
-          a.request()))
+      else if (std::dynamic_pointer_cast<
+        const rmf_task::requests::ChargeBatteryDescription>(
+          request->description()) != nullptr)
       {
         task_type_msg.type = task_type_msg.TYPE_CHARGE_BATTERY;
         const auto task = tasks::make_charge_battery(
@@ -206,9 +206,9 @@ void TaskManager::set_queue(
         _queue.push_back(task);
       }
 
-      else if (const auto request =
-        std::dynamic_pointer_cast<const rmf_task::requests::Delivery>(
-          a.request()))
+      else if (std::dynamic_pointer_cast<
+        const rmf_task::requests::DeliveryDescription>(
+          request->description()) != nullptr)
       {
         task_type_msg.type = task_type_msg.TYPE_DELIVERY;
         const auto task = tasks::make_delivery(
@@ -221,8 +221,8 @@ void TaskManager::set_queue(
         _queue.push_back(task);
       }
 
-      else if (const auto request =
-        std::dynamic_pointer_cast<const rmf_task::requests::Loop>(a.request()))
+      else if (std::dynamic_pointer_cast<
+        const rmf_task::requests::LoopDescription>(request->description()) != nullptr)
       {
         task_type_msg.type = task_type_msg.TYPE_LOOP;
         const auto task = tasks::make_loop(
@@ -273,8 +273,9 @@ const std::vector<rmf_task::ConstRequestPtr> TaskManager::requests() const
   requests.reserve(_queue.size());
   for (const auto& task : _queue)
   {
-    if (std::dynamic_pointer_cast<const rmf_task::requests::ChargeBattery>(
-      task->request()))
+    if (std::dynamic_pointer_cast<
+      const rmf_task::requests::ChargeBatteryDescription>(
+        task->request()->description()))
       continue;
     requests.push_back(task->request());
   }
@@ -461,7 +462,7 @@ void TaskManager::retreat_to_charger()
       task_planner_config->planner(),
       current_state.finish_time());
 
-    const auto finish = charging_request->estimate_finish(
+    const auto finish = charging_request->description()->estimate_finish(
       current_state,
       _context->task_planning_constraints(),
       estimate_cache);

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -154,6 +154,13 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
   std::string id = msg->task_profile.task_id;
   const auto& graph = planner->get_configuration().graph();
 
+  // Generate the priority of the request. The current implementation supports
+  // binary priority
+  auto priority =
+    task_profile.description.priority.value > 0 ?
+    rmf_task::BinaryPriorityScheme::make_high_priority() :
+    rmf_task::BinaryPriorityScheme::make_low_priority();  
+
   // Process Cleaning task
   if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN)
   {
@@ -239,7 +246,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       tool_sink,
       planner,
       start_time,
-      drain_battery);
+      drain_battery,
+      priority);
 
     RCLCPP_INFO(
       node->get_logger(),
@@ -334,7 +342,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       ambient_sink,
       planner,
       start_time,
-      drain_battery);
+      drain_battery,
+      priority);
 
     RCLCPP_INFO(
       node->get_logger(),
@@ -407,7 +416,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb(
       ambient_sink,
       planner,
       start_time,
-      drain_battery);
+      drain_battery,
+      priority);
 
     RCLCPP_INFO(
       node->get_logger(),
@@ -1111,7 +1121,8 @@ bool FleetUpdateHandle::set_task_planner_params(
         *battery_system,
         motion_sink,
         ambient_sink,
-        _pimpl->planner);
+        _pimpl->planner,
+        _pimpl->cost_calculator);
 
     _pimpl->task_planner = std::make_shared<rmf_task::agv::TaskPlanner>(
       task_config);

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp

@@ -28,6 +28,7 @@
 #include <rmf_task/agv/TaskPlanner.hpp>
 #include <rmf_task/Request.hpp>
 #include <rmf_task/requests/Clean.hpp>
+#include <rmf_task/BinaryPriorityScheme.hpp>
 
 #include <rmf_fleet_msgs/msg/dock_summary.hpp>
 
@@ -142,6 +143,8 @@ class FleetUpdateHandle::Implementation
   std::shared_ptr<rmf_battery::DevicePowerSink> ambient_sink = nullptr;
   std::shared_ptr<rmf_battery::DevicePowerSink> tool_sink = nullptr;
   bool drain_battery = true;
+  std::shared_ptr<rmf_task::CostCalculator> cost_calculator =
+    rmf_task::BinaryPriorityScheme::make_cost_calculator();
   std::shared_ptr<rmf_task::agv::TaskPlanner> task_planner = nullptr;
   bool initialized_task_planner = false;
 

rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp

@@ -26,19 +26,26 @@ namespace tasks {
 
 //==============================================================================
 std::shared_ptr<Task> make_charge_battery(
-    const rmf_task::requests::ConstChargeBatteryRequestPtr request,
+    const rmf_task::ConstRequestPtr request,
     const agv::RobotContextPtr& context,
     const rmf_traffic::agv::Plan::Start start,
     const rmf_traffic::Time deployment_time,
     const rmf_task::agv::State finish_state)
 {
+  std::shared_ptr<const rmf_task::requests::ChargeBatteryDescription>
+  description = std::dynamic_pointer_cast<
+    const rmf_task::requests::ChargeBatteryDescription>(request->description());
+
+  if (description == nullptr)
+    return nullptr;
+
   rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint()};
 
   Task::PendingPhases phases;
   phases.push_back(
     phases::GoToPlace::make(context, std::move(start), goal));
   phases.push_back(
-    phases::WaitForCharge::make(context, request->battery_system(), 0.99));
+    phases::WaitForCharge::make(context, description->battery_system(), 0.99));
 
   return Task::make(
     request->id(),

rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp

@@ -32,7 +32,7 @@ namespace tasks {
 
 //==============================================================================
 std::shared_ptr<Task> make_charge_battery(
-    const rmf_task::requests::ConstChargeBatteryRequestPtr request,
+    const rmf_task::ConstRequestPtr request,
     const agv::RobotContextPtr& context,
     const rmf_traffic::agv::Plan::Start start,
     const rmf_traffic::Time deployment_time,

rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp

@@ -24,15 +24,22 @@ namespace tasks {
 
 //==============================================================================
 std::shared_ptr<Task> make_clean(
-    const rmf_task::requests::ConstCleanRequestPtr request,
+    const rmf_task::ConstRequestPtr request,
     const agv::RobotContextPtr& context,
     const rmf_traffic::agv::Plan::Start clean_start,
     const rmf_traffic::Time deployment_time,
     const rmf_task::agv::State finish_state)
 {
-  rmf_traffic::agv::Planner::Goal clean_goal{request->start_waypoint()};
-  auto end_start = request->location_after_clean(clean_start);
-  rmf_traffic::agv::Planner::Goal end_goal{request->end_waypoint()};
+  std::shared_ptr<const rmf_task::requests::CleanDescription> description = 
+    std::dynamic_pointer_cast<
+      const rmf_task::requests::CleanDescription>(request->description());
+
+  if (description == nullptr)
+    return nullptr;
+
+  rmf_traffic::agv::Planner::Goal clean_goal{description->start_waypoint()};
+  auto end_start = description->location_after_clean(clean_start);
+  rmf_traffic::agv::Planner::Goal end_goal{description->end_waypoint()};
   Task::PendingPhases phases;
   phases.push_back(
         phases::GoToPlace::make(context, std::move(clean_start), clean_goal));

rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp

@@ -32,7 +32,7 @@ namespace tasks {
 
 //==============================================================================
 std::shared_ptr<Task> make_clean(
-    const rmf_task::requests::ConstCleanRequestPtr request,
+    const rmf_task::ConstRequestPtr request,
     const agv::RobotContextPtr& context,
     const rmf_traffic::agv::Plan::Start clean_start,
     const rmf_traffic::Time deployment_time,

rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp

@@ -28,34 +28,45 @@ namespace tasks {
 
 //==============================================================================
 std::shared_ptr<Task> make_delivery(
-    const rmf_task::requests::ConstDeliveryRequestPtr request,
+    const rmf_task::ConstRequestPtr request,
     const agv::RobotContextPtr& context,
     const rmf_traffic::agv::Plan::Start pickup_start,
     const rmf_traffic::Time deployment_time,
     const rmf_task::agv::State finish_state)
 {
+
+  std::shared_ptr<const rmf_task::requests::DeliveryDescription> description =
+    std::dynamic_pointer_cast<
+      const rmf_task::requests::DeliveryDescription>(request->description());
+  
+  if (description == nullptr)
+    return nullptr;
+
   Task::PendingPhases phases;
   phases.push_back(
     phases::GoToPlace::make(
-      context, std::move(pickup_start), request->pickup_waypoint()));
+      context, std::move(pickup_start),
+      description->pickup_waypoint()));
 
   phases.push_back(
         std::make_unique<phases::DispenseItem::PendingPhase>(
           context,
           request->id(),
-          request->pickup_dispenser(),
+          description->pickup_dispenser(),
           context->itinerary().description().owner(),
-          request->items()));
+          description->items()));
 
-  auto dropoff_start = request->dropoff_start(pickup_start);
+  auto dropoff_start = description->dropoff_start(pickup_start);
   phases.push_back(
     phases::GoToPlace::make(
-      context, std::move(dropoff_start), request->dropoff_waypoint()));
+      context,
+      std::move(dropoff_start),
+      description->dropoff_waypoint()));
 
 
   std::vector<rmf_ingestor_msgs::msg::IngestorRequestItem> ingestor_items;
-  ingestor_items.reserve(request->items().size());
-  for(const auto& dispenser_item : request->items()){
+  ingestor_items.reserve(description->items().size());
+  for(const auto& dispenser_item : description->items()){
     rmf_ingestor_msgs::msg::IngestorRequestItem item{};
     item.type_guid = dispenser_item.type_guid;
     item.quantity = dispenser_item.quantity;
@@ -67,7 +78,7 @@ std::shared_ptr<Task> make_delivery(
         std::make_unique<phases::IngestItem::PendingPhase>(
           context,
           request->id(),
-          request->dropoff_ingestor(),
+          description->dropoff_ingestor(),
           context->itinerary().description().owner(),
           ingestor_items));
 

rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp

@@ -28,7 +28,7 @@ namespace tasks {
 
 //==============================================================================
 std::shared_ptr<Task> make_delivery(
-    const rmf_task::requests::ConstDeliveryRequestPtr request,
+    const rmf_task::ConstRequestPtr request,
     const agv::RobotContextPtr& context,
     const rmf_traffic::agv::Plan::Start pickup_start,
     const rmf_traffic::Time deployment_time,

rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp

@@ -24,34 +24,40 @@ namespace tasks {
 
 //==============================================================================
 std::shared_ptr<Task> make_loop(
-    const rmf_task::requests::ConstLoopRequestPtr request,
+    const rmf_task::ConstRequestPtr request,
     const agv::RobotContextPtr& context,
     const rmf_traffic::agv::Plan::Start start,
     const rmf_traffic::Time deployment_time,
     const rmf_task::agv::State finish_state)
 {
+  std::shared_ptr<const rmf_task::requests::LoopDescription> description =
+    std::dynamic_pointer_cast<
+      const rmf_task::requests::LoopDescription>(request->description());
+
+  if (description == nullptr)
+    return nullptr;
 
   Task::PendingPhases phases;
-  const auto loop_start = request->loop_start(start);
-  const auto loop_end = request->loop_end(loop_start);
+  const auto loop_start = description->loop_start(start);
+  const auto loop_end = description->loop_end(loop_start);
 
   phases.push_back(
     phases::GoToPlace::make(
-      context, std::move(start), request->start_waypoint()));
+      context, std::move(start), description->start_waypoint()));
 
   phases.push_back(
     phases::GoToPlace::make(
-      context, loop_start, request->finish_waypoint()));
+      context, loop_start, description->finish_waypoint()));
 
-  for (std::size_t i = 1; i < request->num_loops(); ++i)
+  for (std::size_t i = 1; i < description->num_loops(); ++i)
   {
     phases.push_back(
       phases::GoToPlace::make(
-        context, loop_end, request->start_waypoint()));
+        context, loop_end, description->start_waypoint()));
 
     phases.push_back(
       phases::GoToPlace::make(
-        context, loop_start, request->finish_waypoint()));
+        context, loop_start, description->finish_waypoint()));
   }
 
   return Task::make(