From 3be04b161c984674210c5ce70a5bdbbc8626f1ca Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 25 Nov 2020 16:19:53 +0800 Subject: [PATCH 1/2] Added test for planning an impossible to complete task --- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 61 +++++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 538c5e97d..d30ff332c 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -766,4 +767,64 @@ SCENARIO("Grid World") REQUIRE(optimal_cost <= greedy_cost); } + WHEN("A loop request is impossible to fulfil") + { + const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + + rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; + + std::vector initial_states = + { + rmf_task::agv::State{first_location, 9, 1.0}, + }; + + std::vector state_configs = + { + rmf_task::agv::StateConfig{0.2}, + }; + + std::vector requests = + { + rmf_task::requests::Loop::make( + "Loop1", + 0, + 15, + 1000, + motion_sink, + device_sink, + planner, + now, + true) + }; + + std::shared_ptr task_config = + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); + rmf_task::agv::TaskPlanner task_planner(task_config); + + + auto start_time = std::chrono::steady_clock::now(); + const auto greedy_assignments = task_planner.greedy_plan( + now, initial_states, state_configs, requests); + const double greedy_cost = task_planner.compute_cost(greedy_assignments); + auto finish_time = std::chrono::steady_clock::now(); + std::cout << "Greedy solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Greedy", greedy_assignments, greedy_cost); + + task_planner = rmf_task::agv::TaskPlanner(task_config); + start_time = std::chrono::steady_clock::now(); + 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); + finish_time = std::chrono::steady_clock::now(); + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Optimal", optimal_assignments, optimal_cost); + } + } \ No newline at end of file From 03bfac606c20b56553b69e3d2931358de4289770 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 25 Nov 2020 17:30:46 +0800 Subject: [PATCH 2/2] TaskPlanner returns empty assignments if request is impossible to complete --- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 101 ++++++++++++++------ rmf_task/test/unit/agv/test_TaskPlanner.cpp | 4 + 2 files changed, 77 insertions(+), 28 deletions(-) diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index eefc24517..404d1eec2 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -193,7 +193,7 @@ class Candidates // Map finish time to Entry using Map = std::multimap; - static Candidates make( + static std::shared_ptr make( const std::vector& initial_states, const std::vector& state_configs, const rmf_task::Request& request, @@ -283,7 +283,7 @@ class Candidates } }; -Candidates Candidates::make( +std::shared_ptr Candidates::make( const std::vector& initial_states, const std::vector& state_configs, const rmf_task::Request& request, @@ -317,51 +317,82 @@ Candidates Candidates::make( { auto new_finish = request.estimate_finish( battery_estimate.value().finish_state(), state_config, estimate_cache); - assert(new_finish.has_value()); - initial_map.insert( - {new_finish.value().finish_state().finish_time(), - Entry{ - i, - new_finish.value().finish_state(), - new_finish.value().wait_until(), - state, - true}}); - } - else - { - std::cerr << "Unable to create entry for candidate [" << i - << "] and request [" << request.id() << " ]" << std::endl; - assert(false); + if (new_finish.has_value()) + { + initial_map.insert( + {new_finish.value().finish_state().finish_time(), + Entry{ + i, + new_finish.value().finish_state(), + new_finish.value().wait_until(), + state, + true}}); + } } } } - return Candidates(std::move(initial_map)); + if (initial_map.empty()) + { + std::cout << "Unable to create candidates for request [" << request.id() + << "]" << std::endl; + return nullptr; + } + + std::shared_ptr candidates( + new Candidates(std::move(initial_map))); + return candidates; } // ============================================================================ -struct PendingTask +class PendingTask { - PendingTask( +public: + + static std::shared_ptr make( std::vector& initial_states, std::vector& state_configs, rmf_task::ConstRequestPtr request_, rmf_task::ConstRequestPtr charge_battery_request, - std::shared_ptr estimate_cache) + std::shared_ptr estimate_cache); + + rmf_task::ConstRequestPtr request; + Candidates candidates; + rmf_traffic::Time earliest_start_time; + +private: + PendingTask( + rmf_task::ConstRequestPtr request_, + Candidates candidates_) : request(std::move(request_)), - candidates(Candidates::make(initial_states, state_configs, - *request, *charge_battery_request, estimate_cache)), + candidates(candidates_), earliest_start_time(request->earliest_start_time()) { // Do nothing } - - rmf_task::ConstRequestPtr request; - Candidates candidates; - rmf_traffic::Time earliest_start_time; }; +std::shared_ptr PendingTask::make( + std::vector& initial_states, + std::vector& state_configs, + rmf_task::ConstRequestPtr request_, + rmf_task::ConstRequestPtr charge_battery_request, + std::shared_ptr estimate_cache) +{ + const auto candidates = Candidates::make(initial_states, state_configs, + *request_, *charge_battery_request, estimate_cache); + + if (!candidates) + return nullptr; + + std::shared_ptr pending_task( + new PendingTask(request_, *candidates)); + return pending_task; +} + + + // ============================================================================ struct Node { @@ -726,6 +757,8 @@ class TaskPlanner::Implementation assert(initial_states.size() == state_configs.size()); auto node = make_initial_node(initial_states, state_configs, requests, time_now); + if (!node) + return {}; TaskPlanner::Assignments complete_assignments; complete_assignments.resize(node->assigned_tasks.size()); @@ -777,6 +810,8 @@ class TaskPlanner::Implementation } node = make_initial_node(estimates, state_configs, new_tasks, time_now); + if (!node) + return {}; initial_states = estimates; } @@ -871,10 +906,20 @@ class TaskPlanner::Implementation // 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(); + const auto pending_task= PendingTask::make( + initial_states, + state_configs, + request, + charge_battery, + estimate_cache); + + if (!pending_task) + return nullptr; + initial_node->unassigned_tasks.insert( { internal_id, - PendingTask(initial_states, state_configs, request, charge_battery, estimate_cache) + *pending_task }); } diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index d30ff332c..24bbb65c0 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -825,6 +825,10 @@ SCENARIO("Grid World") std::cout << "Optimal solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Optimal", optimal_assignments, optimal_cost); + + REQUIRE(optimal_assignments.empty()); + REQUIRE(greedy_assignments.empty()); + REQUIRE(optimal_cost <= greedy_cost); } } \ No newline at end of file