From 26f011ebcf1b10260cc349936d7450789dbaeca2 Mon Sep 17 00:00:00 2001 From: kjchee Date: Fri, 15 Aug 2025 10:42:21 +0800 Subject: [PATCH 01/10] Add idle robot preferred in search node expansion Signed-off-by: kjchee --- rmf_task/include/rmf_task/State.hpp | 6 + rmf_task/include/rmf_task/TaskPlanner.hpp | 19 +- rmf_task/src/rmf_task/State.cpp | 16 ++ rmf_task/src/rmf_task/TaskPlanner.cpp | 172 ++++++++++++++++-- .../src/rmf_task/internal_task_planning.cpp | 11 ++ .../src/rmf_task/internal_task_planning.hpp | 2 + 6 files changed, 209 insertions(+), 17 deletions(-) diff --git a/rmf_task/include/rmf_task/State.hpp b/rmf_task/include/rmf_task/State.hpp index 0770fd20..0d6001e6 100644 --- a/rmf_task/include/rmf_task/State.hpp +++ b/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 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 diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index 0da5b440..4d0f502b 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -104,10 +104,21 @@ 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] idle_robot_preferred + /// If true, the planner will prioritize assigning tasks to idle robots + /// when expanding search nodes, rather than selecting solely based on the + /// shortest estimated finish time. + /// This helps fleets with many idle robots avoid overloading a single robot + /// simply because it can finish tasks sooner. + /// If all robots are idle or all are busy, the planner falls back to the + /// default behavior of selecting the shortest finish time. + /// Options( bool greedy, std::function interrupter = nullptr, - ConstRequestFactoryPtr finishing_request = nullptr); + ConstRequestFactoryPtr finishing_request = nullptr, + bool idle_robot_preferred = false); /// Set whether a greedy approach should be used Options& greedy(bool value); @@ -128,6 +139,12 @@ class TaskPlanner /// Get the request factory that will generate a finishing task ConstRequestFactoryPtr finishing_request() const; + /// Set whether idle robots are preferred for task assignments + Options& idle_robot_preferred(bool value); + + /// Get whether idle robots are preferred for task assignments + bool idle_robot_preferred() const; + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_task/src/rmf_task/State.cpp b/rmf_task/src/rmf_task/State.cpp index 26bc6337..342c0f75 100644 --- a/rmf_task/src/rmf_task/State.cpp +++ b/rmf_task/src/rmf_task/State.cpp @@ -169,4 +169,20 @@ std::optional 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()) + return idle->value; + + return false; +} + +//============================================================================== +State& State::idle(bool is_idle) +{ + with(is_idle); + return *this; +} + } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index 21b871ca..b54ffce6 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -106,18 +106,21 @@ class TaskPlanner::Options::Implementation bool greedy; std::function interrupter; ConstRequestFactoryPtr finishing_request; + bool idle_robot_preferred; }; //============================================================================== TaskPlanner::Options::Options( bool greedy, std::function interrupter, - ConstRequestFactoryPtr finishing_request) + ConstRequestFactoryPtr finishing_request, + bool idle_robot_preferred) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(greedy), std::move(interrupter), - std::move(finishing_request) + std::move(finishing_request), + std::move(idle_robot_preferred) })) { // Do nothing @@ -164,6 +167,19 @@ ConstRequestFactoryPtr TaskPlanner::Options::finishing_request() const return _pimpl->finishing_request; } +//============================================================================== +auto TaskPlanner::Options::idle_robot_preferred(bool value) -> Options& +{ + _pimpl->idle_robot_preferred = value; + return *this; +} + +//============================================================================== +bool TaskPlanner::Options::idle_robot_preferred() const +{ + return _pimpl->idle_robot_preferred; +} + //============================================================================== class TaskPlanner::Assignment::Implementation { @@ -508,13 +524,26 @@ class TaskPlanner::Implementation } } + static bool has_non_charging(const std::vector& 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& initial_states, const std::vector& requests, const std::function interrupter, ConstRequestFactoryPtr finishing_request, - bool greedy) + bool greedy, + bool idle_robot_preferred) { cost_calculator = config.cost_calculator() ? config.cost_calculator() : @@ -546,7 +575,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, idle_robot_preferred); if (!node) { @@ -1016,19 +1045,127 @@ class TaskPlanner::Implementation ConstNodePtr parent, Filter& filter, const std::vector& initial_states, - rmf_traffic::Time time_now) + rmf_traffic::Time time_now, + bool idle_robot_preferred) { std::vector new_nodes; new_nodes.reserve( - parent->unassigned_tasks.size() + parent->assigned_tasks.size()); - for (const auto& u : parent->unassigned_tasks) + parent->unassigned_tasks.size() + parent->assigned_tasks.size()); + + if (!idle_robot_preferred) // default best candidates strategy (shortest finish time) { - const auto& range = u.second.candidates.best_candidates(); - for (auto it = range.begin; it != range.end; it++) + 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 new_node = expand_candidate( + it, u, parent, &filter, time_now)) + new_nodes.push_back(std::move(new_node)); + } + } + } + + else // idle_robot_preferred style of node expansion + { + // is this index an initially-idle robot and now still has no non-charging assignment? + std::vector 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; } + ); + + // Start to expand each unassigned task + for (const auto& u : parent->unassigned_tasks) { - if (auto new_node = expand_candidate( - it, u, parent, &filter, time_now)) - new_nodes.push_back(std::move(new_node)); + const auto& range = u.second.candidates.best_candidates(); + + // 1) Check if there is any idle-and-still-unassigned candidate inside best_candidates list + + // is there any idle candidate in the best candidates list? + bool has_idle_in_best_candidates_range = false; + if (any_idle_still_unassigned) + { + for (auto it = range.begin; it != range.end; ++it) + { + const std::size_t c = it->second.candidate; + if (idle_and_unassigned[c]) + { + has_idle_in_best_candidates_range = true; + break; + } + } + } + + // 2) If present in best_candidates list, + // prune everyone else and expand only those idle-unassigned candidates + + if (any_idle_still_unassigned && has_idle_in_best_candidates_range) + { + for (auto it = range.begin; it != range.end; ++it) + { + const std::size_t c = it->second.candidate; + if (!idle_and_unassigned[c]) + continue; + + if (auto new_node = expand_candidate(it, u, parent, &filter, time_now)) + new_nodes.push_back(std::move(new_node)); + } + } + else + { + // 3) No idle candidate exists in best_candidates. + // Next we try all other idle-and-still-unassigned agents directly + + bool added_idle_child = false; + if (any_idle_still_unassigned) + { + auto all_candidates = u.second.candidates.all_candidates(); + for (auto it = all_candidates.begin; it != all_candidates.end; ++it) + { + // Check if 'it' is best_candidates(), then exclude it + bool is_in_best_candidates = false; + for (auto best_it = range.begin; best_it != range.end; ++best_it) + { + if (best_it->second.candidate == it->second.candidate) + { + is_in_best_candidates = true; + break; + } + } + + if (is_in_best_candidates) + continue; + + 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)); + added_idle_child = true; + } + } + } + + // 4) If none of the idle agents node added (no idle robots or + // all initially idle robots already assigned), + // we fall back to default Best Candidates expansion method + + if (!added_idle_child) + { + for (auto it = range.begin; it != range.end; ++it) + { + if (auto new_node = expand_candidate(it, u, parent, &filter, time_now)) + new_nodes.push_back(std::move(new_node)); + } + } + } } } @@ -1064,7 +1201,8 @@ class TaskPlanner::Implementation const std::vector& initial_states, const std::size_t num_tasks, rmf_traffic::Time time_now, - std::function interrupter) + std::function interrupter, + bool idle_robot_preferred) { using PriorityQueue = std::priority_queue< ConstNodePtr, @@ -1092,7 +1230,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, idle_robot_preferred); // Add copies and with a newly assigned task to queue for (const auto& n : new_nodes) @@ -1147,7 +1285,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.idle_robot_preferred()); } // ============================================================================ @@ -1163,7 +1302,8 @@ auto TaskPlanner::plan( requests, options.interrupter(), options.finishing_request(), - options.greedy()); + options.greedy(), + options.idle_robot_preferred()); } // ============================================================================ diff --git a/rmf_task/src/rmf_task/internal_task_planning.cpp b/rmf_task/src/rmf_task/internal_task_planning.cpp index fffd8a96..5db9e17c 100644 --- a/rmf_task/src/rmf_task/internal_task_planning.cpp +++ b/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) { diff --git a/rmf_task/src/rmf_task/internal_task_planning.hpp b/rmf_task/src/rmf_task/internal_task_planning.hpp index 07135aa5..ffa6466c 100644 --- a/rmf_task/src/rmf_task/internal_task_planning.hpp +++ b/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( From 805950f8e8d89ce6072f41fe7873b90793075e0b Mon Sep 17 00:00:00 2001 From: kjchee Date: Fri, 15 Aug 2025 11:26:59 +0800 Subject: [PATCH 02/10] make style happy Signed-off-by: kjchee --- rmf_task/include/rmf_task/TaskPlanner.hpp | 2 +- rmf_task/src/rmf_task/TaskPlanner.cpp | 31 +++++++++++++---------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index 4d0f502b..d92853d0 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -104,7 +104,7 @@ 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] idle_robot_preferred /// If true, the planner will prioritize assigning tasks to idle robots /// when expanding search nodes, rather than selecting solely based on the diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index b54ffce6..2e9df7e2 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -524,13 +524,14 @@ class TaskPlanner::Implementation } } - static bool has_non_charging(const std::vector& assignments) + static bool has_non_charging( + const std::vector& assignments) { for (const auto& a : assignments) { if (!std::dynamic_pointer_cast< - const rmf_task::requests::ChargeBattery::Description>( - a.assignment.request()->description())) + const rmf_task::requests::ChargeBattery::Description>( + a.assignment.request()->description())) return true; } return false; @@ -1050,8 +1051,7 @@ class TaskPlanner::Implementation { std::vector new_nodes; new_nodes.reserve( - parent->unassigned_tasks.size() + parent->assigned_tasks.size()); - + parent->unassigned_tasks.size() + parent->assigned_tasks.size()); if (!idle_robot_preferred) // default best candidates strategy (shortest finish time) { for (const auto& u : parent->unassigned_tasks) @@ -1071,7 +1071,8 @@ class TaskPlanner::Implementation // is this index an initially-idle robot and now still has no non-charging assignment? std::vector 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]); + 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( @@ -1095,14 +1096,14 @@ class TaskPlanner::Implementation { const std::size_t c = it->second.candidate; if (idle_and_unassigned[c]) - { - has_idle_in_best_candidates_range = true; - break; + { + has_idle_in_best_candidates_range = true; + break; } } } - // 2) If present in best_candidates list, + // 2) If present in best_candidates list, // prune everyone else and expand only those idle-unassigned candidates if (any_idle_still_unassigned && has_idle_in_best_candidates_range) @@ -1112,8 +1113,8 @@ class TaskPlanner::Implementation const std::size_t c = it->second.candidate; if (!idle_and_unassigned[c]) continue; - - 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)); } } @@ -1145,7 +1146,8 @@ class TaskPlanner::Implementation if (!idle_and_unassigned[it->second.candidate]) continue; - 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)); added_idle_child = true; @@ -1161,7 +1163,8 @@ class TaskPlanner::Implementation { 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)); } } From eff464371561283cebf071f5f1b3da9303f83d23 Mon Sep 17 00:00:00 2001 From: kjchee Date: Fri, 22 Aug 2025 10:12:35 +0800 Subject: [PATCH 03/10] Add IdlePreferred expansion policy for task assignment Signed-off-by: kjchee --- rmf_task/include/rmf_task/TaskPlanner.hpp | 39 ++-- rmf_task/src/rmf_task/TaskPlanner.cpp | 242 +++++++++++----------- 2 files changed, 152 insertions(+), 129 deletions(-) diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index d92853d0..dbffec68 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/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 @@ -105,20 +123,15 @@ class TaskPlanner /// A request factory that generates a tailored task for each agent/AGV /// to perform at the end of their assignments /// - /// \param[in] idle_robot_preferred - /// If true, the planner will prioritize assigning tasks to idle robots - /// when expanding search nodes, rather than selecting solely based on the - /// shortest estimated finish time. - /// This helps fleets with many idle robots avoid overloading a single robot - /// simply because it can finish tasks sooner. - /// If all robots are idle or all are busy, the planner falls back to the - /// default behavior of selecting the shortest finish time. + /// \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 interrupter = nullptr, ConstRequestFactoryPtr finishing_request = nullptr, - bool idle_robot_preferred = false); + ExpansionPolicy expansion_policy = ExpansionPolicy::ShortestFinishTime); /// Set whether a greedy approach should be used Options& greedy(bool value); @@ -139,11 +152,11 @@ class TaskPlanner /// Get the request factory that will generate a finishing task ConstRequestFactoryPtr finishing_request() const; - /// Set whether idle robots are preferred for task assignments - Options& idle_robot_preferred(bool value); + /// Set the expansion policy for task assignments + Options& expansion_policy(ExpansionPolicy policy); - /// Get whether idle robots are preferred for task assignments - bool idle_robot_preferred() const; + /// Get the expansion policy for task assignments + ExpansionPolicy expansion_policy() const; class Implementation; private: diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index 2e9df7e2..8feb74f3 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -28,6 +28,9 @@ #include #include +#include +#include + namespace rmf_task { //============================================================================== @@ -106,7 +109,7 @@ class TaskPlanner::Options::Implementation bool greedy; std::function interrupter; ConstRequestFactoryPtr finishing_request; - bool idle_robot_preferred; + ExpansionPolicy expansion_policy = ExpansionPolicy::ShortestFinishTime; }; //============================================================================== @@ -114,13 +117,13 @@ TaskPlanner::Options::Options( bool greedy, std::function interrupter, ConstRequestFactoryPtr finishing_request, - bool idle_robot_preferred) + ExpansionPolicy expansion_policy) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(greedy), std::move(interrupter), std::move(finishing_request), - std::move(idle_robot_preferred) + std::move(expansion_policy) })) { // Do nothing @@ -168,16 +171,16 @@ ConstRequestFactoryPtr TaskPlanner::Options::finishing_request() const } //============================================================================== -auto TaskPlanner::Options::idle_robot_preferred(bool value) -> Options& +auto TaskPlanner::Options::expansion_policy(ExpansionPolicy policy) -> Options& { - _pimpl->idle_robot_preferred = value; + _pimpl->expansion_policy = policy; return *this; } //============================================================================== -bool TaskPlanner::Options::idle_robot_preferred() const +TaskPlanner::ExpansionPolicy TaskPlanner::Options::expansion_policy() const { - return _pimpl->idle_robot_preferred; + return _pimpl->expansion_policy; } //============================================================================== @@ -544,7 +547,7 @@ class TaskPlanner::Implementation const std::function interrupter, ConstRequestFactoryPtr finishing_request, bool greedy, - bool idle_robot_preferred) + ExpansionPolicy expansion_policy) { cost_calculator = config.cost_calculator() ? config.cost_calculator() : @@ -576,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, idle_robot_preferred); + requests.size(), time_now, interrupter, expansion_policy); if (!node) { @@ -1042,133 +1045,140 @@ class TaskPlanner::Implementation return node; } - std::vector expand( + // Expand using default best_candidates() strategy (shortest finish time) + std::vector expand_shortest_finish_time_robots( ConstNodePtr parent, Filter& filter, - const std::vector& initial_states, - rmf_traffic::Time time_now, - bool idle_robot_preferred) + rmf_traffic::Time time_now) { - std::vector new_nodes; - new_nodes.reserve( - parent->unassigned_tasks.size() + parent->assigned_tasks.size()); - if (!idle_robot_preferred) // default best candidates strategy (shortest finish time) + std::vector nodes; + for (const auto& u : parent->unassigned_tasks) { - 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 = u.second.candidates.best_candidates(); - for (auto it = range.begin; it != range.end; it++) - { - if (auto new_node = expand_candidate( - it, u, parent, &filter, time_now)) - new_nodes.push_back(std::move(new_node)); - } + if (auto node = expand_candidate(it, u, parent, &filter, time_now)) + nodes.push_back(std::move(node)); } } + return nodes; + } - else // idle_robot_preferred style of node expansion + // 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 expand_fastest_idle_robots( + ConstNodePtr parent, + Filter& filter, + const std::vector& initial_states, + rmf_traffic::Time time_now) + { + std::vector new_nodes; + + // is this index an initially-idle robot and now still has no non-charging assignment? + std::vector 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) { - // is this index an initially-idle robot and now still has no non-charging assignment? - std::vector 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; } - ); - - // Start to expand each unassigned task - for (const auto& u : parent->unassigned_tasks) + if (any_idle_still_unassigned) { - const auto& range = u.second.candidates.best_candidates(); + // 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) - // 1) Check if there is any idle-and-still-unassigned candidate inside best_candidates list + 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(); - // is there any idle candidate in the best candidates list? - bool has_idle_in_best_candidates_range = false; - if (any_idle_still_unassigned) + for (auto it = all_candidates.begin; it != all_candidates.end; ++it) { - for (auto it = range.begin; it != range.end; ++it) - { - const std::size_t c = it->second.candidate; - if (idle_and_unassigned[c]) - { - has_idle_in_best_candidates_range = true; - break; - } - } - } + // 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; - // 2) If present in best_candidates list, - // prune everyone else and expand only those idle-unassigned candidates + // skip if not idle-and-still-unassigned + if (!idle_and_unassigned[it->second.candidate]) + continue; - if (any_idle_still_unassigned && has_idle_in_best_candidates_range) - { - for (auto it = range.begin; it != range.end; ++it) + if (auto new_node = + expand_candidate(it, u, parent, &filter, time_now)) { - const std::size_t c = it->second.candidate; - if (!idle_and_unassigned[c]) - continue; - if (auto new_node = expand_candidate( - it, u, parent, &filter, time_now)) - new_nodes.push_back(std::move(new_node)); + new_nodes.push_back(std::move(new_node)); + if (!added_any_idle_child) + added_child_finish_time = it->first; + added_any_idle_child = true; } } - else - { - // 3) No idle candidate exists in best_candidates. - // Next we try all other idle-and-still-unassigned agents directly - bool added_idle_child = false; - if (any_idle_still_unassigned) - { - auto all_candidates = u.second.candidates.all_candidates(); - for (auto it = all_candidates.begin; it != all_candidates.end; ++it) - { - // Check if 'it' is best_candidates(), then exclude it - bool is_in_best_candidates = false; - for (auto best_it = range.begin; best_it != range.end; ++best_it) - { - if (best_it->second.candidate == it->second.candidate) - { - is_in_best_candidates = true; - break; - } - } + // 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; + } - if (is_in_best_candidates) - 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. - if (!idle_and_unassigned[it->second.candidate]) - continue; + const auto& range = u.second.candidates.best_candidates(); - if (auto new_node = expand_candidate( - it, u, parent, &filter, time_now)) - { - new_nodes.push_back(std::move(new_node)); - added_idle_child = true; - } - } - } + for (auto it = range.begin; it != range.end; ++it) + { + if (auto new_node = + expand_candidate(it, u, parent, &filter, time_now)) + new_nodes.push_back(std::move(new_node)); + } + } - // 4) If none of the idle agents node added (no idle robots or - // all initially idle robots already assigned), - // we fall back to default Best Candidates expansion method + return new_nodes; + } - if (!added_idle_child) - { - for (auto it = range.begin; it != range.end; ++it) - { - if (auto new_node = expand_candidate( - it, u, parent, &filter, time_now)) - new_nodes.push_back(std::move(new_node)); - } - } - } + std::vector expand( + ConstNodePtr parent, + Filter& filter, + const std::vector& initial_states, + rmf_traffic::Time time_now, + ExpansionPolicy expansion_policy) + { + std::vector 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; } } @@ -1205,7 +1215,7 @@ class TaskPlanner::Implementation const std::size_t num_tasks, rmf_traffic::Time time_now, std::function interrupter, - bool idle_robot_preferred) + ExpansionPolicy expansion_policy) { using PriorityQueue = std::priority_queue< ConstNodePtr, @@ -1233,7 +1243,7 @@ class TaskPlanner::Implementation // Apply possible actions to expand the node const auto new_nodes = expand( - top, filter, initial_states, time_now, idle_robot_preferred); + top, filter, initial_states, time_now, expansion_policy); // Add copies and with a newly assigned task to queue for (const auto& n : new_nodes) @@ -1289,7 +1299,7 @@ auto TaskPlanner::plan( _pimpl->default_options.interrupter(), _pimpl->default_options.finishing_request(), _pimpl->default_options.greedy(), - _pimpl->default_options.idle_robot_preferred()); + _pimpl->default_options.expansion_policy()); } // ============================================================================ @@ -1306,7 +1316,7 @@ auto TaskPlanner::plan( options.interrupter(), options.finishing_request(), options.greedy(), - options.idle_robot_preferred()); + options.expansion_policy()); } // ============================================================================ From 185be237fae025db126e2eeaed1c810efc38e793 Mon Sep 17 00:00:00 2001 From: kjchee Date: Fri, 26 Sep 2025 18:18:28 +0800 Subject: [PATCH 04/10] Introduce cost-based task assignment strategy Signed-off-by: kjchee --- rmf_task/include/rmf_task/TaskPlanner.hpp | 97 +++++--- rmf_task/src/rmf_task/TaskPlanner.cpp | 261 +++++++++++----------- 2 files changed, 198 insertions(+), 160 deletions(-) diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index dbffec68..ab3306c3 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -39,22 +39,71 @@ 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 + /// 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 { - ShortestFinishTime = 0, - IdlePreferred = 1 + enum class Profile + { + DefaultFastest, + BatteryAware, + Custom + }; + + struct Weights + { + std::vector finish_time; + std::vector battery_penalty; + std::vector 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 @@ -122,16 +171,10 @@ 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 interrupter = nullptr, - ConstRequestFactoryPtr finishing_request = nullptr, - ExpansionPolicy expansion_policy = ExpansionPolicy::ShortestFinishTime); + ConstRequestFactoryPtr finishing_request = nullptr); /// Set whether a greedy approach should be used Options& greedy(bool value); @@ -152,11 +195,13 @@ 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); + /// Set the task assignment strategy (profile & custom weights) + /// used by the planner + Options& task_assignment_strategy(TaskAssignmentStrategy strategy); - /// Get the expansion policy for task assignments - ExpansionPolicy expansion_policy() const; + /// Get the task assignment strategy (profile & custom weights) + /// used by the planner + TaskAssignmentStrategy task_assignment_strategy() const; class Implementation; private: diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index 8feb74f3..5eb74f8e 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -28,9 +28,6 @@ #include #include -#include -#include - namespace rmf_task { //============================================================================== @@ -109,21 +106,20 @@ class TaskPlanner::Options::Implementation bool greedy; std::function interrupter; ConstRequestFactoryPtr finishing_request; - ExpansionPolicy expansion_policy = ExpansionPolicy::ShortestFinishTime; + TaskAssignmentStrategy assignment_strategy; }; //============================================================================== TaskPlanner::Options::Options( bool greedy, std::function interrupter, - ConstRequestFactoryPtr finishing_request, - ExpansionPolicy expansion_policy) + ConstRequestFactoryPtr finishing_request) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(greedy), std::move(interrupter), std::move(finishing_request), - std::move(expansion_policy) + TaskAssignmentStrategy::make(TaskAssignmentStrategy::Profile::DefaultFastest) })) { // Do nothing @@ -171,16 +167,18 @@ ConstRequestFactoryPtr TaskPlanner::Options::finishing_request() const } //============================================================================== -auto TaskPlanner::Options::expansion_policy(ExpansionPolicy policy) -> Options& +auto TaskPlanner::Options::task_assignment_strategy( + TaskAssignmentStrategy strategy) -> Options& { - _pimpl->expansion_policy = policy; + _pimpl->assignment_strategy = strategy; return *this; } //============================================================================== -TaskPlanner::ExpansionPolicy TaskPlanner::Options::expansion_policy() const +TaskPlanner::TaskAssignmentStrategy +TaskPlanner::Options::task_assignment_strategy() const { - return _pimpl->expansion_policy; + return _pimpl->assignment_strategy; } //============================================================================== @@ -527,19 +525,6 @@ class TaskPlanner::Implementation } } - static bool has_non_charging( - const std::vector& 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& initial_states, @@ -547,9 +532,8 @@ class TaskPlanner::Implementation const std::function interrupter, ConstRequestFactoryPtr finishing_request, bool greedy, - ExpansionPolicy expansion_policy) + TaskAssignmentStrategy task_assignment_strategy) { - cost_calculator = config.cost_calculator() ? config.cost_calculator() : rmf_task::BinaryPriorityScheme::make_cost_calculator(); @@ -579,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, expansion_policy); + requests.size(), time_now, interrupter, task_assignment_strategy); if (!node) { @@ -1045,107 +1029,123 @@ class TaskPlanner::Implementation return node; } - // Expand using default best_candidates() strategy (shortest finish time) - std::vector expand_shortest_finish_time_robots( - ConstNodePtr parent, - Filter& filter, - rmf_traffic::Time time_now) + double poly_eval(const std::vector& coeffs, double x) { - std::vector nodes; - for (const auto& u : parent->unassigned_tasks) + double sum = 0.0; + for (std::size_t i = 0; i < coeffs.size(); ++i) { - 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)); - } + sum += coeffs[i] * std::pow(x, static_cast(i)); } - return nodes; + return sum; } - // 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 expand_fastest_idle_robots( - ConstNodePtr parent, - Filter& filter, - const std::vector& initial_states, - rmf_traffic::Time time_now) + double compute_task_cost( + double finish_time, + double soc, + int busy_count, + const TaskPlanner::TaskAssignmentStrategy& task_assignment_strategy) { - std::vector new_nodes; - - // is this index an initially-idle robot and now still has no non-charging assignment? - std::vector 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) + // 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) { - 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; + 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(busy_count); + } - // skip if not idle-and-still-unassigned - if (!idle_and_unassigned[it->second.candidate]) - continue; + double busy_penalty = + poly_eval(task_assignment_strategy.weights.busy_penalty, busy_x); - 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; - } - } + // Final cost + double total_cost = finish_time_cost + battery_penalty + busy_penalty; - // 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; - } + // std::cout << "task_assignment_strategy: " + // << static_cast(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; - // 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. + return total_cost; + } - const auto& range = u.second.candidates.best_candidates(); + // Returns the iterator of the lowest cost candidates + // calculated using the provided strategy + std::vector find_lowest_cost_candidates( + const Candidates::Range& all_candidates, + const Node& parent, + const std::vector& initial_states, + rmf_traffic::Time time_now, + const TaskPlanner::TaskAssignmentStrategy& task_assignment_strategy) + { + std::vector lowest_cost_candidates; + double best_cost = std::numeric_limits::infinity(); - for (auto it = range.begin; it != range.end; ++it) + 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) { - if (auto new_node = - expand_candidate(it, u, parent, &filter, time_now)) - new_nodes.push_back(std::move(new_node)); + 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 new_nodes; + return lowest_cost_candidates; } std::vector expand( @@ -1153,32 +1153,25 @@ class TaskPlanner::Implementation Filter& filter, const std::vector& initial_states, rmf_traffic::Time time_now, - ExpansionPolicy expansion_policy) + TaskAssignmentStrategy task_assignment_strategy) { std::vector new_nodes; new_nodes.reserve( parent->unassigned_tasks.size() + parent->assigned_tasks.size()); - - // Task-assignment node expansions based on policy - switch (expansion_policy) + for (const auto& u : parent->unassigned_tasks) { - 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; - } + const auto& range = find_lowest_cost_candidates( + u.second.candidates.all_candidates(), + *parent, + initial_states, + time_now, + task_assignment_strategy); - case ExpansionPolicy::IdlePreferred: + for (const auto& it : range) { - 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; + if (auto new_node = expand_candidate( + it, u, parent, &filter, time_now)) + new_nodes.push_back(std::move(new_node)); } } @@ -1215,7 +1208,7 @@ class TaskPlanner::Implementation const std::size_t num_tasks, rmf_traffic::Time time_now, std::function interrupter, - ExpansionPolicy expansion_policy) + TaskAssignmentStrategy task_assignment_strategy) { using PriorityQueue = std::priority_queue< ConstNodePtr, @@ -1243,7 +1236,7 @@ class TaskPlanner::Implementation // Apply possible actions to expand the node const auto new_nodes = expand( - top, filter, initial_states, time_now, expansion_policy); + 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) @@ -1299,7 +1292,7 @@ auto TaskPlanner::plan( _pimpl->default_options.interrupter(), _pimpl->default_options.finishing_request(), _pimpl->default_options.greedy(), - _pimpl->default_options.expansion_policy()); + _pimpl->default_options.task_assignment_strategy()); } // ============================================================================ @@ -1316,7 +1309,7 @@ auto TaskPlanner::plan( options.interrupter(), options.finishing_request(), options.greedy(), - options.expansion_policy()); + options.task_assignment_strategy()); } // ============================================================================ From deaf94c0bf4e1dd38ca8a58d2f85fbe8f277400c Mon Sep 17 00:00:00 2001 From: kjchee Date: Mon, 17 Nov 2025 09:53:58 +0800 Subject: [PATCH 05/10] Improve busy_count documentation for assignment logic clarity Signed-off-by: kjchee --- rmf_task/src/rmf_task/TaskPlanner.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index 5eb74f8e..52d22ca7 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -1109,12 +1109,13 @@ class TaskPlanner::Implementation 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. + // we count its current task towards its "busy count". After that, we + // add on any new tasks that have been assigned to this robot so far + // in the planning process. + // + // We do not need to consider tasks that had been queued up for + // this robot prior to this round of planning because those tasks will + // be reassigned based on the outcome of this plan generation. 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; From dc27b7f97763c47fa7bc306be02d0c83c4e061c5 Mon Sep 17 00:00:00 2001 From: kjchee Date: Mon, 17 Nov 2025 10:04:45 +0800 Subject: [PATCH 06/10] Return const reference for task_assignment_strategy() Signed-off-by: kjchee --- rmf_task/include/rmf_task/TaskPlanner.hpp | 2 +- rmf_task/src/rmf_task/TaskPlanner.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index ab3306c3..06df9426 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -201,7 +201,7 @@ class TaskPlanner /// Get the task assignment strategy (profile & custom weights) /// used by the planner - TaskAssignmentStrategy task_assignment_strategy() const; + const TaskAssignmentStrategy& task_assignment_strategy() const; class Implementation; private: diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index 52d22ca7..4e226324 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -175,7 +175,7 @@ auto TaskPlanner::Options::task_assignment_strategy( } //============================================================================== -TaskPlanner::TaskAssignmentStrategy +const TaskPlanner::TaskAssignmentStrategy& TaskPlanner::Options::task_assignment_strategy() const { return _pimpl->assignment_strategy; From 0de02fd253726919d0e07dfd39e0baf573f496f6 Mon Sep 17 00:00:00 2001 From: kjchee Date: Mon, 17 Nov 2025 10:34:56 +0800 Subject: [PATCH 07/10] Convert TaskAssignmentStrategy to full pimpl class for API/ABI stability Signed-off-by: kjchee --- rmf_task/include/rmf_task/TaskPlanner.hpp | 112 +++++++++--------- rmf_task/src/rmf_task/TaskPlanner.cpp | 133 ++++++++++++++++++++-- 2 files changed, 179 insertions(+), 66 deletions(-) diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index 06df9426..75b53387 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -39,71 +39,71 @@ 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 + /// The TaskAssignmentStrategy class contains the various profiles and + /// their associated weights for cost calculation. + class TaskAssignmentStrategy { - enum class Profile + public: + + /// Predefined profiles that initialize the strategy with + /// pre-defined weights and options. + enum class Profile : uint32_t { - DefaultFastest, + /// Standard RMF assignment strategy with fastest-first approach + DefaultFastest = 0, + + /// Prioritize battery level, strongly penalize low SOC with a quadratic term. + /// Still account for task efficiency (fastest-first), but ignore busyness. BatteryAware, - Custom - }; - struct Weights - { - std::vector finish_time; - std::vector battery_penalty; - std::vector busy_penalty; + /// To be overwritten from fleet_config.yaml + Unset }; - - struct Options + + /// Options for computing the busyness penalty. + enum class BusyMode : uint8_t { - enum class BusyMode - { - Binary, - Count - }; - BusyMode busy_mode = BusyMode::Binary; + /// Mode where busyness penalty is 0 if idle, else 1 + Binary = 0, + + /// Mode where busyness penalty is based on task count + Count }; - Profile profile = Profile::DefaultFastest; - Weights weights; - Options options; + /// Constructor + TaskAssignmentStrategy(); - 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; - } + /// Make a strategy initialized from a predefined profile + static TaskAssignmentStrategy make(Profile profile); + + /// Set the finish-time polynomial weights + TaskAssignmentStrategy& finish_time_weights(std::vector values); + + /// Get the finish-time polynomial weights + const std::vector& finish_time_weights() const; + + /// Set the battery penalty polynomial weights + TaskAssignmentStrategy& battery_penalty_weights(std::vector values); + + /// Get the battery penalty polynomial weights + const std::vector& battery_penalty_weights() const; + + /// Set the busy penalty polynomial weights + TaskAssignmentStrategy& busy_penalty_weights(std::vector values); + + /// Get the busy penalty polynomial weights + const std::vector& busy_penalty_weights() const; + + /// Set the busyness penalty mode + TaskAssignmentStrategy& busy_mode(BusyMode mode); + + /// Get the busyness penalty mode + BusyMode busy_mode() const; + + class Implementation; + + private: + rmf_utils::impl_ptr _pimpl; }; /// The Configuration class contains planning parameters that are immutable diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index 4e226324..de4f173f 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -30,6 +30,122 @@ namespace rmf_task { +//============================================================================== +class TaskPlanner::TaskAssignmentStrategy::Implementation +{ +public: + std::vector finish_time; + std::vector battery_penalty; + std::vector busy_penalty; + BusyMode busy_mode = BusyMode::Binary; +}; + +//============================================================================== +TaskPlanner::TaskAssignmentStrategy::TaskAssignmentStrategy() +: _pimpl(rmf_utils::make_impl()) +{ + // Default-constructed strategy has no preset weights. + // TaskPlanner::Options::Options() applies DefaultFastest unless user overrides + // via setters. +} + +//============================================================================== +auto TaskPlanner::TaskAssignmentStrategy::make(Profile profile) + -> TaskAssignmentStrategy +{ + TaskAssignmentStrategy strategy; + + switch (profile) + { + case Profile::DefaultFastest: + // Default RMF assignment strategy with fastest-first approach + strategy.finish_time_weights({0.0, 1.0}); + strategy.battery_penalty_weights({}); + strategy.busy_penalty_weights({}); + strategy.busy_mode(BusyMode::Binary); + 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. + strategy.finish_time_weights({0.0, 1.0}); + strategy.battery_penalty_weights({0.0, 20.0, 60.0}); + strategy.busy_penalty_weights({}); + strategy.busy_mode(BusyMode::Binary); + break; + + case Profile::Unset: + // To be overwritten from fleet_config.yaml, + // user must fill using setters + break; + + default: + // Default to DefaultFastest + return make(Profile::DefaultFastest); + } + return strategy; +} + +//============================================================================== +const std::vector& + TaskPlanner::TaskAssignmentStrategy::finish_time_weights() const +{ + return _pimpl->finish_time; +} + +//============================================================================== +auto TaskPlanner::TaskAssignmentStrategy::finish_time_weights( + std::vector values) -> TaskAssignmentStrategy& +{ + _pimpl->finish_time = std::move(values); + return *this; +} + +//============================================================================== +const std::vector& + TaskPlanner::TaskAssignmentStrategy::battery_penalty_weights() const +{ + return _pimpl->battery_penalty; +} + +//============================================================================== +auto TaskPlanner::TaskAssignmentStrategy::battery_penalty_weights( + std::vector values) -> TaskAssignmentStrategy& +{ + _pimpl->battery_penalty = std::move(values); + return *this; +} + +//============================================================================== +const std::vector& + TaskPlanner::TaskAssignmentStrategy::busy_penalty_weights() const +{ + return _pimpl->busy_penalty; +} + +//============================================================================== +auto TaskPlanner::TaskAssignmentStrategy::busy_penalty_weights( + std::vector values) -> TaskAssignmentStrategy& +{ + _pimpl->busy_penalty = std::move(values); + return *this; +} + +//============================================================================== +TaskPlanner::TaskAssignmentStrategy::BusyMode + TaskPlanner::TaskAssignmentStrategy::busy_mode() const +{ + return _pimpl->busy_mode; +} + +//============================================================================== +auto TaskPlanner::TaskAssignmentStrategy::busy_mode(BusyMode mode) + -> TaskAssignmentStrategy& +{ + _pimpl->busy_mode = mode; + return *this; +} + //============================================================================== class TaskPlanner::Configuration::Implementation { @@ -1047,36 +1163,33 @@ class TaskPlanner::Implementation { // Finish time cost double finish_time_cost = - poly_eval(task_assignment_strategy.weights.finish_time, finish_time); + poly_eval(task_assignment_strategy.finish_time_weights(), finish_time); // Battery penalty double battery_x = 1.0 - soc; double battery_penalty = - poly_eval(task_assignment_strategy.weights.battery_penalty, battery_x); + poly_eval(task_assignment_strategy.battery_penalty_weights(), 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) + if (task_assignment_strategy.busy_mode() == + TaskPlanner::TaskAssignmentStrategy::BusyMode::Binary) { busy_x = (busy_count > 0 ? 1.0 : 0.0); } - else if (task_assignment_strategy.options.busy_mode == - TaskPlanner::TaskAssignmentStrategy::Options::BusyMode::Count) + else if (task_assignment_strategy.busy_mode() == + TaskPlanner::TaskAssignmentStrategy::BusyMode::Count) { busy_x = static_cast(busy_count); } double busy_penalty = - poly_eval(task_assignment_strategy.weights.busy_penalty, busy_x); + poly_eval(task_assignment_strategy.busy_penalty_weights(), busy_x); // Final cost double total_cost = finish_time_cost + battery_penalty + busy_penalty; - // std::cout << "task_assignment_strategy: " - // << static_cast(task_assignment_strategy.profile) - // << std::endl; // std::cout << " In compute_task_cost(): \n" // << " finish_time_cost: " << finish_time_cost // << ", battery_penalty: " << battery_penalty From e368e907cacde3a8f5035e89da4782522a851d2e Mon Sep 17 00:00:00 2001 From: kjchee Date: Mon, 17 Nov 2025 10:46:30 +0800 Subject: [PATCH 08/10] Remove constant offsets that do not affect cost minimization Signed-off-by: kjchee --- rmf_task/src/rmf_task/TaskPlanner.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index de4f173f..06a92318 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -59,7 +59,7 @@ auto TaskPlanner::TaskAssignmentStrategy::make(Profile profile) { case Profile::DefaultFastest: // Default RMF assignment strategy with fastest-first approach - strategy.finish_time_weights({0.0, 1.0}); + strategy.finish_time_weights({1.0}); strategy.battery_penalty_weights({}); strategy.busy_penalty_weights({}); strategy.busy_mode(BusyMode::Binary); @@ -68,8 +68,8 @@ auto TaskPlanner::TaskAssignmentStrategy::make(Profile profile) case Profile::BatteryAware: // Prioritize battery level, strongly penalize low SOC with a quadratic term. // Still account for task efficiency (fastest-first), but ignore busyness. - strategy.finish_time_weights({0.0, 1.0}); - strategy.battery_penalty_weights({0.0, 20.0, 60.0}); + strategy.finish_time_weights({1.0}); + strategy.battery_penalty_weights({20.0, 60.0}); strategy.busy_penalty_weights({}); strategy.busy_mode(BusyMode::Binary); break; @@ -1150,7 +1150,7 @@ class TaskPlanner::Implementation double sum = 0.0; for (std::size_t i = 0; i < coeffs.size(); ++i) { - sum += coeffs[i] * std::pow(x, static_cast(i)); + sum += coeffs[i] * std::pow(x, static_cast(i+1)); } return sum; } From d30a92b9abdaf4aedd196115e0aa4cfe7ca0c6c4 Mon Sep 17 00:00:00 2001 From: kjchee Date: Mon, 17 Nov 2025 11:06:42 +0800 Subject: [PATCH 09/10] Fix style Signed-off-by: kjchee --- rmf_task/include/rmf_task/TaskPlanner.hpp | 6 +++--- rmf_task/src/rmf_task/TaskPlanner.cpp | 15 ++++++++------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index 75b53387..14488f63 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -59,7 +59,7 @@ class TaskPlanner /// To be overwritten from fleet_config.yaml Unset }; - + /// Options for computing the busyness penalty. enum class BusyMode : uint8_t { @@ -75,13 +75,13 @@ class TaskPlanner /// Make a strategy initialized from a predefined profile static TaskAssignmentStrategy make(Profile profile); - + /// Set the finish-time polynomial weights TaskAssignmentStrategy& finish_time_weights(std::vector values); /// Get the finish-time polynomial weights const std::vector& finish_time_weights() const; - + /// Set the battery penalty polynomial weights TaskAssignmentStrategy& battery_penalty_weights(std::vector values); diff --git a/rmf_task/src/rmf_task/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp index 06a92318..4216c919 100644 --- a/rmf_task/src/rmf_task/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -51,7 +51,7 @@ TaskPlanner::TaskAssignmentStrategy::TaskAssignmentStrategy() //============================================================================== auto TaskPlanner::TaskAssignmentStrategy::make(Profile profile) - -> TaskAssignmentStrategy +-> TaskAssignmentStrategy { TaskAssignmentStrategy strategy; @@ -88,7 +88,7 @@ auto TaskPlanner::TaskAssignmentStrategy::make(Profile profile) //============================================================================== const std::vector& - TaskPlanner::TaskAssignmentStrategy::finish_time_weights() const +TaskPlanner::TaskAssignmentStrategy::finish_time_weights() const { return _pimpl->finish_time; } @@ -103,7 +103,7 @@ auto TaskPlanner::TaskAssignmentStrategy::finish_time_weights( //============================================================================== const std::vector& - TaskPlanner::TaskAssignmentStrategy::battery_penalty_weights() const +TaskPlanner::TaskAssignmentStrategy::battery_penalty_weights() const { return _pimpl->battery_penalty; } @@ -118,7 +118,7 @@ auto TaskPlanner::TaskAssignmentStrategy::battery_penalty_weights( //============================================================================== const std::vector& - TaskPlanner::TaskAssignmentStrategy::busy_penalty_weights() const +TaskPlanner::TaskAssignmentStrategy::busy_penalty_weights() const { return _pimpl->busy_penalty; } @@ -133,14 +133,14 @@ auto TaskPlanner::TaskAssignmentStrategy::busy_penalty_weights( //============================================================================== TaskPlanner::TaskAssignmentStrategy::BusyMode - TaskPlanner::TaskAssignmentStrategy::busy_mode() const +TaskPlanner::TaskAssignmentStrategy::busy_mode() const { return _pimpl->busy_mode; } //============================================================================== auto TaskPlanner::TaskAssignmentStrategy::busy_mode(BusyMode mode) - -> TaskAssignmentStrategy& +-> TaskAssignmentStrategy& { _pimpl->busy_mode = mode; return *this; @@ -235,7 +235,8 @@ TaskPlanner::Options::Options( std::move(greedy), std::move(interrupter), std::move(finishing_request), - TaskAssignmentStrategy::make(TaskAssignmentStrategy::Profile::DefaultFastest) + TaskAssignmentStrategy::make( + TaskAssignmentStrategy::Profile::DefaultFastest) })) { // Do nothing From 31c3f1ce21890c95563a37dd0d33c9b8a67bd457 Mon Sep 17 00:00:00 2001 From: kjchee Date: Mon, 17 Nov 2025 14:04:39 +0800 Subject: [PATCH 10/10] Clarify robot idle-state definition Signed-off-by: kjchee --- rmf_task/include/rmf_task/State.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rmf_task/include/rmf_task/State.hpp b/rmf_task/include/rmf_task/State.hpp index 0d6001e6..5cc2e96e 100644 --- a/rmf_task/include/rmf_task/State.hpp +++ b/rmf_task/include/rmf_task/State.hpp @@ -105,8 +105,11 @@ class State : public CompositeData /// CurrentOrientation, or CurrentTime) then this will return a std::nullopt. std::optional 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. + /// Check if the robot is idle. + /// + /// True if the robot is not executing any task. + /// False only when the robot is actively executing a task + /// (finishing requests are not counted as execution). RMF_TASK_DEFINE_COMPONENT(bool, IsIdle); bool is_idle() const; State& idle(bool is_idle);