Feature/Idle Robot Preferred Task Planning

[kjchee]

New feature implementation

Implemented feature

This PR is related to feature proposal: #127

This update introduces a new parameter in the task planner called ExpansionPolicy. It defines the policy used when the task planner assigns unassigned-task during node expansion. Currently, the available options are:

1. ShortestFinishTime (default -- use existing best_candidates() method)
2. IdlePreferred (newly added -- prioritizes assigning to the shortest-finish-time idle robot if available during node expansion)

When IdlePreferred is selected, the planner prioritizes assigning tasks to idle robots (robot that are not executing any task and currently still unassigned during node expansion) during node expansion, rather than solely picking the robot with the shortest estimated finish time. However, if all robot are idle or all robot are busy, then it falls back to the default strategy of selecting the best candidate based on shortest estimated finish time.

This ExpansionOption can be extended in the future to support additional strategies, eg. IdlePreferredHighBattery (preferred idled robot with highest soc) and etc...

Implementation description

1. TaskPlanner.cpp

• Main changes are in the expand().
• First, added a new parameter in rmf_task::TaskPlanner::Options called expansion_policy, it is updated in FleetUpdateHandle.cpp (via fleet_config.yaml), and passed into Task Planner.
• Also, there is a new variable representing each robot's idle state (true if robot is not executing a task ---- determined in TaskManager.cpp: if no _active, no _direct_queue). This is passed into complete_solve() along with initial_states.
• In expand(), we will switch case based on expansion_policy, currently available cases are: ShortestFinishTime, and IdlePreferred.
• If case = ShortestFinishTime,
  • Call a new function: expand_shortest_finish_time_robots()
  • This function uses default node expansion strategy which selects best candidate to assign those unassigned task while expanding next node.
• If case = IdlePreferred,
  • Call another new function: expand_fastest_idle_robots()
  • First, the function check if there is any idle-and-still-unassigned robot from initial_states & parent node:
    • if yes, just expand the node with equally-shortest-finish-time idle robot, and continue to the next unassigned task.
  • Second, if no idle robot exists at all, fall back to using the default best candidates and expand the node.

2. Next is more about how the expansion_policy variable/parameter is updated or passed to TaskPlanner.cpp

• It starts from the fleet config [task_assignment_strategy: "ShortestFinishTime" or "IdlePreferred"] in each fleet's fleet_config.yaml
• This is read by EasyFullControl::FleetConfiguration::from_config_files()
• The config is then passed to Adapter::add_easy_fleet()
• add_easy_fleet() calls set_task_planner_params() to set the rmf_task::TaskPlanner::Options parameter: rmf_task::TaskPlanner::ExpansionPolicy expansion_policy
• It is eventually passed to TaskPlanner and used in the expand() logic.

3. To test it:

• Will need a separate PR in rmf_ros2 Feature/Support Idle Robot Preferred in Task Planning rmf_ros2#466 which provides some changes on EasyFullControl & FleetUpdateHandle to read in the [task_assignment_strategy] parameter from fleet_config.yaml
• With that PR together with this PR (so in total: rmf_ros2 & rmf_task), we will still need to add a parameter ([task_assignment_strategy]) in fleet_config.yaml to activate it, [task_assignment_strategy] by default is "ShortestFinishTime" if not set.
  • available options: "ShortestFinishTime", "IdlePreferred" .
  • eg. task_assignment_strategy: "IdlePreferred", to put into fleet_config.yaml

GenAI Use

We follow OSRA's policy on GenAI tools

• I used a GenAI tool in this PR.
• I did not use GenAI

Generated-by:

[mxgrey]

Overall I'm not convinced that putting a bias towards robots that are in an idle state is the most meaningful or effective strategy to maximize utilization of the robots. I suspect it might make more sense to use a bias (cost weight) that favors using robots with a higher expected battery level.

That being said, since the approach in this PR is simpler to implement, opt-in, and should be intuitive enough for system integrators to get a desirable behavior, I'm okay with merging this as a core feature. I've just left a few remarks where I think there are opportunities to improve the code.

[mxgrey]

I'm not sure if enum is the right data structure to have here. There's a possibility that some policy options in the future may need additional information, or maybe multiple policy options can be active at once.

I would recommend just adding a new method to Options for

bool prioritize_idle_robots() const;
Options& prioritize_idle_robots(bool choice);

[mxgrey]

I suggest leaving the constructor as-is and just having a method to set prioritize_idle_robots after constructing this class.

[mxgrey]

Style nitpick: always use { ... } around a for-block.

[mxgrey]

Maybe I'm overlooking something, but I feel like it would be simpler to do

std::unordered_set<std::size_t> idle_and_unassigned;
for (std::size_t i = 0; i < initial_sates.size(); ++i)
{
  if (initial_states[i].is_idle() && !has_non_charging(parent->assigned_tasks[i])
  {
    idle_and_unassigned.insert(i);
  }
}

const bool any_idle_still_unassigned = !idle_and_unassigned.empty();

Then later you can use idle_and_unassigned as a list of which robots should be preferred.

[kjchee]

I suspect it might make more sense to use a bias (cost weight) that favors using robots with a higher expected battery level.

Grey, you are referring to using cost system to select the 'best candidate' for the next parent->unassigned_tasks (at here:

rmf_task/rmf_task/src/rmf_task/TaskPlanner.cpp

Line 1024 in b765a66

for (const auto& u : parent->unassigned_tasks)

), correct?

And the cost calculation is based on what user prefer, eg. he prefers higher battery level + idle state robot, then inside the cost calculation, there is some equation like:

r = robot, t=task, w= weight

Cost(r, t) = w1 * FinishTime(r, t)
           + w2 * BatteryPenalty(r)
           + w3 * IdleBonus(r)

[kjchee]

And at the fleet_config, maybe SI just need to set the task_assignment_strategy to idle+higher_battery or idle+fastest or default (fastest)...

Then, inside rmf core, we will auto assign each of the w1,w2,w3 to the strategy they opted? So that, user/SI do not need to touch/set the weightage w1,w2,w3...

[mxgrey]

you are referring to using cost system to select the 'best candidate'

That's right.

And the cost calculation is based on what user prefer

Also correct.

Then, inside rmf core, we will auto assign each of the w1,w2,w3 to the strategy they opted? So that, user/SI do not need to touch/set the weightage w1,w2,w3...

I'm a little ambivalent about this detail. I can see good arguments for not allowing the SI to set the weights directly, but I can also see sound arguments for why they should be able to, since different applications may care about these different factors to varying degrees. If we were to go this route, I would probably allow the weights to be set by the SI, but also provide a catalog of profiles that have preset weight values.

[kjchee]

If we were to go this route, I would probably allow the weights to be set by the SI, but also provide a catalog of profiles that have preset weight values.

sure, flexible enough for all type of SI/deployments.

And the cost calculation is based on what user prefer, eg. he prefers higher battery level + idle state robot, then inside the cost calculation, there is some equation like:

r = robot, t=task, w= weight

Cost(r, t) = w1 * FinishTime(r, t)
           + w2 * BatteryPenalty(r)
           + w3 * IdleBonus(r)

Regarding this draft cost calculation idea, would it be ok to proceed with this direction? Do you have any feedback or suggestions for improvement?

[mxgrey]

Regarding this draft cost calculation idea, would it be ok to proceed with this direction?

If you're okay with taking that approach, I'm happy to support it via code review.

Do you have any feedback or suggestions for improvement?

I think you've captured the idea quite well in your formula. A few additional thoughts that come to mind:

• All the premade weight profiles we provide should set w1 to 1.0 so that the rest of the weights can be thought of in terms of "How much do I want to weigh this factor per second of robot time spent?".
• I wonder if a linear weight is sufficient for the battery penalty. I can imagine users wanting something polynomial so that the closer the battery is to being empty the more drastically we weigh its cost. Instead of linear weights, we could consider giving each cost factor (FinishTime, BatteryPenalty, IdleBonus) a weight vector of std::vector<double> where each element in that vector gets multiplied by pow(x, i) where x is its associated cost factor and i is the index of the weight.

[kjchee]

If you're okay with taking that approach, I'm happy to support it via code review.

yes, i will proceed and update with new commits, thanks for your time for reviewing!

• All the premade weight profiles we provide should set w1 to 1.0 so that the rest of the weights can be thought of in terms of "How much do I want to weigh this factor per second of robot time spent?".

ok, i understand this point.
We always normalise w1 = 1.0, then if:

• Battery penalty w2 = 3.0

  • means we treat a bad battery state as equally bad as 3 extra seconds of robot time

• Idle bonus w3 = -5.0

  • means we value idleness as much as saving 5 seconds of robot time

• Instead of linear weights, we could consider giving each cost factor (FinishTime, BatteryPenalty, IdleBonus) a weight vector of std::vector<double> where each element in that vector gets multiplied by pow(x, i) where x is its associated cost factor and i is the index of the weight.

ok, if i understand correctly, for the example of batteryPenalty:
eg,

weight vector of BatteryPenalty, w2 = [0, 5, 20]
SoC = 0.8
x = (1 - SoC)

BatteryPenalty = w2[0]*pow(x,0) + w2[1]*pow(x,1) + w2[2]*pow(x,2)
               = 0*1 + 5*0.2 + 20*0.04
               = 0.0 + 1.0 + 0.8
               = 1.8

Then,

SoC = 1.0, x = 0.0
BatteryPenalty = 5*0.0 + 20*0.0 = 0.0

SoC = 0.8, x = 0.2
BatteryPenalty = 5*0.2 + 20*0.04 = 1.0 + 0.8 = 1.8

SoC = 0.6, x = 0.4
BatteryPenalty = 5*0.4 + 20*0.16 = 2.0 + 3.2 = 5.2

SoC = 0.4, x = 0.6
BatteryPenalty = 5*0.6 + 20*0.36 = 3.0 + 7.2 = 10.2

SoC = 0.2, x = 0.8
BatteryPenalty = 5*0.8 + 20*0.64 = 4.0 + 12.8 = 16.8

So to achieve this outcome.

[mxgrey]

Battery penalty w2 = 3.0 means we treat a bad battery state as equally bad as 3 extra seconds of robot time

More specifically 3 seconds of robot time per fraction of battery spent. I agree with your representation of x for the battery penalty: vary from 0.0 (no charge spent - full battery) to 1.0 (all charge spent - empty battery).

Idle bonus w3 = -5.0 means we value idleness as much as saving 5 seconds of robot time

I might recommend reframing Idle Bonus to Busy Penalty where x would be 0.0 if the robot has no tasks currently assigned and then 1.0 if it does. Or instead of a 0 - 1 binary, we could consider giving the integer value of however many tasks have been assigned to the robot in the current state of the plan. Or we could let the user specify if they want it to be binary vs a count.

With an assigned task count as the x value, the weight can be positive to indicate more cost for the more busy robots.

[kjchee]

reframing Idle Bonus to Busy Penalty

we could let the user specify if they want it to be binary vs a count.

got it, this is shaping up to be a more complete solution, will start working on it

[kjchee]

@mxgrey, this PR is ready for review, thanks!

I put the newly proposed config at open-rmf/rmf_demos#332. Also, i've updated the rmf_ros2 portion in the original rmf_ros2 PR open-rmf/rmf_ros2#466.

I've tested the following with rmf_demos Office:

1. "DefaultFastest" strategy (original RMF fastest-first approach)

• set in fleet_config:

  task_assignment_strategy:
      profile: "DefaultFastest" 
  

• TinyRobot1 is at "supplies", while TinyRobot2 is at "tinyRobot2_charger"

• send dispatch_task_request:
  ros2 run rmf_demos_tasks dispatch_patrol -p patrol_D1 -n 1 --use_sim_time
ros2 run rmf_demos_tasks dispatch_patrol -p pantry -n 1 --use_sim_time

• can observe TinyRobot2 first being sent to patrol_D1. And later, the 'pantry' task will be assigned to TinyRobot2 as well, because it is the fastest-finish robot.

2. "BatteryAware" strategy (with task efficiency + battery soc in cost calculation)

• set in fleet_config:

  task_assignment_strategy:
      profile: "BatteryAware" 
  

• TinyRobot1 is at "supplies", while TinyRobot2 is at "tinyRobot2_charger"

• I modified a bit in the fleet_adapter.py to permanently change TinyRobot2's battery soc to 0.3, while TinyRobot1 untouched with 1.0 soc.

• send dispatch_task_request:
  ros2 run rmf_demos_tasks dispatch_patrol -p pantry -n 1 --use_sim_time

• can observe TinyRobot1 is assigned the task, even though it is furthest away (from supplies to pantry).

3. "Custom" strategy (with task efficiency + battery soc + heavy penalty in busyness in cost calculation)

• set in fleet_config:

  task_assignment_strategy:
    profile: "Custom" 
    weights: 
      finish_time: [0.0, 1.0]
      battery_penalty: [0.0, 5.0, 20.0]
      busy_penalty: [0.0, 60.0] 
  
    options:
      busy_mode: "Binary"
  

• TinyRobot1 is at "supplies", while TinyRobot2 is at "tinyRobot2_charger"

• send dispatch_task_request:
  ros2 run rmf_demos_tasks dispatch_patrol -p patrol_D1 -n 1 --use_sim_time
ros2 run rmf_demos_tasks dispatch_patrol -p pantry -n 1 --use_sim_time

• can observe TinyRobot2 is assigned the 'patrol_D1' task. Then, the 'pantry' is assigned to TinyRobot1 even though it is furthest. All because TinyRobot2 have busy_penalty of 60.

Also, to check on the actual cost, can uncomment some std::cout in compute_task_cost() & find_lowest_cost_candidates() of TaskPlanner.cpp. I used those printout to verify the cost calculation.