From d00b3f3f2d97958d5618da5ef8fac962cd65a8c9 Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Tue, 3 Nov 2020 10:06:46 +0800 Subject: [PATCH 1/2] Change request id field from size_t to string Modifies task planner, tests and requests code to use string based id instead of size_t. --- rmf_task/include/rmf_task/Request.hpp | 2 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 6 ++ .../rmf_task/requests/ChargeBattery.hpp | 4 +- rmf_task/include/rmf_task/requests/Clean.hpp | 5 +- .../include/rmf_task/requests/Delivery.hpp | 5 +- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 60 ++++++++++++++++--- .../src/rmf_task/requests/ChargeBattery.cpp | 5 +- rmf_task/src/rmf_task/requests/Clean.cpp | 6 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 6 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 28 ++++----- 10 files changed, 91 insertions(+), 36 deletions(-) diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 19a389176..e31cf5209 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -37,7 +37,7 @@ class Request using SharedPtr = std::shared_ptr; /// Get the id of the task - virtual std::size_t id() const = 0; + virtual std::string id() const = 0; /// Estimate the state of the robot when the task is finished along with the /// time the robot has to wait before commencing the task diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 0232c65ab..aa9f948cb 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -113,6 +113,9 @@ class TaskPlanner /// Constructor /// + /// \param[in] id + /// The id for the request this assignment contains + /// /// \param[in] request /// The task request for this assignment /// @@ -122,10 +125,13 @@ class TaskPlanner /// \param[in] earliest_start_time /// The earliest time the agent will begin exececuting this task Assignment( + size_t request_id, rmf_task::RequestPtr request, State state, rmf_traffic::Time deployment_time); + // Get the id of the request this task contains + size_t request_id() const; // Get the request of this task rmf_task::RequestPtr request() const; diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 959cae7f6..0031e09c2 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -18,6 +18,8 @@ #ifndef INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP #define INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP +#include + #include #include @@ -46,7 +48,7 @@ class ChargeBattery : public rmf_task::Request rmf_traffic::Time start_time, bool drain_battery = true); - std::size_t id() const final; + std::string id() const final; rmf_utils::optional estimate_finish( const agv::State& initial_state, diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index 98d148e25..e8aa02c58 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -19,6 +19,7 @@ #define INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP #include +#include #include #include @@ -41,7 +42,7 @@ class Clean : public rmf_task::Request public: static rmf_task::Request::SharedPtr make( - std::size_t id, + std::string id, std::size_t start_waypoint, std::size_t end_waypoint, rmf_traffic::Trajectory& cleaning_path, @@ -52,7 +53,7 @@ class Clean : public rmf_task::Request rmf_traffic::Time start_time, bool drain_battery = true); - std::size_t id() const final; + std::string id() const final; rmf_utils::optional estimate_finish( const agv::State& initial_state, diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 062840ed0..9a37702a5 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -19,6 +19,7 @@ #define INCLUDE__RMF_TASK__REQUESTS__DELIVERY_HPP #include +#include #include #include @@ -40,7 +41,7 @@ class Delivery : public rmf_task::Request public: static rmf_task::Request::SharedPtr make( - std::size_t id, + std::string id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, std::shared_ptr motion_sink, @@ -49,7 +50,7 @@ class Delivery : public rmf_task::Request rmf_traffic::Time start_time, bool drain_battery = true); - std::size_t id() const final; + std::string id() const final; rmf_utils::optional estimate_finish( const agv::State& initial_state, diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index d882648e0..ea72cca71 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -117,6 +117,7 @@ class TaskPlanner::Assignment::Implementation { public: + size_t request_id; rmf_task::RequestPtr request; State state; rmf_traffic::Time deployment_time; @@ -124,11 +125,13 @@ class TaskPlanner::Assignment::Implementation //============================================================================== TaskPlanner::Assignment::Assignment( + size_t request_id, rmf_task::RequestPtr request, State state, rmf_traffic::Time deployment_time) : _pimpl(rmf_utils::make_impl( Implementation{ + request_id, std::move(request), std::move(state), deployment_time @@ -137,6 +140,12 @@ TaskPlanner::Assignment::Assignment( // Do nothing } +//============================================================================== +std::size_t TaskPlanner::Assignment::request_id() const +{ + return _pimpl->request_id; +} + //============================================================================== rmf_task::RequestPtr TaskPlanner::Assignment::request() const { @@ -388,6 +397,28 @@ struct Node unassigned_invariants.erase(erase_it); assert(popped_invariant); } + + void create_internal_task_id(Request::SharedPtr request) + { + size_t internal_id = _internal_task_ids.size(); + _internal_task_ids.insert({request->id(), internal_id}); + } + + void create_internal_task_ids(std::vector requests) + { + for (auto request : requests) + { + create_internal_task_id(request); + } + } + + std::size_t get_internal_task_id(Request::SharedPtr request) + { + return _internal_task_ids.at(request->id()); + } + + private: + std::unordered_map _internal_task_ids; }; @@ -505,7 +536,7 @@ class Filter { // We add 1 to the task_id to differentiate between task_id == 0 and // a task being unassigned. - const std::size_t id = s.request()->id() + 1; + const size_t id = s.request_id() + 1; output += id << (_shift * (count++)); } } @@ -534,8 +565,10 @@ class Filter for (std::size_t j=0; j < a.size(); ++j) { - if (a[j].request()->id() != b[j].request()->id()) + if (a[j].request_id() != b[j].request_id()) + { return false; + } } } @@ -571,7 +604,7 @@ bool Filter::ignore(const Node& node) if (t < current_agent.size()) { - const auto& task_id = current_agent[t].request()->id(); + const auto& task_id = current_agent[t].request_id(); const auto agent_insertion = agent_table->agent.insert({a, nullptr}); if (agent_insertion.second) agent_insertion.first->second = std::make_unique(); @@ -687,7 +720,6 @@ class TaskPlanner::Implementation for (const auto& a : new_assignments) { all_assignments.push_back(a); - // all_assignments.back().task_id = task_id_map.at(a.task_id); } } @@ -797,12 +829,19 @@ class TaskPlanner::Implementation // TODO(YV): Come up with a better solution for charge_battery_request auto charge_battery = make_charging_request(time_now); - for (const auto& request : requests) + + initial_node->create_internal_task_ids(requests); + initial_node->create_internal_task_id(charge_battery); + + for(size_t i = 0; i < requests.size(); ++i) + { + size_t internal_id = initial_node->get_internal_task_id(requests[i]); initial_node->unassigned_tasks.insert( { - request->id(), - PendingTask(initial_states, state_configs, request, charge_battery) + internal_id, + PendingTask(initial_states, state_configs, requests[i], charge_battery) }); + } initial_node->cost_estimate = compute_f(*initial_node, time_now); @@ -877,7 +916,7 @@ class TaskPlanner::Implementation // Assign the unassigned task new_node->assigned_tasks[entry.candidate].push_back( - Assignment{u.second.request, entry.state, entry.wait_until}); + Assignment{u.first, u.second.request, entry.state, entry.wait_until}); // Erase the assigned task from unassigned tasks new_node->pop_unassigned(u.first); @@ -932,9 +971,11 @@ class TaskPlanner::Implementation auto battery_estimate = charge_battery->estimate_finish(entry.state, state_config); if (battery_estimate.has_value()) { + size_t internal_task_id = new_node->get_internal_task_id(charge_battery); new_node->assigned_tasks[entry.candidate].push_back( Assignment { + internal_task_id, charge_battery, battery_estimate.value().finish_state(), battery_estimate.value().wait_until() @@ -998,12 +1039,15 @@ class TaskPlanner::Implementation } auto charge_battery = make_charging_request(state.finish_time()); + auto charge_battery_id = new_node->get_internal_task_id(charge_battery); + auto estimate = charge_battery->estimate_finish( state, state_configs[agent]); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( Assignment{ + charge_battery_id, charge_battery, estimate.value().finish_state(), estimate.value().wait_until()}); diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index fa8fdb680..0df6dac0d 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -15,6 +15,7 @@ * */ +#include #include namespace rmf_task { @@ -29,7 +30,7 @@ class ChargeBattery::Implementation {} // fixed id for now - std::size_t _id = 1001; + std::string _id = "Charge"; rmf_battery::agv::BatterySystemPtr _battery_system; std::shared_ptr _motion_sink; std::shared_ptr _device_sink; @@ -75,7 +76,7 @@ ChargeBattery::ChargeBattery() {} //============================================================================== -std::size_t ChargeBattery::id() const +std::string ChargeBattery::id() const { return _pimpl->_id; } diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 931e53f49..d4e27fe3b 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -30,7 +30,7 @@ class Clean::Implementation Implementation() {} - std::size_t id; + std::string id; std::size_t start_waypoint; std::size_t end_waypoint; rmf_traffic::Trajectory cleaning_path; @@ -47,7 +47,7 @@ class Clean::Implementation //============================================================================== rmf_task::Request::SharedPtr Clean::make( - std::size_t id, + std::string id, std::size_t start_waypoint, std::size_t end_waypoint, rmf_traffic::Trajectory& cleaning_path, @@ -102,7 +102,7 @@ Clean::Clean() {} //============================================================================== -std::size_t Clean::id() const +std::string Clean::id() const { return _pimpl->id; } diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 980b3160e..6c525ed1f 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -30,7 +30,7 @@ class Delivery::Implementation Implementation() {} - std::size_t _id; + std::string _id; std::size_t _pickup_waypoint; std::size_t _dropoff_waypoint; std::shared_ptr _motion_sink; @@ -45,7 +45,7 @@ class Delivery::Implementation //============================================================================== rmf_task::Request::SharedPtr Delivery::make( - std::size_t id, + std::string id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, std::shared_ptr motion_sink, @@ -100,7 +100,7 @@ Delivery::Delivery() {} //============================================================================== -std::size_t Delivery::id() const +std::string Delivery::id() const { return _pimpl->_id; } diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 360d51a6c..c576782b3 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -153,7 +153,7 @@ SCENARIO("Grid World") std::vector requests = { rmf_task::requests::Delivery::make( - 1, + "1", 0, 3, motion_sink, @@ -163,7 +163,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 2, + "2", 15, 2, motion_sink, @@ -173,7 +173,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 3, + "3", 7, 9, motion_sink, @@ -228,7 +228,7 @@ SCENARIO("Grid World") std::vector requests = { rmf_task::requests::Delivery::make( - 1, + "1", 0, 3, motion_sink, @@ -238,7 +238,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 2, + "2", 15, 2, motion_sink, @@ -248,7 +248,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 3, + "3", 7, 9, motion_sink, @@ -258,7 +258,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 4, + "4", 8, 11, motion_sink, @@ -268,7 +268,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 5, + "5", 10, 0, motion_sink, @@ -278,7 +278,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 6, + "6", 4, 8, motion_sink, @@ -288,7 +288,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 7, + "7", 8, 14, motion_sink, @@ -298,7 +298,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 8, + "8", 5, 11, motion_sink, @@ -308,7 +308,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 9, + "9", 9, 0, motion_sink, @@ -318,7 +318,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 10, + "10", 1, 3, motion_sink, @@ -328,7 +328,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 11, + "11", 0, 12, motion_sink, From 6206e41a71763de0d3bff29a0845b54a0e849c9c Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Tue, 3 Nov 2020 18:20:24 +0800 Subject: [PATCH 2/2] Replace map with autogenerated IDs upon task insertion into Node Creates wrappers for Assignments, containing an internal_id field. This field is generated by the Node when inserting a new task. --- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 7 - rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 164 +++++++++--------- 2 files changed, 80 insertions(+), 91 deletions(-) diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index aa9f948cb..f564ea259 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -113,9 +113,6 @@ class TaskPlanner /// Constructor /// - /// \param[in] id - /// The id for the request this assignment contains - /// /// \param[in] request /// The task request for this assignment /// @@ -125,14 +122,10 @@ class TaskPlanner /// \param[in] earliest_start_time /// The earliest time the agent will begin exececuting this task Assignment( - size_t request_id, rmf_task::RequestPtr request, State state, rmf_traffic::Time deployment_time); - // Get the id of the request this task contains - size_t request_id() const; - // Get the request of this task rmf_task::RequestPtr request() const; diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index ea72cca71..621e47e57 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -117,7 +117,6 @@ class TaskPlanner::Assignment::Implementation { public: - size_t request_id; rmf_task::RequestPtr request; State state; rmf_traffic::Time deployment_time; @@ -125,13 +124,11 @@ class TaskPlanner::Assignment::Implementation //============================================================================== TaskPlanner::Assignment::Assignment( - size_t request_id, rmf_task::RequestPtr request, State state, rmf_traffic::Time deployment_time) : _pimpl(rmf_utils::make_impl( Implementation{ - request_id, std::move(request), std::move(state), deployment_time @@ -141,13 +138,7 @@ TaskPlanner::Assignment::Assignment( } //============================================================================== -std::size_t TaskPlanner::Assignment::request_id() const -{ - return _pimpl->request_id; -} - -//============================================================================== -rmf_task::RequestPtr TaskPlanner::Assignment::request() const +rmf_task::RequestPtr TaskPlanner::Assignment::request() const { return _pimpl->request; } @@ -200,10 +191,10 @@ class Candidates using Map = std::multimap; static Candidates make( - const std::vector& initial_states, - const std::vector& state_configs, - const rmf_task::Request& request, - const rmf_task::Request& charge_battery_request); + const std::vector& initial_states, + const std::vector& state_configs, + const rmf_task::Request& request, + const rmf_task::Request& charge_battery_request); Candidates(const Candidates& other) { @@ -287,10 +278,10 @@ class Candidates }; Candidates Candidates::make( - const std::vector& initial_states, - const std::vector& state_configs, - const rmf_task::Request& request, - const rmf_task::Request& charge_battery_request) + const std::vector& initial_states, + const std::vector& state_configs, + const rmf_task::Request& request, + const rmf_task::Request& charge_battery_request) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) @@ -323,7 +314,7 @@ Candidates Candidates::make( << "] and request [" << request.id() << " ]" << std::endl; assert(false); } - } + } } @@ -354,7 +345,13 @@ struct PendingTask // ============================================================================ struct Node { - using AssignedTasks = TaskPlanner::Assignments; + struct AssignmentWrapper + { + std::size_t internal_id; + TaskPlanner::Assignment assignment; + }; + + using AssignedTasks = std::vector>; using UnassignedTasks = std::unordered_map; using InvariantSet = std::multiset; @@ -364,6 +361,13 @@ struct Node double cost_estimate; rmf_traffic::Time latest_time; InvariantSet unassigned_invariants; + std::size_t next_available_internal_id = 1; + + // ID 0 is reserved for charging tasks + std::size_t get_available_internal_id(bool charging_task = false) + { + return charging_task ? 0 : next_available_internal_id++; + } void sort_invariants() { @@ -385,7 +389,7 @@ struct Node bool popped_invariant = false; InvariantSet::iterator erase_it; for (auto it = unassigned_invariants.begin(); - it != unassigned_invariants.end(); ++it) + it != unassigned_invariants.end(); ++it) { if (it->task_id == task_id) { @@ -397,28 +401,6 @@ struct Node unassigned_invariants.erase(erase_it); assert(popped_invariant); } - - void create_internal_task_id(Request::SharedPtr request) - { - size_t internal_id = _internal_task_ids.size(); - _internal_task_ids.insert({request->id(), internal_id}); - } - - void create_internal_task_ids(std::vector requests) - { - for (auto request : requests) - { - create_internal_task_id(request); - } - } - - std::size_t get_internal_task_id(Request::SharedPtr request) - { - return _internal_task_ids.at(request->id()); - } - - private: - std::unordered_map _internal_task_ids; }; @@ -536,7 +518,7 @@ class Filter { // We add 1 to the task_id to differentiate between task_id == 0 and // a task being unassigned. - const size_t id = s.request_id() + 1; + const std::size_t id = s.internal_id + 1; output += id << (_shift * (count++)); } } @@ -565,7 +547,7 @@ class Filter for (std::size_t j=0; j < a.size(); ++j) { - if (a[j].request_id() != b[j].request_id()) + if (a[j].internal_id != b[j].internal_id) { return false; } @@ -604,7 +586,7 @@ bool Filter::ignore(const Node& node) if (t < current_agent.size()) { - const auto& task_id = current_agent[t].request_id(); + const auto& task_id = current_agent[t].internal_id; const auto agent_insertion = agent_table->agent.insert({a, nullptr}); if (agent_insertion.second) agent_insertion.first->second = std::make_unique(); @@ -634,8 +616,13 @@ bool Filter::ignore(const Node& node) const rmf_traffic::Duration segmentation_threshold = rmf_traffic::time::from_seconds(1.0); -} // anonymous namespace +inline double compute_g_assignment(const TaskPlanner::Assignment& assignment) +{ + return rmf_traffic::time::to_seconds(assignment.state().finish_time() + - assignment.request()->earliest_start_time()); +} +} // anonymous namespace class TaskPlanner::Implementation { @@ -661,16 +648,15 @@ class TaskPlanner::Implementation { for (const auto& assignment : agent) { - cost += - rmf_traffic::time::to_seconds( - assignment.state().finish_time() - assignment.request()->earliest_start_time()); + cost += compute_g_assignment(assignment); } } return cost; } - Assignments prune_assignments(Assignments& assignments) + TaskPlanner::Assignments prune_assignments( + TaskPlanner::Assignments& assignments) { for (std::size_t a = 0; a < assignments.size(); ++a) { @@ -681,7 +667,7 @@ class TaskPlanner::Implementation // TODO(YV): Remove this after fixing the planner if (std::dynamic_pointer_cast( assignments[a].back().request())) - assignments[a].pop_back(); + assignments[a].pop_back(); } return assignments; @@ -699,7 +685,7 @@ class TaskPlanner::Implementation auto node = make_initial_node(initial_states, state_configs, requests, time_now); - Node::AssignedTasks complete_assignments; + TaskPlanner::Assignments complete_assignments; complete_assignments.resize(node->assigned_tasks.size()); while (node) @@ -719,12 +705,14 @@ class TaskPlanner::Implementation const auto& new_assignments = node->assigned_tasks[i]; for (const auto& a : new_assignments) { - all_assignments.push_back(a); + all_assignments.push_back(a.assignment); } } if (node->unassigned_tasks.empty()) + { return prune_assignments(complete_assignments); + } std::vector new_tasks; for (const auto& u : node->unassigned_tasks) @@ -743,7 +731,7 @@ class TaskPlanner::Implementation if (assignments.empty()) estimates[i] = initial_states[i]; else - estimates[i] = assignments.back().state(); + estimates[i] = assignments.back().assignment.state(); } node = make_initial_node(estimates, state_configs, new_tasks, time_now); @@ -755,7 +743,15 @@ class TaskPlanner::Implementation double compute_g(const Node& node) { - return compute_g(node.assigned_tasks); + double cost = 0.0; + for (const auto& agent : node.assigned_tasks) + { + for (const auto& assignment : agent) + { + cost += compute_g_assignment(assignment.assignment); + } + } + return cost; } double compute_h(const Node& node, const rmf_traffic::Time time_now) @@ -797,7 +793,7 @@ class TaskPlanner::Implementation else value = rmf_traffic::time::to_seconds( - assignments.back().state().finish_time() - time_now); + assignments.back().assignment.state().finish_time() - time_now); } } @@ -829,17 +825,15 @@ class TaskPlanner::Implementation // TODO(YV): Come up with a better solution for charge_battery_request auto charge_battery = make_charging_request(time_now); - - initial_node->create_internal_task_ids(requests); - initial_node->create_internal_task_id(charge_battery); - - for(size_t i = 0; i < requests.size(); ++i) + for (const auto& request : requests) { - size_t internal_id = initial_node->get_internal_task_id(requests[i]); + // Generate a unique internal id for the request. Currently, multiple + // requests with the same string id will be assigned different internal ids + std::size_t internal_id = initial_node->get_available_internal_id(); initial_node->unassigned_tasks.insert( { internal_id, - PendingTask(initial_states, state_configs, requests[i], charge_battery) + PendingTask(initial_states, state_configs, request, charge_battery) }); } @@ -885,7 +879,7 @@ class TaskPlanner::Implementation if (a.empty()) continue; - const auto finish_time = a.back().state().finish_time(); + const auto finish_time = a.back().assignment.state().finish_time(); if (latest < finish_time) latest = finish_time; } @@ -916,7 +910,8 @@ class TaskPlanner::Implementation // Assign the unassigned task new_node->assigned_tasks[entry.candidate].push_back( - Assignment{u.first, u.second.request, entry.state, entry.wait_until}); + Node::AssignmentWrapper{u.first, + Assignment{u.second.request, entry.state, entry.wait_until}}); // Erase the assigned task from unassigned tasks new_node->pop_unassigned(u.first); @@ -971,15 +966,14 @@ class TaskPlanner::Implementation auto battery_estimate = charge_battery->estimate_finish(entry.state, state_config); if (battery_estimate.has_value()) { - size_t internal_task_id = new_node->get_internal_task_id(charge_battery); new_node->assigned_tasks[entry.candidate].push_back( - Assignment - { - internal_task_id, - charge_battery, - battery_estimate.value().finish_state(), - battery_estimate.value().wait_until() - }); + { new_node->get_available_internal_id(true), + Assignment + { + charge_battery, + battery_estimate.value().finish_state(), + battery_estimate.value().wait_until() + }}); for (auto& new_u : new_node->unassigned_tasks) { const auto finish = @@ -1033,25 +1027,27 @@ class TaskPlanner::Implementation if (!assignments.empty()) { if (std::dynamic_pointer_cast( - assignments.back().request())) + assignments.back().assignment.request())) return nullptr; - state = assignments.back().state(); + state = assignments.back().assignment.state(); } auto charge_battery = make_charging_request(state.finish_time()); - auto charge_battery_id = new_node->get_internal_task_id(charge_battery); - auto estimate = charge_battery->estimate_finish( state, state_configs[agent]); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( - Assignment{ - charge_battery_id, - charge_battery, - estimate.value().finish_state(), - estimate.value().wait_until()}); - + Node::AssignmentWrapper + { + new_node->get_available_internal_id(true), + Assignment + { + charge_battery, + estimate.value().finish_state(), + estimate.value().wait_until() + } + }); for (auto& new_u : new_node->unassigned_tasks) { const auto finish =