Customizable weights for task assignment cost

rmf_task/include/rmf_task/State.hpp

@@ -104,6 +104,12 @@ class State : public CompositeData
   /// If any necessary component is missing (i.e. CurrentWaypoint,
   /// CurrentOrientation, or CurrentTime) then this will return a std::nullopt.
   std::optional<rmf_traffic::agv::Plan::Start> extract_plan_start() const;
+
+  /// Check if the robot is idle, i.e. it is not assigned to any task and is
+  /// not currently executing any task.
+  RMF_TASK_DEFINE_COMPONENT(bool, IsIdle);
+  bool is_idle() const;
+  State& idle(bool is_idle);
 };
 
 } // namespace rmf_task

rmf_task/include/rmf_task/TaskPlanner.hpp

@@ -39,6 +39,24 @@ class TaskPlanner
 {
 public:
 
+  /// The ExpansionPolicy enum defines how the planner expands search nodes
+  /// during the task assignment search.
+  ///
+  /// 0: ShortestFinishTime: The planner expands the search nodes based on the
+  ///   shortest estimated finish time of the tasks, get from best_candidates().
+  ///
+  /// 1: IdlePreferred: The planner expands the search nodes by prioritizing
+  ///   idle robots, which means that if there are idle robots available, they
+  ///   will be preferred for task assignments over busy robots, even if the
+  ///   busy robots can finish tasks sooner. Only the shortest finish time
+  ///   idle robots will be considered.
+  ///
+  enum class ExpansionPolicy
+  {
+    ShortestFinishTime = 0,
+    IdlePreferred = 1
+  };
+
   /// The Configuration class contains planning parameters that are immutable
   /// for each TaskPlanner instance and should not change in between plans.
   class Configuration
@@ -104,10 +122,16 @@ class TaskPlanner
     /// \param[in] finishing_request
     ///   A request factory that generates a tailored task for each agent/AGV
     ///   to perform at the end of their assignments
+    ///
+    /// \param[in] expansion_policy
+    ///   The policy that defines how the planner expands search nodes during
+    ///   task assignment. Available options are in enum class ExpansionPolicy.
+    ///
     Options(
       bool greedy,
       std::function<bool()> interrupter = nullptr,
-      ConstRequestFactoryPtr finishing_request = nullptr);
+      ConstRequestFactoryPtr finishing_request = nullptr,
+      ExpansionPolicy expansion_policy = ExpansionPolicy::ShortestFinishTime);
 
     /// Set whether a greedy approach should be used
     Options& greedy(bool value);
@@ -128,6 +152,12 @@ class TaskPlanner
     /// Get the request factory that will generate a finishing task
     ConstRequestFactoryPtr finishing_request() const;
 
+    /// Set the expansion policy for task assignments
+    Options& expansion_policy(ExpansionPolicy policy);
+
+    /// Get the expansion policy for task assignments
+    ExpansionPolicy expansion_policy() const;
+
     class Implementation;
   private:
     rmf_utils::impl_ptr<Implementation> _pimpl;

rmf_task/src/rmf_task/State.cpp

@@ -169,4 +169,20 @@ std::optional<rmf_traffic::agv::Plan::Start> State::extract_plan_start() const
   return rmf_traffic::agv::Plan::Start(t->value, wp->value, ori->value);
 }
 
+//==============================================================================
+bool State::is_idle() const
+{
+  if (const auto* idle = get<IsIdle>())
+    return idle->value;
+
+  return false;
+}
+
+//==============================================================================
+State& State::idle(bool is_idle)
+{
+  with<IsIdle>(is_idle);
+  return *this;
+}
+
 } // namespace rmf_task

rmf_task/src/rmf_task/TaskPlanner.cpp

@@ -28,6 +28,9 @@
 #include <queue>
 #include <string_view>
 
+#include <algorithm>
+#include <iterator>
+
 namespace rmf_task {
 
 //==============================================================================
@@ -106,18 +109,21 @@ class TaskPlanner::Options::Implementation
   bool greedy;
   std::function<bool()> interrupter;
   ConstRequestFactoryPtr finishing_request;
+  ExpansionPolicy expansion_policy = ExpansionPolicy::ShortestFinishTime;
 };
 
 //==============================================================================
 TaskPlanner::Options::Options(
   bool greedy,
   std::function<bool()> interrupter,
-  ConstRequestFactoryPtr finishing_request)
+  ConstRequestFactoryPtr finishing_request,
+  ExpansionPolicy expansion_policy)
 : _pimpl(rmf_utils::make_impl<Implementation>(
       Implementation{
         std::move(greedy),
         std::move(interrupter),
-        std::move(finishing_request)
+        std::move(finishing_request),
+        std::move(expansion_policy)
       }))
 {
   // Do nothing
@@ -164,6 +170,19 @@ ConstRequestFactoryPtr TaskPlanner::Options::finishing_request() const
   return _pimpl->finishing_request;
 }
 
+//==============================================================================
+auto TaskPlanner::Options::expansion_policy(ExpansionPolicy policy) -> Options&
+{
+  _pimpl->expansion_policy = policy;
+  return *this;
+}
+
+//==============================================================================
+TaskPlanner::ExpansionPolicy TaskPlanner::Options::expansion_policy() const
+{
+  return _pimpl->expansion_policy;
+}
+
 //==============================================================================
 class TaskPlanner::Assignment::Implementation
 {
@@ -508,13 +527,27 @@ class TaskPlanner::Implementation
     }
   }
 
+  static bool has_non_charging(
+    const std::vector<Node::AssignmentWrapper>& assignments)
+  {
+    for (const auto& a : assignments)
+    {
+      if (!std::dynamic_pointer_cast<
+          const rmf_task::requests::ChargeBattery::Description>(
+          a.assignment.request()->description()))
+        return true;
+    }
+    return false;
+  }
+
   Result complete_solve(
     rmf_traffic::Time time_now,
     std::vector<State>& initial_states,
     const std::vector<ConstRequestPtr>& requests,
     const std::function<bool()> interrupter,
     ConstRequestFactoryPtr finishing_request,
-    bool greedy)
+    bool greedy,
+    ExpansionPolicy expansion_policy)
   {
 
     cost_calculator = config.cost_calculator() ? config.cost_calculator() :
@@ -546,7 +579,7 @@ class TaskPlanner::Implementation
         node = greedy_solve(node, initial_states, time_now);
       else
         node = solve(node, initial_states,
-            requests.size(), time_now, interrupter);
+            requests.size(), time_now, interrupter, expansion_policy);
 
       if (!node)
       {
@@ -1012,26 +1045,143 @@ class TaskPlanner::Implementation
     return node;
   }
 
-  std::vector<ConstNodePtr> expand(
+  // Expand using default best_candidates() strategy (shortest finish time)
+  std::vector<ConstNodePtr> expand_shortest_finish_time_robots(
+    ConstNodePtr parent,
+    Filter& filter,
+    rmf_traffic::Time time_now)
+  {
+    std::vector<ConstNodePtr> nodes;
+    for (const auto& u : parent->unassigned_tasks)
+    {
+      const auto& range = u.second.candidates.best_candidates();
+      for (auto it = range.begin; it != range.end; ++it)
+      {
+        if (auto node = expand_candidate(it, u, parent, &filter, time_now))
+          nodes.push_back(std::move(node));
+      }
+    }
+    return nodes;
+  }
+
+  // Expand shortest-finish-time-idle-robots strategy:
+  // 1) If there are any idle robots that are still unassigned, try to expand them
+  //    first.
+  // 2) If there are no idle robots or all initially idle robots already assigned,
+  //    fall back to the default best_candidates() strategy.
+  std::vector<ConstNodePtr> expand_fastest_idle_robots(
     ConstNodePtr parent,
     Filter& filter,
     const std::vector<State>& initial_states,
     rmf_traffic::Time time_now)
   {
     std::vector<ConstNodePtr> new_nodes;
-    new_nodes.reserve(
-      parent->unassigned_tasks.size() + parent->assigned_tasks.size());
+
+    // is this index an initially-idle robot and now still has no non-charging assignment?
+    std::vector<bool> idle_and_unassigned(initial_states.size(), false);
+    for (std::size_t i = 0; i < initial_states.size(); ++i)
+      idle_and_unassigned[i] =
+        initial_states[i].is_idle() &&
+        !has_non_charging(parent->assigned_tasks[i]);
+
+    // is there any idle-and-still-unassigned candidate?
+    const bool any_idle_still_unassigned = std::any_of(
+      idle_and_unassigned.begin(),
+      idle_and_unassigned.end(),
+      [](bool v) { return v; }
+    );
+
     for (const auto& u : parent->unassigned_tasks)
     {
+      if (any_idle_still_unassigned)
+      {
+        // 1) Check if the next candidate is idle-and-still-unassigned from
+        //    all_candidates list, if yes, then add it to new_nodes.
+        //    (Will expand next equally-best-shortest-finish-time
+        //    idle robots only)
+
+        const auto& all_candidates = u.second.candidates.all_candidates();
+        bool added_any_idle_child = false;
+        rmf_traffic::Time added_child_finish_time = rmf_traffic::Time::max();
+
+        for (auto it = all_candidates.begin; it != all_candidates.end; ++it)
+        {
+          // stop if we've already added a child and the finish time of
+          // this next candidate is not equally-shortest as the previous one
+          if (added_any_idle_child && added_child_finish_time != it->first)
+            break;
+
+          // skip if not idle-and-still-unassigned
+          if (!idle_and_unassigned[it->second.candidate])
+            continue;
+
+          if (auto new_node =
+            expand_candidate(it, u, parent, &filter, time_now))
+          {
+            new_nodes.push_back(std::move(new_node));
+            if (!added_any_idle_child)
+              added_child_finish_time = it->first;
+            added_any_idle_child = true;
+          }
+        }
+
+        // If we added any idle robot, we skip the rest of the candidates
+        // and proceed to the next unassigned task.
+        if (added_any_idle_child)
+          continue;
+      }
+
+      // 2) If none of the idle robots node added (no idle robots or
+      //    all initially idle robots already assigned),
+      //    we fall back to default Best Candidates expansion strategy.
+
       const auto& range = u.second.candidates.best_candidates();
-      for (auto it = range.begin; it != range.end; it++)
+
+      for (auto it = range.begin; it != range.end; ++it)
       {
-        if (auto new_node = expand_candidate(
-            it, u, parent, &filter, time_now))
+        if (auto new_node =
+          expand_candidate(it, u, parent, &filter, time_now))
           new_nodes.push_back(std::move(new_node));
       }
     }
 
+    return new_nodes;
+  }
+
+  std::vector<ConstNodePtr> expand(
+    ConstNodePtr parent,
+    Filter& filter,
+    const std::vector<State>& initial_states,
+    rmf_traffic::Time time_now,
+    ExpansionPolicy expansion_policy)
+  {
+    std::vector<ConstNodePtr> new_nodes;
+    new_nodes.reserve(
+      parent->unassigned_tasks.size() + parent->assigned_tasks.size());
+
+    // Task-assignment node expansions based on policy
+    switch (expansion_policy)
+    {
+      case ExpansionPolicy::ShortestFinishTime:
+      {
+        auto n = expand_shortest_finish_time_robots(parent, filter, time_now);
+        new_nodes.insert(new_nodes.end(),
+          std::make_move_iterator(n.begin()),
+          std::make_move_iterator(n.end()));
+        break;
+      }
+
+      case ExpansionPolicy::IdlePreferred:
+      {
+        auto n = expand_fastest_idle_robots(
+          parent, filter, initial_states, time_now);
+        new_nodes.insert(new_nodes.end(),
+          std::make_move_iterator(n.begin()),
+          std::make_move_iterator(n.end()));
+        break;
+      }
+    }
+
     // Assign charging task to each robot
     for (std::size_t i = 0; i < parent->assigned_tasks.size(); ++i)
     {
@@ -1064,7 +1214,8 @@ class TaskPlanner::Implementation
     const std::vector<State>& initial_states,
     const std::size_t num_tasks,
     rmf_traffic::Time time_now,
-    std::function<bool()> interrupter)
+    std::function<bool()> interrupter,
+    ExpansionPolicy expansion_policy)
   {
     using PriorityQueue = std::priority_queue<
       ConstNodePtr,
@@ -1092,7 +1243,7 @@ class TaskPlanner::Implementation
 
       // Apply possible actions to expand the node
       const auto new_nodes = expand(
-        top, filter, initial_states, time_now);
+        top, filter, initial_states, time_now, expansion_policy);
 
       // Add copies and with a newly assigned task to queue
       for (const auto& n : new_nodes)
@@ -1147,7 +1298,8 @@ auto TaskPlanner::plan(
     requests,
     _pimpl->default_options.interrupter(),
     _pimpl->default_options.finishing_request(),
-    _pimpl->default_options.greedy());
+    _pimpl->default_options.greedy(),
+    _pimpl->default_options.expansion_policy());
 }
 
 // ============================================================================
@@ -1163,7 +1315,8 @@ auto TaskPlanner::plan(
     requests,
     options.interrupter(),
     options.finishing_request(),
-    options.greedy());
+    options.greedy(),
+    options.expansion_policy());
 }
 
 // ============================================================================

rmf_task/src/rmf_task/internal_task_planning.cpp

@@ -85,6 +85,17 @@ Candidates::Range Candidates::best_candidates() const
   return range;
 }
 
+// ============================================================================
+Candidates::Range Candidates::all_candidates() const
+{
+  assert(!_value_map.empty());
+
+  Range range;
+  range.begin = _value_map.begin();
+  range.end = _value_map.end();
+  return range;
+}
+
 // ============================================================================
 Candidates& Candidates::operator=(const Candidates& other)
 {

rmf_task/src/rmf_task/internal_task_planning.hpp

@@ -87,6 +87,8 @@ class Candidates
 
   Range best_candidates() const;
 
+  Range all_candidates() const;
+
   rmf_traffic::Time best_finish_time() const;
 
   void update_candidate(