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,73 @@ class TaskPlanner
 {
 public:
 
+  /// The strategy to use when assigning tasks to robots during node expansion.
+  /// This struct defines the various profiles and their associated weights
+  /// for cost calculation.
+  struct TaskAssignmentStrategy
+  {
+    enum class Profile
+    {
+      DefaultFastest,
+      BatteryAware,
+      Custom
+    };
+
+    struct Weights
+    {
+      std::vector<double> finish_time;
+      std::vector<double> battery_penalty;
+      std::vector<double> busy_penalty;
+    };
+
+    struct Options
+    {
+      enum class BusyMode
+      {
+        Binary,
+        Count
+      };
+      BusyMode busy_mode = BusyMode::Binary;
+    };
+
+    Profile profile = Profile::DefaultFastest;
+    Weights weights;
+    Options options;
+
+    static TaskAssignmentStrategy make(Profile profile)
+    {
+      TaskAssignmentStrategy model;
+      model.profile = profile;
+
+      switch (profile)
+      {
+        case Profile::DefaultFastest:
+          // Default RMF assignment strategy with fastest-first approach
+          model.weights.finish_time = {0.0, 1.0};
+          model.weights.battery_penalty = {0.0};
+          model.weights.busy_penalty = {0.0};
+          break;
+
+        case Profile::BatteryAware:
+          // Prioritize battery level, strongly penalize low SOC with a quadratic term.
+          // Still account for task efficiency (fastest-first), but ignore busyness.
+          model.weights.finish_time = {0.0, 1.0};
+          model.weights.battery_penalty = {0.0, 20.0, 60.0};
+          model.weights.busy_penalty = {0.0};
+          break;
+
+        case Profile::Custom:
+          // To be overwritten from fleet_config.yaml
+          break;
+
+        default:
+          // Default to DefaultFastest
+          return make(Profile::DefaultFastest);
+      }
+      return model;
+    }
+  };
+
   /// The Configuration class contains planning parameters that are immutable
   /// for each TaskPlanner instance and should not change in between plans.
   class Configuration
@@ -128,6 +195,14 @@ class TaskPlanner
     /// Get the request factory that will generate a finishing task
     ConstRequestFactoryPtr finishing_request() const;
 
+    /// Set the task assignment strategy (profile & custom weights)
+    /// used by the planner
+    Options& task_assignment_strategy(TaskAssignmentStrategy strategy);
+
+    /// Get the task assignment strategy (profile & custom weights)
+    /// used by the planner
+    TaskAssignmentStrategy task_assignment_strategy() 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

@@ -106,6 +106,7 @@ class TaskPlanner::Options::Implementation
   bool greedy;
   std::function<bool()> interrupter;
   ConstRequestFactoryPtr finishing_request;
+  TaskAssignmentStrategy assignment_strategy;
 };
 
 //==============================================================================
@@ -117,7 +118,8 @@ TaskPlanner::Options::Options(
       Implementation{
         std::move(greedy),
         std::move(interrupter),
-        std::move(finishing_request)
+        std::move(finishing_request),
+        TaskAssignmentStrategy::make(TaskAssignmentStrategy::Profile::DefaultFastest)
       }))
 {
   // Do nothing
@@ -164,6 +166,21 @@ ConstRequestFactoryPtr TaskPlanner::Options::finishing_request() const
   return _pimpl->finishing_request;
 }
 
+//==============================================================================
+auto TaskPlanner::Options::task_assignment_strategy(
+  TaskAssignmentStrategy strategy) -> Options&
+{
+  _pimpl->assignment_strategy = strategy;
+  return *this;
+}
+
+//==============================================================================
+TaskPlanner::TaskAssignmentStrategy
+TaskPlanner::Options::task_assignment_strategy() const
+{
+  return _pimpl->assignment_strategy;
+}
+
 //==============================================================================
 class TaskPlanner::Assignment::Implementation
 {
@@ -514,9 +531,9 @@ class TaskPlanner::Implementation
     const std::vector<ConstRequestPtr>& requests,
     const std::function<bool()> interrupter,
     ConstRequestFactoryPtr finishing_request,
-    bool greedy)
+    bool greedy,
+    TaskAssignmentStrategy task_assignment_strategy)
   {
-
     cost_calculator = config.cost_calculator() ? config.cost_calculator() :
       rmf_task::BinaryPriorityScheme::make_cost_calculator();
 
@@ -546,7 +563,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, task_assignment_strategy);
 
       if (!node)
       {
@@ -1012,19 +1029,145 @@ class TaskPlanner::Implementation
     return node;
   }
 
+  double poly_eval(const std::vector<double>& coeffs, double x)
+  {
+    double sum = 0.0;
+    for (std::size_t i = 0; i < coeffs.size(); ++i)
+    {
+      sum += coeffs[i] * std::pow(x, static_cast<int>(i));
+    }
+    return sum;
+  }
+
+  double compute_task_cost(
+    double finish_time,
+    double soc,
+    int busy_count,
+    const TaskPlanner::TaskAssignmentStrategy& task_assignment_strategy)
+  {
+    // Finish time cost
+    double finish_time_cost =
+      poly_eval(task_assignment_strategy.weights.finish_time, finish_time);
+
+    // Battery penalty
+    double battery_x = 1.0 - soc;
+    double battery_penalty =
+      poly_eval(task_assignment_strategy.weights.battery_penalty, battery_x);
+
+    // Busyness penalty
+    // busy_x = 0 if idle, else 1 or task count depending on BusyMode
+    double busy_x = 0.0;
+    if (task_assignment_strategy.options.busy_mode ==
+      TaskPlanner::TaskAssignmentStrategy::Options::BusyMode::Binary)
+    {
+      busy_x = (busy_count > 0 ? 1.0 : 0.0);
+    }
+    else if (task_assignment_strategy.options.busy_mode ==
+      TaskPlanner::TaskAssignmentStrategy::Options::BusyMode::Count)
+    {
+      busy_x = static_cast<double>(busy_count);
+    }
+
+    double busy_penalty =
+      poly_eval(task_assignment_strategy.weights.busy_penalty, busy_x);
+
+    // Final cost
+    double total_cost = finish_time_cost + battery_penalty + busy_penalty;
+
+    // std::cout << "task_assignment_strategy: "
+    //       << static_cast<int>(task_assignment_strategy.profile)
+    //       << std::endl;
+    // std::cout << " In compute_task_cost(): \n"
+    //           << "  finish_time_cost: " << finish_time_cost
+    //           << ", battery_penalty: " << battery_penalty
+    //           << ", busy_penalty: " << busy_penalty
+    //           << ", total_cost: " << total_cost
+    //           << std::endl;
+
+    return total_cost;
+  }
+
+  // Returns the iterator of the lowest cost candidates
+  // calculated using the provided strategy
+  std::vector<Candidates::Map::const_iterator> find_lowest_cost_candidates(
+    const Candidates::Range& all_candidates,
+    const Node& parent,
+    const std::vector<State>& initial_states,
+    rmf_traffic::Time time_now,
+    const TaskPlanner::TaskAssignmentStrategy& task_assignment_strategy)
+  {
+    std::vector<Candidates::Map::const_iterator> lowest_cost_candidates;
+    double best_cost = std::numeric_limits<double>::infinity();
+
+    for (auto it = all_candidates.begin; it != all_candidates.end; ++it)
+    {
+      const auto& entry = it->second;
+      // duration of task finish time from current time
+      double finish_time = rmf_traffic::time::to_seconds(it->first - time_now);
+      // make sure finish_time is non-negative
+      finish_time = std::max(0.0, finish_time);
+      double soc = entry.state.battery_soc().value_or(1.0);
+
+      // If a robot is not idle at the start of planning (!initial_states.idle()),
+      // we consider it busy with one task.
+      // Reason being at the FleetUpdateHandle aggregate_expectations() there,
+      // it will re-putting all queued-tasks of robot into pending_requests & pass
+      // to task planner to replan altogether.
+      // So when robot is not idle (!initial_states.idle()), it will be having
+      // only 1 task, which is the task it is executing.
+      int initially_busy = initial_states[entry.candidate].is_idle() ? 0 : 1;
+      int task_assigned = parent.assigned_tasks[entry.candidate].size();
+      auto busy_count = initially_busy + task_assigned;
+
+      double cost =
+        compute_task_cost(finish_time, soc, busy_count,
+          task_assignment_strategy);
+
+      if (cost < best_cost)
+      {
+        best_cost = cost;
+        // found new best, reset the list
+        lowest_cost_candidates.clear();
+        lowest_cost_candidates.push_back(it);
+      }
+      else if (std::fabs(cost - best_cost) < 1e-9)
+      {
+        // Tie, include this candidate too
+        lowest_cost_candidates.push_back(it);
+      }
+
+      // std::cout << "In find_lowest_cost_candidates(): \n";
+      // std::cout << "  Candidate: Robot " << entry.candidate
+      //           << ", finish_time: " << finish_time
+      //           << ", soc: " << soc
+      //           << ", busy_count: " << busy_count
+      //           << ", cost: " << cost
+      //           << std::endl;
+    }
+
+    return lowest_cost_candidates;
+  }
+
   std::vector<ConstNodePtr> expand(
     ConstNodePtr parent,
     Filter& filter,
     const std::vector<State>& initial_states,
-    rmf_traffic::Time time_now)
+    rmf_traffic::Time time_now,
+    TaskAssignmentStrategy task_assignment_strategy)
   {
     std::vector<ConstNodePtr> new_nodes;
     new_nodes.reserve(
       parent->unassigned_tasks.size() + parent->assigned_tasks.size());
     for (const auto& u : parent->unassigned_tasks)
     {
-      const auto& range = u.second.candidates.best_candidates();
-      for (auto it = range.begin; it != range.end; it++)
+      const auto& range = find_lowest_cost_candidates(
+        u.second.candidates.all_candidates(),
+        *parent,
+        initial_states,
+        time_now,
+        task_assignment_strategy);
+
+      for (const auto& it : range)
       {
         if (auto new_node = expand_candidate(
             it, u, parent, &filter, time_now))
@@ -1064,7 +1207,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,
+    TaskAssignmentStrategy task_assignment_strategy)
   {
     using PriorityQueue = std::priority_queue<
       ConstNodePtr,
@@ -1092,7 +1236,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, task_assignment_strategy);
 
       // Add copies and with a newly assigned task to queue
       for (const auto& n : new_nodes)
@@ -1147,7 +1291,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.task_assignment_strategy());
 }
 
 // ============================================================================
@@ -1163,7 +1308,8 @@ auto TaskPlanner::plan(
     requests,
     options.interrupter(),
     options.finishing_request(),
-    options.greedy());
+    options.greedy(),
+    options.task_assignment_strategy());
 }
 
 // ============================================================================

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(