From 55323cab642c22dde89c1579964cd999bf5949d3 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 6 Oct 2020 17:21:25 +0800 Subject: [PATCH 1/6] Changed API of state, removed default arguments for times. Signed-off-by: Aaron Chong --- rmf_tasks/include/rmf_tasks/Estimate.hpp | 11 ++++++ rmf_tasks/include/rmf_tasks/Request.hpp | 12 +++---- rmf_tasks/include/rmf_tasks/agv/State.hpp | 34 ++++++++++++++----- .../include/rmf_tasks/requests/Delivery.hpp | 4 +-- rmf_tasks/src/agv/State.cpp | 32 ++++++++--------- rmf_tasks/src/requests/Delivery.cpp | 8 ++--- 6 files changed, 65 insertions(+), 36 deletions(-) diff --git a/rmf_tasks/include/rmf_tasks/Estimate.hpp b/rmf_tasks/include/rmf_tasks/Estimate.hpp index b57c8e79e..d0d41f0c9 100644 --- a/rmf_tasks/include/rmf_tasks/Estimate.hpp +++ b/rmf_tasks/include/rmf_tasks/Estimate.hpp @@ -29,14 +29,25 @@ class Estimate { public: + /// Constructor of an estimate of a task request. + /// + /// \param[in] finish_state + /// Finish state of the robot once it completes the task request. + /// + /// \param[in] wait_until + /// The ideal time the robot starts executing this task. Estimate(agv::State finish_state, rmf_traffic::Time wait_until); + /// Finish state of the robot once it completes the task request. agv::State finish_state() const; + /// Sets a new finish state for the robot. Estimate& finish_state(agv::State new_finish_state); + /// The ideal time the robot starts executing this task. rmf_traffic::Time wait_until() const; + /// Sets a new starting time for the robot to execute the task request. Estimate& wait_until(rmf_traffic::Time new_wait_until); class Implementation; diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp index 83fbc915e..c11e7d6a8 100644 --- a/rmf_tasks/include/rmf_tasks/Request.hpp +++ b/rmf_tasks/include/rmf_tasks/Request.hpp @@ -29,26 +29,26 @@ namespace rmf_tasks { -/// Implement this for new tasks +/// Implement this for new type of requests. class Request { public: using SharedPtr = std::shared_ptr; - // Get the id of the task + /// Get the id of the task virtual std::size_t 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 + /// Estimate the state of the robot when the task is finished along with the + /// time the robot has to wait before commencing the task virtual rmf_utils::optional estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const = 0; - // Estimate the invariant component of the task's duration + /// Estimate the invariant component of the task's duration virtual rmf_traffic::Duration invariant_duration() const = 0; - // Get the earliest start time that this task may begin + /// Get the earliest start time that this task may begin virtual rmf_traffic::Time earliest_start_time() const = 0; }; diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index bd569874b..e2c49fe96 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -28,6 +28,7 @@ namespace rmf_tasks { namespace agv { +/// This state representation is used for task planning. class State { public: @@ -35,32 +36,49 @@ class State /// Constructor /// /// \param[in] waypoint + /// Current state's location waypoint index. + /// /// \param[in] charging_waypoint - /// \param[in] finish_time + /// The charging waypoint index of this robot. + /// + /// \param[in] finish_duration + /// Time left until the robot finishes it's current task, or until the + /// agent is ready to take on another task. + /// /// \param[in] battery_soc - /// \param[in] threshold_soc + /// Current battery state of charge of the robot. This value needs to be + /// between 0.0 to 1.0. State( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Time finish_time = std::chrono::steady_clock::now(), - double battery_soc = 1.0); - - State(); + rmf_traffic::Duration finish_duration, + double battery_soc); + /// The current location waypoint index. std::size_t waypoint() const; + /// Sets the current location waypoint index. State& waypoint(std::size_t new_waypoint); + /// Robot's charging waypoint index. std::size_t charging_waypoint() const; + /// Sets the charging waypoint index. State& charging_waypoint(std::size_t new_charging_waypoint); - rmf_traffic::Time finish_time() const; + /// The duration until the robot finishes it's current task or when it is + /// ready for a new task. + rmf_traffic::Duration finish_duration() const; - State& finish_time(rmf_traffic::Time new_finish_time); + /// Sets the finish duration for the robot. + State& finish_time(rmf_traffic::Duration new_finish_duration); + /// The current battery state of charge of the robot. This value is between + /// 0.0 and 1.0. double battery_soc() const; + /// Sets a new battery state of charge value. This value needs to be between + /// 0.0 and 1.0. State& battery_soc(double new_battery_soc); class Implementation; diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp index 16a2e8937..0bd9a3dc1 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp @@ -46,8 +46,8 @@ class Delivery : public rmf_tasks::Request std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, - bool drain_battery = true, - rmf_traffic::Time start_time = std::chrono::steady_clock::now()); + rmf_traffic::Time start_time, + bool drain_battery = true); std::size_t id() const final; diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index accbd3112..dd6ec4cd0 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -15,6 +15,8 @@ * */ +#include + #include namespace rmf_tasks { @@ -28,17 +30,21 @@ class State::Implementation Implementation( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Time finish_time, + rmf_traffic::Duration finish_duration, double battery_soc) : _waypoint(waypoint), _charging_waypoint(charging_waypoint), - _finish_time(finish_time), + _finish_duration(finish_duration), _battery_soc(battery_soc) - {} + { + if (_battery_soc < 0.0 || _battery_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge needs be between 0.0 and 1.0."); + } std::size_t _waypoint; std::size_t _charging_waypoint; - rmf_traffic::Time _finish_time; + rmf_traffic::Duration _finish_duration; double _battery_soc; }; @@ -46,17 +52,11 @@ class State::Implementation State::State( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Time finish_time, + rmf_traffic::Duration finish_duration, double battery_soc) : _pimpl(rmf_utils::make_impl( Implementation( - waypoint, charging_waypoint, finish_time, battery_soc))) -{} - -//============================================================================== -State::State() -: _pimpl(rmf_utils::make_impl( - Implementation(0, 0, std::chrono::steady_clock::now(), 0.0))) + waypoint, charging_waypoint, finish_duration, battery_soc))) {} //============================================================================== @@ -86,15 +86,15 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint) } //============================================================================== -rmf_traffic::Time State::finish_time() const +rmf_traffic::Duration State::finish_duration() const { - return _pimpl->_finish_time; + return _pimpl->_finish_duration; } //============================================================================== -State& State::finish_time(rmf_traffic::Time new_finish_time) +State& State::finish_time(rmf_traffic::Duration new_finish_duration) { - _pimpl->_finish_time = new_finish_time; + _pimpl->_finish_duration = new_finish_duration; return *this; } diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index 2bc9aac43..96654358d 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -37,8 +37,8 @@ class Delivery::Implementation std::shared_ptr _motion_sink; std::shared_ptr _device_sink; std::shared_ptr _planner; - bool _drain_battery; rmf_traffic::Time _start_time; + bool _drain_battery; rmf_traffic::Duration _invariant_duration; double _invariant_battery_drain; @@ -52,8 +52,8 @@ rmf_tasks::Request::SharedPtr Delivery::make( std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, - bool drain_battery, - rmf_traffic::Time start_time) + rmf_traffic::Time start_time, + bool drain_battery) { std::shared_ptr delivery(new Delivery()); delivery->_pimpl->_id = id; @@ -62,8 +62,8 @@ rmf_tasks::Request::SharedPtr Delivery::make( delivery->_pimpl->_motion_sink = std::move(motion_sink); delivery->_pimpl->_device_sink = std::move(device_sink); delivery->_pimpl->_planner = std::move(planner); - delivery->_pimpl->_drain_battery = drain_battery; delivery->_pimpl->_start_time = start_time; + delivery->_pimpl->_drain_battery = drain_battery; // Calculate duration of invariant component of task const auto plan_start_time = std::chrono::steady_clock::now(); From 27ca864d711b16cedcf863a4578f9ace3862b2da Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 7 Oct 2020 17:46:13 +0800 Subject: [PATCH 2/6] Finished changing time implementations to use durations --- rmf_tasks/include/rmf_tasks/Request.hpp | 1 + rmf_tasks/include/rmf_tasks/agv/State.hpp | 2 +- .../include/rmf_tasks/agv/TaskPlanner.hpp | 6 +- .../rmf_tasks/requests/ChargeBattery.hpp | 1 + .../include/rmf_tasks/requests/Clean.hpp | 1 + .../include/rmf_tasks/requests/Delivery.hpp | 1 + rmf_tasks/src/agv/State.cpp | 2 +- rmf_tasks/src/agv/TaskPlanner.cpp | 115 +++++++++++------- rmf_tasks/src/requests/ChargeBattery.cpp | 16 ++- rmf_tasks/src/requests/Clean.cpp | 24 ++-- rmf_tasks/src/requests/Delivery.cpp | 23 ++-- rmf_tasks/test/unit/agv/test_TaskPlanner.cpp | 98 +++++++-------- 12 files changed, 173 insertions(+), 117 deletions(-) diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp index c11e7d6a8..e34e3cb24 100644 --- a/rmf_tasks/include/rmf_tasks/Request.hpp +++ b/rmf_tasks/include/rmf_tasks/Request.hpp @@ -42,6 +42,7 @@ class Request /// Estimate the state of the robot when the task is finished along with the /// time the robot has to wait before commencing the task virtual rmf_utils::optional estimate_finish( + rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const = 0; diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index e2c49fe96..6db41118a 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -71,7 +71,7 @@ class State rmf_traffic::Duration finish_duration() const; /// Sets the finish duration for the robot. - State& finish_time(rmf_traffic::Duration new_finish_duration); + State& finish_duration(rmf_traffic::Duration new_finish_duration); /// The current battery state of charge of the robot. This value is between /// 0.0 and 1.0. diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp index 731258b8c..85b7124b9 100644 --- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp @@ -127,6 +127,7 @@ class TaskPlanner /// Get the greedy planner based assignments for a set of initial states and /// requests Assignments greedy_plan( + rmf_traffic::Time relative_start_time, std::vector initial_states, std::vector state_configs, std::vector requests); @@ -138,12 +139,15 @@ class TaskPlanner /// recommended to call plan() method and use the greedy solution for bidding. /// If a bid is awarded, the optimal solution may be used for assignments. Assignments optimal_plan( + rmf_traffic::Time relative_start_time, std::vector initial_states, std::vector state_configs, std::vector requests, std::function interrupter); - double compute_cost(const Assignments& assignments); + double compute_cost( + const Assignments& assignments, + rmf_traffic::Time relative_start_time); class Implementation; diff --git a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp index 4bfcef0d6..e2e999f29 100644 --- a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp @@ -48,6 +48,7 @@ class ChargeBattery : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( + rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const final; diff --git a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp index 759fbe335..cb6e51bfe 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp @@ -55,6 +55,7 @@ class Clean : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( + rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const final; diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp index 0bd9a3dc1..41ce4ebcb 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp @@ -52,6 +52,7 @@ class Delivery : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( + rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const final; diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index dd6ec4cd0..c1abd4a23 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -92,7 +92,7 @@ rmf_traffic::Duration State::finish_duration() const } //============================================================================== -State& State::finish_time(rmf_traffic::Duration new_finish_duration) +State& State::finish_duration(rmf_traffic::Duration new_finish_duration) { _pimpl->_finish_duration = new_finish_duration; return *this; diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index 37624a943..5facf222a 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -166,7 +166,8 @@ class Candidates const std::vector& initial_states, const std::vector& state_configs, const rmf_tasks::Request& request, - const rmf_tasks::Request& charge_battery_request); + const rmf_tasks::Request& charge_battery_request, + rmf_traffic::Time relative_start_time); Candidates(const Candidates& other) { @@ -215,13 +216,14 @@ class Candidates void update_candidate( std::size_t candidate, State state, - rmf_traffic::Time wait_until) + rmf_traffic::Time wait_until, + rmf_traffic::Time relative_start_time) { const auto it = _candidate_map.at(candidate); _value_map.erase(it); _candidate_map[candidate] = _value_map.insert( { - state.finish_time(), + relative_start_time + state.finish_duration(), Entry{candidate, state, wait_until} }); } @@ -253,31 +255,37 @@ Candidates Candidates::make( const std::vector& initial_states, const std::vector& state_configs, const rmf_tasks::Request& request, - const rmf_tasks::Request& charge_battery_request) + const rmf_tasks::Request& charge_battery_request, + rmf_traffic::Time relative_start_time) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) { const auto& state = initial_states[i]; const auto& state_config = state_configs[i]; - const auto finish = request.estimate_finish(state, state_config); + const auto finish = + request.estimate_finish(relative_start_time, state, state_config); if (finish.has_value()) { initial_map.insert({ - finish.value().finish_state().finish_time(), + relative_start_time + finish.value().finish_state().finish_duration(), Entry{i, finish.value().finish_state(), finish.value().wait_until()}}); } else { auto battery_estimate = - charge_battery_request.estimate_finish(state, state_config); + charge_battery_request.estimate_finish( + relative_start_time, state, state_config); if (battery_estimate.has_value()) { - auto new_finish = request.estimate_finish( - battery_estimate.value().finish_state(), state_config); + auto new_finish = + request.estimate_finish( + relative_start_time, + battery_estimate.value().finish_state(), + state_config); assert(new_finish.has_value()); initial_map.insert( - {new_finish.value().finish_state().finish_time(), + {relative_start_time + new_finish.value().finish_state().finish_duration(), Entry{i, new_finish.value().finish_state(), new_finish.value().wait_until()}}); } else @@ -300,10 +308,11 @@ struct PendingTask std::vector& initial_states, std::vector& state_configs, rmf_tasks::Request::SharedPtr request_, - rmf_tasks::Request::SharedPtr charge_battery_request) + rmf_tasks::Request::SharedPtr charge_battery_request, + rmf_traffic::Time relative_start_time) : request(std::move(request_)), candidates(Candidates::make( - initial_states, state_configs, *request, *charge_battery_request)), + initial_states, state_configs, *request, *charge_battery_request, relative_start_time)), earliest_start_time(request->earliest_start_time()) { // Do nothing @@ -582,7 +591,7 @@ class TaskPlanner::Implementation std::shared_ptr config; - double compute_g(const Assignments& assigned_tasks) + double compute_g(const Assignments& assigned_tasks, rmf_traffic::Time relative_start_time) { double cost = 0.0; for (const auto& agent : assigned_tasks) @@ -591,7 +600,7 @@ class TaskPlanner::Implementation { cost += rmf_traffic::time::to_seconds( - assignment.state().finish_time() - assignment.earliest_start_time()); + relative_start_time + assignment.state().finish_duration() - assignment.earliest_start_time()); } } @@ -641,6 +650,7 @@ class TaskPlanner::Implementation } Assignments complete_solve( + rmf_traffic::Time relative_start_time, std::vector& initial_states, const std::vector& state_configs, const std::vector& requests, @@ -648,9 +658,8 @@ class TaskPlanner::Implementation bool greedy) { assert(initial_states.size() == state_configs.size()); - - const rmf_traffic::Time start_time = std::chrono::steady_clock::now(); - auto node = make_initial_node(initial_states, state_configs, requests, start_time); + + auto node = make_initial_node(initial_states, state_configs, requests, relative_start_time); Node::AssignedTasks complete_assignments; complete_assignments.resize(node->assigned_tasks.size()); @@ -658,9 +667,9 @@ class TaskPlanner::Implementation while (node) { if (greedy) - node = greedy_solve(node, initial_states, state_configs, start_time); + node = greedy_solve(node, initial_states, state_configs, relative_start_time); else - node = solve(node, initial_states, state_configs, requests.size(), start_time, interrupter); + node = solve(node, initial_states, state_configs, requests.size(), relative_start_time, interrupter); if (!node) { @@ -697,7 +706,9 @@ class TaskPlanner::Implementation // copy final state estimates std::vector estimates; - estimates.resize(node->assigned_tasks.size()); + estimates.resize( + node->assigned_tasks.size(), + State(0, 0, rmf_traffic::Duration(0), 0.0)); for (std::size_t i = 0; i < node->assigned_tasks.size(); ++i) { const auto& assignments = node->assigned_tasks[i]; @@ -707,16 +718,16 @@ class TaskPlanner::Implementation estimates[i] = assignments.back().state(); } - node = make_initial_node(estimates, state_configs, new_tasks, start_time); + node = make_initial_node(estimates, state_configs, new_tasks, relative_start_time); initial_states = estimates; } return complete_assignments; } - double compute_g(const Node& node) + double compute_g(const Node& node, const rmf_traffic::Time relative_start_time) { - return compute_g(node.assigned_tasks); + return compute_g(node.assigned_tasks, relative_start_time); } double compute_h(const Node& node, const rmf_traffic::Time relative_start_time) @@ -757,8 +768,7 @@ class TaskPlanner::Implementation value = 0.0; else value = - rmf_traffic::time::to_seconds( - assignments.back().state().finish_time() - relative_start_time); + rmf_traffic::time::to_seconds(assignments.back().state().finish_duration()); } } @@ -775,7 +785,7 @@ class TaskPlanner::Implementation double compute_f(const Node& n, const rmf_traffic::Time relative_start_time) { - return compute_g(n) + compute_h(n, relative_start_time); + return compute_g(n, relative_start_time) + compute_h(n, relative_start_time); } ConstNodePtr make_initial_node( @@ -792,7 +802,7 @@ class TaskPlanner::Implementation initial_node->unassigned_tasks.insert( { request->id(), - PendingTask(initial_states, state_configs, request, config->charge_battery_request()) + PendingTask(initial_states, state_configs, request, config->charge_battery_request(), relative_start_time) }); initial_node->cost_estimate = compute_f(*initial_node, relative_start_time); @@ -804,8 +814,8 @@ class TaskPlanner::Implementation rmf_traffic::Time latest = rmf_traffic::Time::min(); for (const auto& s : initial_states) { - if (latest < s.finish_time()) - latest = s.finish_time(); + if (latest < relative_start_time + s.finish_duration()) + latest = relative_start_time + s.finish_duration(); } return latest; @@ -829,7 +839,7 @@ class TaskPlanner::Implementation return initial_node; } - rmf_traffic::Time get_latest_time(const Node& node) + rmf_traffic::Time get_latest_time(const Node& node, rmf_traffic::Time relative_start_time) { rmf_traffic::Time latest = rmf_traffic::Time::min(); for (const auto& a : node.assigned_tasks) @@ -837,7 +847,7 @@ class TaskPlanner::Implementation if (a.empty()) continue; - const auto finish_time = a.back().state().finish_time(); + const auto finish_time = relative_start_time + a.back().state().finish_duration(); if (latest < finish_time) latest = finish_time; } @@ -880,14 +890,15 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.request->estimate_finish( - entry.state, state_config); + relative_start_time, entry.state, state_config); if (finish.has_value()) { new_u.second.candidates.update_candidate( entry.candidate, finish.value().finish_state(), - finish.value().wait_until()); + finish.value().wait_until(), + relative_start_time); } else { @@ -919,7 +930,9 @@ class TaskPlanner::Implementation if (add_charger) { - auto battery_estimate = config->charge_battery_request()->estimate_finish(entry.state, state_config); + auto battery_estimate = + config->charge_battery_request()->estimate_finish( + relative_start_time, entry.state, state_config); if (battery_estimate.has_value()) { new_node->assigned_tasks[entry.candidate].push_back( @@ -932,11 +945,17 @@ class TaskPlanner::Implementation for (auto& new_u : new_node->unassigned_tasks) { const auto finish = - new_u.second.request->estimate_finish(battery_estimate.value().finish_state(), state_config); + new_u.second.request->estimate_finish( + relative_start_time, + battery_estimate.value().finish_state(), + state_config); if (finish.has_value()) { new_u.second.candidates.update_candidate( - entry.candidate, finish.value().finish_state(), finish.value().wait_until()); + entry.candidate, + finish.value().finish_state(), + finish.value().wait_until(), + relative_start_time); } else { @@ -955,7 +974,7 @@ class TaskPlanner::Implementation // Update the cost estimate for new_node new_node->cost_estimate = compute_f(*new_node, relative_start_time); - new_node->latest_time = get_latest_time(*new_node); + new_node->latest_time = get_latest_time(*new_node, relative_start_time); // Apply filter if (filter && filter->ignore(*new_node)) @@ -987,7 +1006,7 @@ class TaskPlanner::Implementation } auto estimate = config->charge_battery_request()->estimate_finish( - state, state_configs[agent]); + relative_start_time, state, state_configs[agent]); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( @@ -1000,11 +1019,16 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.request->estimate_finish( - estimate.value().finish_state(), state_configs[agent]); + relative_start_time, + estimate.value().finish_state(), + state_configs[agent]); if (finish.has_value()) { new_u.second.candidates.update_candidate( - agent, finish.value().finish_state(), finish.value().wait_until()); + agent, + finish.value().finish_state(), + finish.value().wait_until(), + relative_start_time); } else { @@ -1013,7 +1037,7 @@ class TaskPlanner::Implementation } new_node->cost_estimate = compute_f(*new_node, relative_start_time); - new_node->latest_time = get_latest_time(*new_node); + new_node->latest_time = get_latest_time(*new_node, relative_start_time); return new_node; } @@ -1183,11 +1207,13 @@ TaskPlanner::TaskPlanner(std::shared_ptr config) } auto TaskPlanner::greedy_plan( + rmf_traffic::Time relative_start_time, std::vector initial_states, std::vector state_configs, std::vector requests) -> Assignments { return _pimpl->complete_solve( + relative_start_time, initial_states, state_configs, requests, @@ -1196,12 +1222,14 @@ auto TaskPlanner::greedy_plan( } auto TaskPlanner::optimal_plan( + rmf_traffic::Time relative_start_time, std::vector initial_states, std::vector state_configs, std::vector requests, std::function interrupter) -> Assignments { return _pimpl->complete_solve( + relative_start_time, initial_states, state_configs, requests, @@ -1209,9 +1237,10 @@ auto TaskPlanner::optimal_plan( false); } -auto TaskPlanner::compute_cost(const Assignments& assignments) -> double +double TaskPlanner::compute_cost( + const Assignments& assignments, rmf_traffic::Time relative_start_time) { - return _pimpl->compute_g(assignments); + return _pimpl->compute_g(assignments, relative_start_time); } diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index a794dc8af..760968bec 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -81,6 +81,7 @@ std::size_t ChargeBattery::id() const //============================================================================== rmf_utils::optional ChargeBattery::estimate_finish( + rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const { @@ -94,10 +95,10 @@ rmf_utils::optional ChargeBattery::estimate_finish( agv::State state( initial_state.charging_waypoint(), initial_state.charging_waypoint(), - initial_state.finish_time(), + initial_state.finish_duration(), initial_state.battery_soc()); - const auto start_time = initial_state.finish_time(); + const auto start_time = relative_start_time + initial_state.finish_duration(); double battery_soc = initial_state.battery_soc(); rmf_traffic::Duration variant_duration(0); @@ -141,10 +142,15 @@ rmf_utils::optional ChargeBattery::estimate_finish( (3600 * delta_soc * _pimpl->_battery_system->nominal_capacity()) / _pimpl->_battery_system->charging_current(); - const rmf_traffic::Time wait_until = initial_state.finish_time(); - state.finish_time( + const rmf_traffic::Time wait_until = + relative_start_time + initial_state.finish_duration(); + + const rmf_traffic::Time new_finish_time = wait_until + variant_duration + - rmf_traffic::time::from_seconds(time_to_charge)); + rmf_traffic::time::from_seconds(time_to_charge); + const rmf_traffic::Duration new_finish_duration = + new_finish_time - relative_start_time; + state.finish_duration(new_finish_duration); state.battery_soc(_pimpl->_charge_soc); return Estimate(state, wait_until); diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp index ae97b11f4..987553f69 100644 --- a/rmf_tasks/src/requests/Clean.cpp +++ b/rmf_tasks/src/requests/Clean.cpp @@ -110,18 +110,20 @@ std::size_t Clean::id() const //============================================================================== rmf_utils::optional Clean::estimate_finish( + rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const { agv::State state( _pimpl->end_waypoint, initial_state.charging_waypoint(), - initial_state.finish_time(), + initial_state.finish_duration(), initial_state.battery_soc()); rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = initial_state.finish_time(); + const rmf_traffic::Time start_time = + relative_start_time + initial_state.finish_duration(); double battery_soc = initial_state.battery_soc(); double dSOC_motion = 0.0; double dSOC_ambient = 0.0; @@ -161,13 +163,17 @@ rmf_utils::optional Clean::estimate_finish( } const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; - const rmf_traffic::Time wait_until = - initial_state.finish_time() > ideal_start ? - initial_state.finish_time() : ideal_start; + const rmf_traffic::Time state_finish_time = + relative_start_time + initial_state.finish_duration(); + const rmf_traffic::Time wait_until = state_finish_time > ideal_start ? + state_finish_time : ideal_start; // Factor in invariants - state.finish_time( - wait_until + variant_duration + _pimpl->invariant_duration); + const rmf_traffic::Time new_finish_time = + wait_until + variant_duration + _pimpl->invariant_duration; + const rmf_traffic::Duration new_finish_duration = + new_finish_time - relative_start_time; + state.finish_duration(new_finish_duration); if (_pimpl->drain_battery) { @@ -183,7 +189,7 @@ rmf_utils::optional Clean::estimate_finish( if ( _pimpl->end_waypoint != state.charging_waypoint()) { rmf_traffic::agv::Planner::Start start{ - state.finish_time(), + relative_start_time + state.finish_duration(), _pimpl->end_waypoint, 0.0}; @@ -195,7 +201,7 @@ rmf_utils::optional Clean::estimate_finish( result_to_charger->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); const rmf_traffic::Duration retreat_duration = - finish_time - state.finish_time(); + finish_time - (relative_start_time + state.finish_duration()); dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); dSOC_ambient = _pimpl->ambient_sink->compute_change_in_charge( diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index abad1b7af..afa56d5ca 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -108,18 +108,21 @@ std::size_t Delivery::id() const //============================================================================== rmf_utils::optional Delivery::estimate_finish( + rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const { agv::State state( _pimpl->_dropoff_waypoint, initial_state.charging_waypoint(), - initial_state.finish_time(), + initial_state.finish_duration(), initial_state.battery_soc()); rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = initial_state.finish_time(); + const rmf_traffic::Time start_time = + relative_start_time + initial_state.finish_duration(); + double battery_soc = initial_state.battery_soc(); double dSOC_motion = 0.0; double dSOC_device = 0.0; @@ -159,13 +162,15 @@ rmf_utils::optional Delivery::estimate_finish( } const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; - const rmf_traffic::Time wait_until = - initial_state.finish_time() > ideal_start ? - initial_state.finish_time() : ideal_start; + const rmf_traffic::Time state_finish_time = + relative_start_time + initial_state.finish_duration(); + const rmf_traffic::Time wait_until = state_finish_time > ideal_start ? + state_finish_time : ideal_start; // Factor in invariants - state.finish_time( - wait_until + variant_duration + _pimpl->_invariant_duration); + const rmf_traffic::Time new_finish_time = + wait_until + variant_duration + _pimpl->_invariant_duration; + state.finish_duration(new_finish_time - relative_start_time); if (_pimpl->_drain_battery) { @@ -181,7 +186,7 @@ rmf_utils::optional Delivery::estimate_finish( if ( _pimpl->_dropoff_waypoint != state.charging_waypoint()) { rmf_traffic::agv::Planner::Start start{ - state.finish_time(), + relative_start_time + state.finish_duration(), _pimpl->_dropoff_waypoint, 0.0}; @@ -193,7 +198,7 @@ rmf_utils::optional Delivery::estimate_finish( result_to_charger->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); const rmf_traffic::Duration retreat_duration = - finish_time - state.finish_time(); + finish_time - (relative_start_time + state.finish_duration()); dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); dSOC_device = _pimpl->_device_sink->compute_change_in_charge( diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp index 01f377307..c14ea0912 100644 --- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp @@ -44,7 +44,8 @@ inline void display_solution( std::string parent, const rmf_tasks::agv::TaskPlanner::Assignments& assignments, - const double cost) + const double cost, + rmf_traffic::Time relative_start_time) { std::cout << parent << " cost: " << cost << std::endl; std::cout << parent << " assignments:" << std::endl; @@ -55,7 +56,8 @@ inline void display_solution( { const auto& s = a.state(); const double start_seconds = a.earliest_start_time().time_since_epoch().count()/1e9; - const double finish_seconds = s.finish_time().time_since_epoch().count()/1e9; + const rmf_traffic::Time finish_time = relative_start_time + s.finish_duration(); + const double finish_seconds = finish_time.time_since_epoch().count()/1e9; std::cout << " <" << a.task_id() << ": " << start_seconds << ", "<< finish_seconds << ", " << 100* s.battery_soc() << "%>" << std::endl; @@ -139,8 +141,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_tasks::agv::State{13, 13}, - rmf_tasks::agv::State{2, 2} + rmf_tasks::agv::State{13, 13, rmf_traffic::Duration(0), 1.0}, + rmf_tasks::agv::State{2, 2, rmf_traffic::Duration(0), 1.0} }; std::vector state_configs = @@ -158,8 +160,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 2, @@ -168,8 +170,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 3, @@ -178,8 +180,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)) + now + rmf_traffic::time::from_seconds(0), + drain_battery) }; std::shared_ptr task_config = @@ -187,15 +189,15 @@ SCENARIO("Grid World") rmf_tasks::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( - initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments); + now, initial_states, state_configs, requests); + const double greedy_cost = task_planner.compute_cost(greedy_assignments, now); const auto optimal_assignments = task_planner.optimal_plan( - initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments); + now, initial_states, state_configs, requests, nullptr); + const double optimal_cost = task_planner.compute_cost(optimal_assignments, now); - display_solution("Greedy", greedy_assignments, greedy_cost); - display_solution("Optimal", optimal_assignments, optimal_cost); + display_solution("Greedy", greedy_assignments, greedy_cost, now); + display_solution("Optimal", optimal_assignments, optimal_cost, now); REQUIRE(optimal_cost <= greedy_cost); } @@ -206,8 +208,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_tasks::agv::State{13, 13}, - rmf_tasks::agv::State{2, 2} + rmf_tasks::agv::State{13, 13, std::chrono::seconds(0), 1.0}, + rmf_tasks::agv::State{2, 2, std::chrono::seconds(0), 1.0} }; std::vector state_configs = @@ -225,8 +227,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 2, @@ -235,8 +237,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 3, @@ -245,8 +247,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 3, @@ -255,8 +257,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 4, @@ -265,8 +267,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(50000)), + now + rmf_traffic::time::from_seconds(50000), + drain_battery), rmf_tasks::requests::Delivery::make( 5, @@ -275,8 +277,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(50000)), + now + rmf_traffic::time::from_seconds(50000), + drain_battery), rmf_tasks::requests::Delivery::make( 6, @@ -285,8 +287,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 7, @@ -295,8 +297,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 8, @@ -305,8 +307,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 9, @@ -315,8 +317,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 10, @@ -325,8 +327,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 11, @@ -335,8 +337,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)) + now + rmf_traffic::time::from_seconds(60000), + drain_battery) }; std::shared_ptr task_config = @@ -344,15 +346,15 @@ SCENARIO("Grid World") rmf_tasks::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( - initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments); + now, initial_states, state_configs, requests); + const double greedy_cost = task_planner.compute_cost(greedy_assignments, now); const auto optimal_assignments = task_planner.optimal_plan( - initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments); + now, initial_states, state_configs, requests, nullptr); + const double optimal_cost = task_planner.compute_cost(optimal_assignments, now); - display_solution("Greedy", greedy_assignments, greedy_cost); - display_solution("Optimal", optimal_assignments, optimal_cost); + display_solution("Greedy", greedy_assignments, greedy_cost, now); + display_solution("Optimal", optimal_assignments, optimal_cost, now); REQUIRE(optimal_cost <= greedy_cost); } From 23758acce7d7d86c67fa09522d97e057046454d2 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 7 Oct 2020 18:12:32 +0800 Subject: [PATCH 3/6] Throws exception when invalid arguments are passed, eg. battery soc must be between 0.0 and 1.0 Signed-off-by: Aaron Chong --- rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp | 5 +++++ rmf_tasks/src/agv/State.cpp | 4 ++++ rmf_tasks/src/agv/StateConfig.cpp | 10 +++++++++- 3 files changed, 18 insertions(+), 1 deletion(-) diff --git a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp index 87274040f..0c21cda49 100644 --- a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp @@ -30,10 +30,15 @@ class StateConfig /// Constructor /// /// \param[in] threshold_soc + /// Minimum charge level the battery is allowed to deplete to. This + /// value needs to be between 0.0 and 1.0. StateConfig(double threshold_soc); + /// Gets the battery state of charge threshold value. double threshold_soc() const; + /// Sets a new battery state of charge threshold value. This value needs to be + /// between 0.0 and 1.0. StateConfig& threshold_soc(double threshold_soc); class Implementation; diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index c1abd4a23..198deeaa2 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -107,6 +107,10 @@ double State::battery_soc() const //============================================================================== State& State::battery_soc(double new_battery_soc) { + if (new_battery_soc < 0.0 || new_battery_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge needs be between 0.0 and 1.0."); + _pimpl->_battery_soc = new_battery_soc; return *this; } diff --git a/rmf_tasks/src/agv/StateConfig.cpp b/rmf_tasks/src/agv/StateConfig.cpp index d5593cb72..b187fe0d8 100644 --- a/rmf_tasks/src/agv/StateConfig.cpp +++ b/rmf_tasks/src/agv/StateConfig.cpp @@ -15,6 +15,8 @@ * */ +#include + #include namespace rmf_tasks { @@ -34,7 +36,9 @@ StateConfig::StateConfig(double threshold_soc) threshold_soc })) { - // Do nothing + if (threshold_soc < 0.0 || threshold_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge threshold needs be between 0.0 and 1.0."); } double StateConfig::threshold_soc() const @@ -44,6 +48,10 @@ double StateConfig::threshold_soc() const auto StateConfig::threshold_soc(double threshold_soc) -> StateConfig& { + if (threshold_soc < 0.0 || threshold_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge threshold needs be between 0.0 and 1.0."); + _pimpl->threshold_soc = threshold_soc; return *this; } From f46dd9c4c059d843d8c6286424e24fe79e484657 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Sun, 11 Oct 2020 22:33:11 +0800 Subject: [PATCH 4/6] Removed default argument for time now Signed-off-by: Aaron Chong --- rmf_tasks/include/rmf_tasks/Request.hpp | 2 +- rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp | 2 +- rmf_tasks/include/rmf_tasks/requests/Clean.hpp | 4 ++-- rmf_tasks/src/requests/Clean.cpp | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp index b454b45c1..ac23c18bd 100644 --- a/rmf_tasks/include/rmf_tasks/Request.hpp +++ b/rmf_tasks/include/rmf_tasks/Request.hpp @@ -42,7 +42,7 @@ class Request /// Estimate the state of the robot when the task is finished along with the /// time the robot has to wait before commencing the task virtual rmf_utils::optional estimate_finish( - rmf_traffic::Time relative_start_time, + rmf_traffic::Time time_now, const agv::State& initial_state, const agv::StateConfig& state_config) const = 0; diff --git a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp index e2e999f29..c0ef4e2bf 100644 --- a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp @@ -48,7 +48,7 @@ class ChargeBattery : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( - rmf_traffic::Time relative_start_time, + rmf_traffic::Time time_now, const agv::State& initial_state, const agv::StateConfig& state_config) const final; diff --git a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp index cb6e51bfe..bbd5e4083 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp @@ -49,8 +49,8 @@ class Clean : public rmf_tasks::Request std::shared_ptr ambient_sink, std::shared_ptr cleaning_sink, std::shared_ptr planner, - bool drain_battery = true, - rmf_traffic::Time start_time = std::chrono::steady_clock::now()); + rmf_traffic::Time start_time, + bool drain_battery = true); std::size_t id() const final; diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp index 4c3b060c8..e0b84bfc2 100644 --- a/rmf_tasks/src/requests/Clean.cpp +++ b/rmf_tasks/src/requests/Clean.cpp @@ -55,8 +55,8 @@ rmf_tasks::Request::SharedPtr Clean::make( std::shared_ptr ambient_sink, std::shared_ptr cleaning_sink, std::shared_ptr planner, - bool drain_battery, - rmf_traffic::Time start_time) + rmf_traffic::Time start_time, + bool drain_battery) { std::shared_ptr clean(new Clean()); clean->_pimpl->id = id; From 613ee52a7fcaff5d35e23f687dab301e54b06eb6 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 12 Oct 2020 14:08:06 +0800 Subject: [PATCH 5/6] Reverting back to finish_time, using rmf_traffic::agv::Plan::Start to hold onto location and time Signed-off-by: Aaron Chong --- rmf_tasks/include/rmf_tasks/Request.hpp | 1 - rmf_tasks/include/rmf_tasks/agv/State.hpp | 42 +++++++------ .../rmf_tasks/requests/ChargeBattery.hpp | 1 - .../include/rmf_tasks/requests/Clean.hpp | 1 - .../include/rmf_tasks/requests/Delivery.hpp | 1 - rmf_tasks/src/agv/State.cpp | 59 +++++++++++-------- rmf_tasks/src/agv/TaskPlanner.cpp | 37 ++++++------ rmf_tasks/src/requests/ChargeBattery.cpp | 17 +++--- rmf_tasks/src/requests/Clean.cpp | 22 ++++--- rmf_tasks/src/requests/Delivery.cpp | 21 +++---- rmf_tasks/test/unit/agv/test_TaskPlanner.cpp | 29 +++++---- 11 files changed, 121 insertions(+), 110 deletions(-) diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp index ac23c18bd..c8fbbcf0a 100644 --- a/rmf_tasks/include/rmf_tasks/Request.hpp +++ b/rmf_tasks/include/rmf_tasks/Request.hpp @@ -42,7 +42,6 @@ class Request /// Estimate the state of the robot when the task is finished along with the /// time the robot has to wait before commencing the task virtual rmf_utils::optional estimate_finish( - rmf_traffic::Time time_now, const agv::State& initial_state, const agv::StateConfig& state_config) const = 0; diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index 6db41118a..454076f99 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -24,6 +24,7 @@ #include #include +#include namespace rmf_tasks { namespace agv { @@ -35,30 +36,27 @@ class State /// Constructor /// - /// \param[in] waypoint - /// Current state's location waypoint index. + /// \param[in] plan_start + /// Current state's next start for new plans, includes the time which + /// the plan can feasibly start, according to the finishing time of any + /// tasks that the robot is currently performing. /// /// \param[in] charging_waypoint /// The charging waypoint index of this robot. /// - /// \param[in] finish_duration - /// Time left until the robot finishes it's current task, or until the - /// agent is ready to take on another task. - /// /// \param[in] battery_soc /// Current battery state of charge of the robot. This value needs to be /// between 0.0 to 1.0. State( - std::size_t waypoint, + rmf_traffic::agv::Plan::Start plan_start, std::size_t charging_waypoint, - rmf_traffic::Duration finish_duration, double battery_soc); - /// The current location waypoint index. - std::size_t waypoint() const; + /// The next plan start based on the current state. + rmf_traffic::agv::Plan::Start plan_start() const; - /// Sets the current location waypoint index. - State& waypoint(std::size_t new_waypoint); + /// Sets the next plan start. + State& plan_start(rmf_traffic::agv::Plan::Start new_plan_start); /// Robot's charging waypoint index. std::size_t charging_waypoint() const; @@ -66,13 +64,6 @@ class State /// Sets the charging waypoint index. State& charging_waypoint(std::size_t new_charging_waypoint); - /// The duration until the robot finishes it's current task or when it is - /// ready for a new task. - rmf_traffic::Duration finish_duration() const; - - /// Sets the finish duration for the robot. - State& finish_duration(rmf_traffic::Duration new_finish_duration); - /// The current battery state of charge of the robot. This value is between /// 0.0 and 1.0. double battery_soc() const; @@ -81,6 +72,19 @@ class State /// 0.0 and 1.0. State& battery_soc(double new_battery_soc); + /// The current location waypoint index. + std::size_t waypoint() const; + + /// Sets the current location waypoint index. + State& waypoint(std::size_t new_waypoint); + + /// The time which the robot finishes its current task or when it is ready for + /// a new task. + rmf_traffic::Time finish_time() const; + + /// Sets the finish time for the robot. + State& finish_time(rmf_traffic::Time new_finish_time); + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp index c0ef4e2bf..4bfcef0d6 100644 --- a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp @@ -48,7 +48,6 @@ class ChargeBattery : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( - rmf_traffic::Time time_now, const agv::State& initial_state, const agv::StateConfig& state_config) const final; diff --git a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp index bbd5e4083..53102f1df 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp @@ -55,7 +55,6 @@ class Clean : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( - rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const final; diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp index 41ce4ebcb..0bd9a3dc1 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp @@ -52,7 +52,6 @@ class Delivery : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( - rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const final; diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index 198deeaa2..13b1cef09 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -28,13 +28,11 @@ class State::Implementation public: Implementation( - std::size_t waypoint, + rmf_traffic::agv::Plan::Start plan_start, std::size_t charging_waypoint, - rmf_traffic::Duration finish_duration, double battery_soc) - : _waypoint(waypoint), + : _plan_start(std::move(plan_start)), _charging_waypoint(charging_waypoint), - _finish_duration(finish_duration), _battery_soc(battery_soc) { if (_battery_soc < 0.0 || _battery_soc > 1.0) @@ -42,33 +40,30 @@ class State::Implementation "Battery State of Charge needs be between 0.0 and 1.0."); } - std::size_t _waypoint; + rmf_traffic::agv::Plan::Start _plan_start; std::size_t _charging_waypoint; - rmf_traffic::Duration _finish_duration; double _battery_soc; }; //============================================================================== State::State( - std::size_t waypoint, + rmf_traffic::agv::Plan::Start plan_start, std::size_t charging_waypoint, - rmf_traffic::Duration finish_duration, double battery_soc) : _pimpl(rmf_utils::make_impl( - Implementation( - waypoint, charging_waypoint, finish_duration, battery_soc))) + Implementation(std::move(plan_start), charging_waypoint, battery_soc))) {} //============================================================================== -std::size_t State::waypoint() const +rmf_traffic::agv::Plan::Start State::plan_start() const { - return _pimpl->_waypoint; + return _pimpl->_plan_start; } //============================================================================== -State& State::waypoint(std::size_t new_waypoint) +State& State::plan_start(rmf_traffic::agv::Plan::Start new_plan_start) { - _pimpl->_waypoint = new_waypoint; + _pimpl->_plan_start = std::move(new_plan_start); return *this; } @@ -86,34 +81,48 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint) } //============================================================================== -rmf_traffic::Duration State::finish_duration() const +double State::battery_soc() const { - return _pimpl->_finish_duration; + return _pimpl->_battery_soc; } //============================================================================== -State& State::finish_duration(rmf_traffic::Duration new_finish_duration) +State& State::battery_soc(double new_battery_soc) { - _pimpl->_finish_duration = new_finish_duration; + if (new_battery_soc < 0.0 || new_battery_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge needs be between 0.0 and 1.0."); + + _pimpl->_battery_soc = new_battery_soc; return *this; } //============================================================================== -double State::battery_soc() const +std::size_t State::waypoint() const { - return _pimpl->_battery_soc; + return _pimpl->_plan_start.waypoint(); } //============================================================================== -State& State::battery_soc(double new_battery_soc) +State& State::waypoint(std::size_t new_waypoint) { - if (new_battery_soc < 0.0 || new_battery_soc > 1.0) - throw std::invalid_argument( - "Battery State of Charge needs be between 0.0 and 1.0."); + _pimpl->_plan_start.waypoint(new_waypoint); + return *this; +} - _pimpl->_battery_soc = new_battery_soc; +//============================================================================== +rmf_traffic::Time State::finish_time() const +{ + return _pimpl->_plan_start.time(); +} + +//============================================================================== +State& State::finish_time(rmf_traffic::Time new_finish_time) +{ + _pimpl->_plan_start.time(new_finish_time); return *this; } +//============================================================================== } // namespace agv } // namespace rmf_tasks diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index 12c5211df..b8954355c 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -223,7 +223,7 @@ class Candidates _value_map.erase(it); _candidate_map[candidate] = _value_map.insert( { - relative_start_time + state.finish_duration(), + state.finish_time(), Entry{candidate, state, wait_until} }); } @@ -264,28 +264,26 @@ Candidates Candidates::make( const auto& state = initial_states[i]; const auto& state_config = state_configs[i]; const auto finish = - request.estimate_finish(relative_start_time, state, state_config); + request.estimate_finish(state, state_config); if (finish.has_value()) { initial_map.insert({ - relative_start_time + finish.value().finish_state().finish_duration(), + finish.value().finish_state().finish_time(), Entry{i, finish.value().finish_state(), finish.value().wait_until()}}); } else { auto battery_estimate = - charge_battery_request.estimate_finish( - relative_start_time, state, state_config); + charge_battery_request.estimate_finish(state, state_config); if (battery_estimate.has_value()) { auto new_finish = request.estimate_finish( - relative_start_time, battery_estimate.value().finish_state(), state_config); assert(new_finish.has_value()); initial_map.insert( - {relative_start_time + new_finish.value().finish_state().finish_duration(), + {new_finish.value().finish_state().finish_time(), Entry{i, new_finish.value().finish_state(), new_finish.value().wait_until()}}); } else @@ -600,7 +598,7 @@ class TaskPlanner::Implementation { cost += rmf_traffic::time::to_seconds( - relative_start_time + assignment.state().finish_duration() - assignment.earliest_start_time()); + assignment.state().finish_time() - assignment.earliest_start_time()); } } @@ -668,9 +666,11 @@ class TaskPlanner::Implementation // copy final state estimates std::vector estimates; + rmf_traffic::agv::Plan::Start empty_new_start( + relative_start_time, 0, 0.0); estimates.resize( node->assigned_tasks.size(), - State(0, 0, rmf_traffic::Duration(0), 0.0)); + State(empty_new_start, 0, 0.0)); for (std::size_t i = 0; i < node->assigned_tasks.size(); ++i) { const auto& assignments = node->assigned_tasks[i]; @@ -730,7 +730,8 @@ class TaskPlanner::Implementation value = 0.0; else value = - rmf_traffic::time::to_seconds(assignments.back().state().finish_duration()); + rmf_traffic::time::to_seconds( + relative_start_time - assignments.back().state().finish_time()); } } @@ -776,8 +777,8 @@ class TaskPlanner::Implementation rmf_traffic::Time latest = rmf_traffic::Time::min(); for (const auto& s : initial_states) { - if (latest < relative_start_time + s.finish_duration()) - latest = relative_start_time + s.finish_duration(); + if (latest < s.finish_time()) + latest = s.finish_time(); } return latest; @@ -809,7 +810,7 @@ class TaskPlanner::Implementation if (a.empty()) continue; - const auto finish_time = relative_start_time + a.back().state().finish_duration(); + const auto finish_time = a.back().state().finish_time(); if (latest < finish_time) latest = finish_time; } @@ -851,8 +852,7 @@ class TaskPlanner::Implementation for (auto& new_u : new_node->unassigned_tasks) { const auto finish = - new_u.second.request->estimate_finish( - relative_start_time, entry.state, state_config); + new_u.second.request->estimate_finish(entry.state, state_config); if (finish.has_value()) { @@ -893,8 +893,7 @@ class TaskPlanner::Implementation if (add_charger) { auto battery_estimate = - config->charge_battery_request()->estimate_finish( - relative_start_time, entry.state, state_config); + config->charge_battery_request()->estimate_finish(entry.state, state_config); if (battery_estimate.has_value()) { new_node->assigned_tasks[entry.candidate].push_back( @@ -908,7 +907,6 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.request->estimate_finish( - relative_start_time, battery_estimate.value().finish_state(), state_config); if (finish.has_value()) @@ -968,7 +966,7 @@ class TaskPlanner::Implementation } auto estimate = config->charge_battery_request()->estimate_finish( - relative_start_time, state, state_configs[agent]); + state, state_configs[agent]); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( @@ -981,7 +979,6 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.request->estimate_finish( - relative_start_time, estimate.value().finish_state(), state_configs[agent]); if (finish.has_value()) diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index 37b35dd64..0ab9957f5 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -79,7 +79,6 @@ std::size_t ChargeBattery::id() const //============================================================================== rmf_utils::optional ChargeBattery::estimate_finish( - rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const { @@ -87,13 +86,16 @@ rmf_utils::optional ChargeBattery::estimate_finish( return rmf_utils::nullopt; // Compute time taken to reach charging waypoint from current location - agv::State state( + rmf_traffic::agv::Plan::Start final_plan_start( + initial_state.finish_time(), initial_state.charging_waypoint(), + initial_state.plan_start().orientation()); + agv::State state( + std::move(final_plan_start), initial_state.charging_waypoint(), - initial_state.finish_duration(), initial_state.battery_soc()); - const auto start_time = relative_start_time + initial_state.finish_duration(); + const auto start_time = initial_state.finish_time(); double battery_soc = initial_state.battery_soc(); rmf_traffic::Duration variant_duration(0); @@ -134,15 +136,12 @@ rmf_utils::optional ChargeBattery::estimate_finish( (3600 * delta_soc * _pimpl->_battery_system->nominal_capacity()) / _pimpl->_battery_system->charging_current(); - const rmf_traffic::Time wait_until = - relative_start_time + initial_state.finish_duration(); + const rmf_traffic::Time wait_until = initial_state.finish_time(); const rmf_traffic::Time new_finish_time = wait_until + variant_duration + rmf_traffic::time::from_seconds(time_to_charge); - const rmf_traffic::Duration new_finish_duration = - new_finish_time - relative_start_time; - state.finish_duration(new_finish_duration); + state.finish_time(new_finish_time); state.battery_soc(_pimpl->_charge_soc); return Estimate(state, wait_until); diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp index e0b84bfc2..716a80302 100644 --- a/rmf_tasks/src/requests/Clean.cpp +++ b/rmf_tasks/src/requests/Clean.cpp @@ -109,20 +109,21 @@ std::size_t Clean::id() const //============================================================================== rmf_utils::optional Clean::estimate_finish( - rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const { + rmf_traffic::agv::Plan::Start final_plan_start( + initial_state.finish_time(), + _pimpl->end_waypoint, + initial_state.plan_start().orientation()); agv::State state( - _pimpl->end_waypoint, + std::move(final_plan_start), initial_state.charging_waypoint(), - initial_state.finish_duration(), initial_state.battery_soc()); rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = - relative_start_time + initial_state.finish_duration(); + const rmf_traffic::Time start_time = initial_state.finish_time(); double battery_soc = initial_state.battery_soc(); double dSOC_motion = 0.0; double dSOC_ambient = 0.0; @@ -159,17 +160,14 @@ rmf_utils::optional Clean::estimate_finish( } const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; - const rmf_traffic::Time state_finish_time = - relative_start_time + initial_state.finish_duration(); + const rmf_traffic::Time state_finish_time = initial_state.finish_time(); const rmf_traffic::Time wait_until = state_finish_time > ideal_start ? state_finish_time : ideal_start; // Factor in invariants const rmf_traffic::Time new_finish_time = wait_until + variant_duration + _pimpl->invariant_duration; - const rmf_traffic::Duration new_finish_duration = - new_finish_time - relative_start_time; - state.finish_duration(new_finish_duration); + state.finish_time(new_finish_time); if (_pimpl->drain_battery) { @@ -182,7 +180,7 @@ rmf_utils::optional Clean::estimate_finish( if ( _pimpl->end_waypoint != state.charging_waypoint()) { rmf_traffic::agv::Planner::Start start{ - relative_start_time + state.finish_duration(), + state.finish_time(), _pimpl->end_waypoint, 0.0}; @@ -194,7 +192,7 @@ rmf_utils::optional Clean::estimate_finish( result_to_charger->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); const rmf_traffic::Duration retreat_duration = - finish_time - (relative_start_time + state.finish_duration()); + finish_time - state.finish_time(); dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); dSOC_ambient = _pimpl->ambient_sink->compute_change_in_charge( diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index 65e3775bd..d6c936f62 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -107,20 +107,22 @@ std::size_t Delivery::id() const //============================================================================== rmf_utils::optional Delivery::estimate_finish( - rmf_traffic::Time relative_start_time, + // rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const { + rmf_traffic::agv::Plan::Start final_plan_start( + initial_state.finish_time(), + _pimpl->_dropoff_waypoint, + initial_state.plan_start().orientation()); agv::State state( - _pimpl->_dropoff_waypoint, + std::move(final_plan_start), initial_state.charging_waypoint(), - initial_state.finish_duration(), initial_state.battery_soc()); rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = - relative_start_time + initial_state.finish_duration(); + const rmf_traffic::Time start_time = initial_state.finish_time(); double battery_soc = initial_state.battery_soc(); double dSOC_motion = 0.0; @@ -158,15 +160,14 @@ rmf_utils::optional Delivery::estimate_finish( } const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; - const rmf_traffic::Time state_finish_time = - relative_start_time + initial_state.finish_duration(); + const rmf_traffic::Time state_finish_time = initial_state.finish_time(); const rmf_traffic::Time wait_until = state_finish_time > ideal_start ? state_finish_time : ideal_start; // Factor in invariants const rmf_traffic::Time new_finish_time = wait_until + variant_duration + _pimpl->_invariant_duration; - state.finish_duration(new_finish_time - relative_start_time); + state.finish_time(new_finish_time); if (_pimpl->_drain_battery) { @@ -179,7 +180,7 @@ rmf_utils::optional Delivery::estimate_finish( if ( _pimpl->_dropoff_waypoint != state.charging_waypoint()) { rmf_traffic::agv::Planner::Start start{ - relative_start_time + state.finish_duration(), + state.finish_time(), _pimpl->_dropoff_waypoint, 0.0}; @@ -191,7 +192,7 @@ rmf_utils::optional Delivery::estimate_finish( result_to_charger->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); const rmf_traffic::Duration retreat_duration = - finish_time - (relative_start_time + state.finish_duration()); + finish_time - state.finish_time(); dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); dSOC_device = _pimpl->_device_sink->compute_change_in_charge( diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp index c14ea0912..5420f72d9 100644 --- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp @@ -44,8 +44,7 @@ inline void display_solution( std::string parent, const rmf_tasks::agv::TaskPlanner::Assignments& assignments, - const double cost, - rmf_traffic::Time relative_start_time) + const double cost) { std::cout << parent << " cost: " << cost << std::endl; std::cout << parent << " assignments:" << std::endl; @@ -56,7 +55,7 @@ inline void display_solution( { const auto& s = a.state(); const double start_seconds = a.earliest_start_time().time_since_epoch().count()/1e9; - const rmf_traffic::Time finish_time = relative_start_time + s.finish_duration(); + const rmf_traffic::Time finish_time = s.finish_time(); const double finish_seconds = finish_time.time_since_epoch().count()/1e9; std::cout << " <" << a.task_id() << ": " << start_seconds << ", "<< finish_seconds << ", " << 100* s.battery_soc() @@ -138,11 +137,15 @@ SCENARIO("Grid World") WHEN("Planning for 3 requests and 2 agents") { const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + + rmf_traffic::agv::Plan::Start first_start(now, 13, default_orientation); + rmf_traffic::agv::Plan::Start second_start(now, 2, default_orientation); std::vector initial_states = { - rmf_tasks::agv::State{13, 13, rmf_traffic::Duration(0), 1.0}, - rmf_tasks::agv::State{2, 2, rmf_traffic::Duration(0), 1.0} + rmf_tasks::agv::State{std::move(first_start), 13, 1.0}, + rmf_tasks::agv::State{std::move(second_start), 2, 1.0} }; std::vector state_configs = @@ -196,8 +199,8 @@ SCENARIO("Grid World") now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments, now); - display_solution("Greedy", greedy_assignments, greedy_cost, now); - display_solution("Optimal", optimal_assignments, optimal_cost, now); + display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Optimal", optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); } @@ -205,11 +208,15 @@ SCENARIO("Grid World") WHEN("Planning for 11 requests and 2 agents") { const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + + rmf_traffic::agv::Plan::Start first_start(now, 13, default_orientation); + rmf_traffic::agv::Plan::Start second_start(now, 2, default_orientation); std::vector initial_states = { - rmf_tasks::agv::State{13, 13, std::chrono::seconds(0), 1.0}, - rmf_tasks::agv::State{2, 2, std::chrono::seconds(0), 1.0} + rmf_tasks::agv::State{std::move(first_start), 13, 1.0}, + rmf_tasks::agv::State{std::move(second_start), 2, 1.0} }; std::vector state_configs = @@ -353,8 +360,8 @@ SCENARIO("Grid World") now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments, now); - display_solution("Greedy", greedy_assignments, greedy_cost, now); - display_solution("Optimal", optimal_assignments, optimal_cost, now); + display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Optimal", optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); } From 8594dd95a7c403532a4599293469d0ed3dbffbd6 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 12 Oct 2020 15:16:59 +0800 Subject: [PATCH 6/6] Removed unused parameters for relative_start_time Signed-off-by: Aaron Chong --- .../include/rmf_tasks/agv/TaskPlanner.hpp | 4 +- rmf_tasks/src/agv/TaskPlanner.cpp | 53 ++++++++----------- rmf_tasks/src/requests/ChargeBattery.cpp | 8 +-- rmf_tasks/src/requests/Clean.cpp | 8 +-- rmf_tasks/src/requests/Delivery.cpp | 9 ++-- rmf_tasks/test/unit/agv/test_TaskPlanner.cpp | 24 ++++----- 6 files changed, 48 insertions(+), 58 deletions(-) diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp index 85b7124b9..aba301b88 100644 --- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp @@ -145,9 +145,7 @@ class TaskPlanner std::vector requests, std::function interrupter); - double compute_cost( - const Assignments& assignments, - rmf_traffic::Time relative_start_time); + double compute_cost(const Assignments& assignments); class Implementation; diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index b8954355c..bb03a2996 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -166,8 +166,7 @@ class Candidates const std::vector& initial_states, const std::vector& state_configs, const rmf_tasks::Request& request, - const rmf_tasks::Request& charge_battery_request, - rmf_traffic::Time relative_start_time); + const rmf_tasks::Request& charge_battery_request); Candidates(const Candidates& other) { @@ -216,8 +215,7 @@ class Candidates void update_candidate( std::size_t candidate, State state, - rmf_traffic::Time wait_until, - rmf_traffic::Time relative_start_time) + rmf_traffic::Time wait_until) { const auto it = _candidate_map.at(candidate); _value_map.erase(it); @@ -255,8 +253,7 @@ Candidates Candidates::make( const std::vector& initial_states, const std::vector& state_configs, const rmf_tasks::Request& request, - const rmf_tasks::Request& charge_battery_request, - rmf_traffic::Time relative_start_time) + const rmf_tasks::Request& charge_battery_request) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) @@ -306,11 +303,10 @@ struct PendingTask std::vector& initial_states, std::vector& state_configs, rmf_tasks::Request::SharedPtr request_, - rmf_tasks::Request::SharedPtr charge_battery_request, - rmf_traffic::Time relative_start_time) + rmf_tasks::Request::SharedPtr charge_battery_request) : request(std::move(request_)), candidates(Candidates::make( - initial_states, state_configs, *request, *charge_battery_request, relative_start_time)), + initial_states, state_configs, *request, *charge_battery_request)), earliest_start_time(request->earliest_start_time()) { // Do nothing @@ -589,7 +585,7 @@ class TaskPlanner::Implementation std::shared_ptr config; - double compute_g(const Assignments& assigned_tasks, rmf_traffic::Time relative_start_time) + double compute_g(const Assignments& assigned_tasks) { double cost = 0.0; for (const auto& agent : assigned_tasks) @@ -666,11 +662,11 @@ class TaskPlanner::Implementation // copy final state estimates std::vector estimates; - rmf_traffic::agv::Plan::Start empty_new_start( - relative_start_time, 0, 0.0); + rmf_traffic::agv::Plan::Start empty_new_start{ + relative_start_time, 0, 0.0}; estimates.resize( node->assigned_tasks.size(), - State(empty_new_start, 0, 0.0)); + State{empty_new_start, 0, 0.0}); for (std::size_t i = 0; i < node->assigned_tasks.size(); ++i) { const auto& assignments = node->assigned_tasks[i]; @@ -680,16 +676,17 @@ class TaskPlanner::Implementation estimates[i] = assignments.back().state(); } - node = make_initial_node(estimates, state_configs, new_tasks, relative_start_time); + node = make_initial_node( + estimates, state_configs, new_tasks, relative_start_time); initial_states = estimates; } return complete_assignments; } - double compute_g(const Node& node, const rmf_traffic::Time relative_start_time) + double compute_g(const Node& node) { - return compute_g(node.assigned_tasks, relative_start_time); + return compute_g(node.assigned_tasks); } double compute_h(const Node& node, const rmf_traffic::Time relative_start_time) @@ -748,7 +745,7 @@ class TaskPlanner::Implementation double compute_f(const Node& n, const rmf_traffic::Time relative_start_time) { - return compute_g(n, relative_start_time) + compute_h(n, relative_start_time); + return compute_g(n) + compute_h(n, relative_start_time); } ConstNodePtr make_initial_node( @@ -765,7 +762,7 @@ class TaskPlanner::Implementation initial_node->unassigned_tasks.insert( { request->id(), - PendingTask(initial_states, state_configs, request, config->charge_battery_request(), relative_start_time) + PendingTask(initial_states, state_configs, request, config->charge_battery_request()) }); initial_node->cost_estimate = compute_f(*initial_node, relative_start_time); @@ -802,7 +799,7 @@ class TaskPlanner::Implementation return initial_node; } - rmf_traffic::Time get_latest_time(const Node& node, rmf_traffic::Time relative_start_time) + rmf_traffic::Time get_latest_time(const Node& node) { rmf_traffic::Time latest = rmf_traffic::Time::min(); for (const auto& a : node.assigned_tasks) @@ -859,8 +856,7 @@ class TaskPlanner::Implementation new_u.second.candidates.update_candidate( entry.candidate, finish.value().finish_state(), - finish.value().wait_until(), - relative_start_time); + finish.value().wait_until()); } else { @@ -914,8 +910,7 @@ class TaskPlanner::Implementation new_u.second.candidates.update_candidate( entry.candidate, finish.value().finish_state(), - finish.value().wait_until(), - relative_start_time); + finish.value().wait_until()); } else { @@ -934,7 +929,7 @@ class TaskPlanner::Implementation // Update the cost estimate for new_node new_node->cost_estimate = compute_f(*new_node, relative_start_time); - new_node->latest_time = get_latest_time(*new_node, relative_start_time); + new_node->latest_time = get_latest_time(*new_node); // Apply filter if (filter && filter->ignore(*new_node)) @@ -986,8 +981,7 @@ class TaskPlanner::Implementation new_u.second.candidates.update_candidate( agent, finish.value().finish_state(), - finish.value().wait_until(), - relative_start_time); + finish.value().wait_until()); } else { @@ -996,7 +990,7 @@ class TaskPlanner::Implementation } new_node->cost_estimate = compute_f(*new_node, relative_start_time); - new_node->latest_time = get_latest_time(*new_node, relative_start_time); + new_node->latest_time = get_latest_time(*new_node); return new_node; } @@ -1196,10 +1190,9 @@ auto TaskPlanner::optimal_plan( false); } -double TaskPlanner::compute_cost( - const Assignments& assignments, rmf_traffic::Time relative_start_time) +double TaskPlanner::compute_cost(const Assignments& assignments) { - return _pimpl->compute_g(assignments, relative_start_time); + return _pimpl->compute_g(assignments); } diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index 0ab9957f5..41e360b02 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -86,14 +86,14 @@ rmf_utils::optional ChargeBattery::estimate_finish( return rmf_utils::nullopt; // Compute time taken to reach charging waypoint from current location - rmf_traffic::agv::Plan::Start final_plan_start( + rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), initial_state.charging_waypoint(), - initial_state.plan_start().orientation()); - agv::State state( + initial_state.plan_start().orientation()}; + agv::State state{ std::move(final_plan_start), initial_state.charging_waypoint(), - initial_state.battery_soc()); + initial_state.battery_soc()}; const auto start_time = initial_state.finish_time(); diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp index 716a80302..a7f9aba0c 100644 --- a/rmf_tasks/src/requests/Clean.cpp +++ b/rmf_tasks/src/requests/Clean.cpp @@ -112,14 +112,14 @@ rmf_utils::optional Clean::estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const { - rmf_traffic::agv::Plan::Start final_plan_start( + rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), _pimpl->end_waypoint, - initial_state.plan_start().orientation()); - agv::State state( + initial_state.plan_start().orientation()}; + agv::State state{ std::move(final_plan_start), initial_state.charging_waypoint(), - initial_state.battery_soc()); + initial_state.battery_soc()}; rmf_traffic::Duration variant_duration(0); diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index d6c936f62..3adc6342c 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -107,18 +107,17 @@ std::size_t Delivery::id() const //============================================================================== rmf_utils::optional Delivery::estimate_finish( - // rmf_traffic::Time relative_start_time, const agv::State& initial_state, const agv::StateConfig& state_config) const { - rmf_traffic::agv::Plan::Start final_plan_start( + rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), _pimpl->_dropoff_waypoint, - initial_state.plan_start().orientation()); - agv::State state( + initial_state.plan_start().orientation()}; + agv::State state{ std::move(final_plan_start), initial_state.charging_waypoint(), - initial_state.battery_soc()); + initial_state.battery_soc()}; rmf_traffic::Duration variant_duration(0); diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp index 5420f72d9..b5d9c8bf7 100644 --- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp @@ -139,13 +139,13 @@ SCENARIO("Grid World") const auto now = std::chrono::steady_clock::now(); const double default_orientation = 0.0; - rmf_traffic::agv::Plan::Start first_start(now, 13, default_orientation); - rmf_traffic::agv::Plan::Start second_start(now, 2, default_orientation); + rmf_traffic::agv::Plan::Start first_start{now, 13, default_orientation}; + rmf_traffic::agv::Plan::Start second_start{now, 2, default_orientation}; std::vector initial_states = { - rmf_tasks::agv::State{std::move(first_start), 13, 1.0}, - rmf_tasks::agv::State{std::move(second_start), 2, 1.0} + rmf_tasks::agv::State{first_start, 13, 1.0}, + rmf_tasks::agv::State{second_start, 2, 1.0} }; std::vector state_configs = @@ -193,11 +193,11 @@ SCENARIO("Grid World") const auto greedy_assignments = task_planner.greedy_plan( now, initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments, now); + const double greedy_cost = task_planner.compute_cost(greedy_assignments); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments, now); + const double optimal_cost = task_planner.compute_cost(optimal_assignments); display_solution("Greedy", greedy_assignments, greedy_cost); display_solution("Optimal", optimal_assignments, optimal_cost); @@ -210,13 +210,13 @@ SCENARIO("Grid World") const auto now = std::chrono::steady_clock::now(); const double default_orientation = 0.0; - rmf_traffic::agv::Plan::Start first_start(now, 13, default_orientation); - rmf_traffic::agv::Plan::Start second_start(now, 2, default_orientation); + rmf_traffic::agv::Plan::Start first_start{now, 13, default_orientation}; + rmf_traffic::agv::Plan::Start second_start{now, 2, default_orientation}; std::vector initial_states = { - rmf_tasks::agv::State{std::move(first_start), 13, 1.0}, - rmf_tasks::agv::State{std::move(second_start), 2, 1.0} + rmf_tasks::agv::State{first_start, 13, 1.0}, + rmf_tasks::agv::State{second_start, 2, 1.0} }; std::vector state_configs = @@ -354,11 +354,11 @@ SCENARIO("Grid World") const auto greedy_assignments = task_planner.greedy_plan( now, initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments, now); + const double greedy_cost = task_planner.compute_cost(greedy_assignments); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments, now); + const double optimal_cost = task_planner.compute_cost(optimal_assignments); display_solution("Greedy", greedy_assignments, greedy_cost); display_solution("Optimal", optimal_assignments, optimal_cost);