From c7cd8813649b64a71fb4e85ec366103ddb82dd29 Mon Sep 17 00:00:00 2001 From: Yadu Date: Tue, 31 Aug 2021 22:21:21 +0800 Subject: [PATCH] Allow task planner to append a finishing task when robot goes idle (#28) Signed-off-by: Yadunund --- rmf_task/CMakeLists.txt | 1 + .../include/rmf_task/BinaryPriorityScheme.hpp | 7 +- rmf_task/include/rmf_task/CostCalculator.hpp | 2 + rmf_task/include/rmf_task/Estimate.hpp | 20 +- rmf_task/include/rmf_task/Priority.hpp | 1 + rmf_task/include/rmf_task/Request.hpp | 29 +- rmf_task/include/rmf_task/RequestFactory.hpp | 44 ++ rmf_task/include/rmf_task/agv/Constraints.hpp | 2 + rmf_task/include/rmf_task/agv/Parameters.hpp | 2 + rmf_task/include/rmf_task/agv/State.hpp | 2 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 110 ++++- .../rmf_task/requests/ChargeBattery.hpp | 19 +- .../requests/ChargeBatteryFactory.hpp | 53 +++ rmf_task/include/rmf_task/requests/Clean.hpp | 34 +- .../include/rmf_task/requests/Delivery.hpp | 36 +- rmf_task/include/rmf_task/requests/Loop.hpp | 31 +- .../rmf_task/requests/ParkRobotFactory.hpp | 63 +++ rmf_task/src/rmf_task/Request.cpp | 15 +- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 194 ++++++++- .../src/rmf_task/requests/ChargeBattery.cpp | 5 +- .../requests/ChargeBatteryFactory.cpp | 45 +++ rmf_task/src/rmf_task/requests/Clean.cpp | 5 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 5 +- rmf_task/src/rmf_task/requests/Loop.cpp | 5 +- .../rmf_task/requests/ParkRobotFactory.cpp | 87 ++++ rmf_task/test/unit/agv/test_TaskPlanner.cpp | 378 ++++++++++++++---- 26 files changed, 1064 insertions(+), 131 deletions(-) create mode 100644 rmf_task/include/rmf_task/RequestFactory.hpp create mode 100644 rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp create mode 100644 rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp create mode 100644 rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp create mode 100644 rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index 9d3ac4f5..a883fbc4 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(rmf_cmake_uncrustify QUIET) file(GLOB lib_srcs "src/rmf_task/agv/*.cpp" "src/rmf_task/requests/*.cpp" + "src/rmf_task/requests/factory/*.cpp" "src/rmf_task/*.cpp" ) diff --git a/rmf_task/include/rmf_task/BinaryPriorityScheme.hpp b/rmf_task/include/rmf_task/BinaryPriorityScheme.hpp index a083f1cb..227df56d 100644 --- a/rmf_task/include/rmf_task/BinaryPriorityScheme.hpp +++ b/rmf_task/include/rmf_task/BinaryPriorityScheme.hpp @@ -26,15 +26,16 @@ namespace rmf_task { //============================================================================== -/// This is for a binary prioritization scheme. Tasks are assigned either high priority or low priority. +/// A class that serves as a binary prioritization scheme by genrating either +/// high or low Priority objects for requests. class BinaryPriorityScheme { public: /// Use these to assign the task priority - // In the current implementation this returns a nullptr. + /// In the current implementation this returns a nullptr. static std::shared_ptr make_low_priority(); - // Get a shared pointer to a high priority object of the binary prioritization scheme + /// Get a shared pointer to a high priority object of the binary prioritization scheme static std::shared_ptr make_high_priority(); /// Use this to give the appropriate cost calculator to the task planner diff --git a/rmf_task/include/rmf_task/CostCalculator.hpp b/rmf_task/include/rmf_task/CostCalculator.hpp index 2320ddae..b48fa830 100644 --- a/rmf_task/include/rmf_task/CostCalculator.hpp +++ b/rmf_task/include/rmf_task/CostCalculator.hpp @@ -24,6 +24,8 @@ namespace rmf_task { //============================================================================== // Forward declare abstract interface. The definition will remain as internal detail. +/// A class that is used within the TaskPlanner to compute the cost of a set of +/// assignments. class CostCalculator; using CostCalculatorPtr = std::shared_ptr; diff --git a/rmf_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp index 27a288c2..cf216122 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -28,30 +28,34 @@ namespace rmf_task { //============================================================================== -/// Estimates for requests +/// A class to store the time that the AGV should wait till before executing the +/// request and the state of the AGV after finishing the request. +/// Note: The wait time is different from the earliest_start_time specified in +/// the request definition. The wait time may be earlier to ensure that the AGV +/// arrvies at the first location of the request by the earliest_start_time class Estimate { public: - /// Constructor of an estimate of a task request. + /// Constructor of an estimate of the request. /// /// \param[in] finish_state - /// Finish state of the robot once it completes the task request. + /// Finish state of the robot once it completes the request. /// /// \param[in] wait_until - /// The ideal time the robot starts executing this task. + /// The ideal time the robot starts executing this request. Estimate(agv::State finish_state, rmf_traffic::Time wait_until); - /// Finish state of the robot once it completes the task request. + /// Finish state of the robot once it completes the 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. + /// The ideal time the robot starts executing this request. rmf_traffic::Time wait_until() const; - /// Sets a new starting time for the robot to execute the task request. + /// Sets a new starting time for the robot to execute the request. Estimate& wait_until(rmf_traffic::Time new_wait_until); class Implementation; @@ -60,7 +64,7 @@ class Estimate }; //============================================================================== -/// Stores computed estimates between pairs of waypoints +/// A class to cache the computed estimates between pairs of waypoints class EstimateCache { public: diff --git a/rmf_task/include/rmf_task/Priority.hpp b/rmf_task/include/rmf_task/Priority.hpp index 872f0b0a..702729c3 100644 --- a/rmf_task/include/rmf_task/Priority.hpp +++ b/rmf_task/include/rmf_task/Priority.hpp @@ -24,6 +24,7 @@ namespace rmf_task { //============================================================================== // Forward declare abstract interface. The definition will remain as internal detail. +/// A class to specify the priority for a request class Priority; using PriorityPtr = std::shared_ptr; using ConstPriorityPtr = std::shared_ptr; diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index a35a3530..e59cd55f 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__TASK_HPP -#define RMF_TASK__TASK_HPP +#ifndef RMF_TASK__REQUEST_HPP +#define RMF_TASK__REQUEST_HPP #include @@ -36,6 +36,8 @@ namespace rmf_task { class Request { public: + /// An abstract interface for computing the estimate and invariant durations + /// of this request class Model { public: @@ -53,10 +55,21 @@ class Request virtual ~Model() = default; }; + /// An abstract interface to define the specifics of this request. This + /// implemented description will differentiate this request from others. class Description { public: + /// Generate a Model for this request based on the unique traits of this + /// description + /// + /// \param[in] earliest_start_time + /// The earliest time this request should begin execution. This is usually the + /// requested start time for the request. + /// + /// \param[in] parameters + /// The parameters that describe this AGV virtual std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, const agv::Parameters& parameters) const = 0; @@ -69,8 +82,7 @@ class Request /// Constructor /// /// \param[in] earliest_start_time - /// The earliest time this request should begin execution. This is usually the - /// requested start time for the request. + /// The desired start time for this request /// /// \param[in] priority /// The priority for this request. This is provided by the Priority Scheme. For @@ -78,11 +90,15 @@ class Request /// /// \param[in] description /// The description for this request + /// + /// \param[in] automatic + /// True if this request is auto-generated Request( const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - DescriptionPtr description); + DescriptionPtr description, + bool automatic = false); /// The unique id for this request const std::string& id() const; @@ -96,6 +112,9 @@ class Request /// Get the description of this request const DescriptionPtr& description() const; + // Returns true if this request was automatically generated + bool automatic() const; + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_task/include/rmf_task/RequestFactory.hpp b/rmf_task/include/rmf_task/RequestFactory.hpp new file mode 100644 index 00000000..2698d811 --- /dev/null +++ b/rmf_task/include/rmf_task/RequestFactory.hpp @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef RMF_TASK__REQUESTFACTORY_HPP +#define RMF_TASK__REQUESTFACTORY_HPP + +#include + +namespace rmf_task { + +//============================================================================== +/// An abstract interface for generating a tailored request for an AGV given +class RequestFactory +{ +public: + + /// Generate a request for the AGV given the state that the robot will have + /// when it is ready to perform the request + virtual ConstRequestPtr make_request( + const agv::State& state) const = 0; + + virtual ~RequestFactory() = default; +}; + +using RequestFactoryPtr = std::shared_ptr; +using ConstRequestFactoryPtr = std::shared_ptr; + +} // namespace rmf_task + +#endif // RMF_TASK__REQUESTFACTORY_HPP diff --git a/rmf_task/include/rmf_task/agv/Constraints.hpp b/rmf_task/include/rmf_task/agv/Constraints.hpp index fe19d6ee..718685b9 100644 --- a/rmf_task/include/rmf_task/agv/Constraints.hpp +++ b/rmf_task/include/rmf_task/agv/Constraints.hpp @@ -24,6 +24,8 @@ namespace rmf_task { namespace agv { //============================================================================== +/// A class that describes constraints that are common among the agents/AGVs +/// available for performing requests class Constraints { public: diff --git a/rmf_task/include/rmf_task/agv/Parameters.hpp b/rmf_task/include/rmf_task/agv/Parameters.hpp index 26631569..08bf0f06 100644 --- a/rmf_task/include/rmf_task/agv/Parameters.hpp +++ b/rmf_task/include/rmf_task/agv/Parameters.hpp @@ -30,6 +30,8 @@ namespace rmf_task { namespace agv { //============================================================================== +/// A class that containts parameters that are common among the agents/AGVs +/// available for performing requests class Parameters { public: diff --git a/rmf_task/include/rmf_task/agv/State.hpp b/rmf_task/include/rmf_task/agv/State.hpp index b1db7c77..957ac793 100644 --- a/rmf_task/include/rmf_task/agv/State.hpp +++ b/rmf_task/include/rmf_task/agv/State.hpp @@ -29,7 +29,7 @@ namespace rmf_task { namespace agv { //============================================================================== -/// This state representation is used for task planning. +/// A class that is used to describe the state of an AGV class State { public: diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 1020a4d8..5d3120e7 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -19,6 +19,7 @@ #define RMF_TASK__AGV__TASKPLANNER_HPP #include +#include #include #include #include @@ -83,6 +84,55 @@ class TaskPlanner rmf_utils::impl_ptr _pimpl; }; + /// The Options class contains planning parameters that can change between + /// each planning attempt. + class Options + { + public: + + /// Constructor + /// + /// \param[in] greedy + /// If true, a greedy approach will be used to solve for the task + /// assignments. Optimality is not guaranteed but the solution time may be + /// faster. If false, an A* based approach will be used within the planner + /// which guarantees optimality but may take longer to solve. + /// + /// \param[in] interrupter + /// A function that can determine whether the planning should be interrupted. + /// + /// \param[in] finishing_request + /// A request factory that generates a tailored task for each agent/AGV + /// to perform at the end of their assignments + Options( + bool greedy, + std::function interrupter = nullptr, + ConstRequestFactoryPtr finishing_request = nullptr); + + /// Set whether a greedy approach should be used + Options& greedy(bool value); + + /// Get whether a greedy approach will be used + bool greedy() const; + + /// Set an interrupter callback that will indicate to the planner if it + /// should stop trying to plan + Options& interrupter(std::function interrupter); + + /// Get the interrupter that will be used in this Options + const std::function& interrupter() const; + + /// Set the request factory that will generate a finishing task + Options& finishing_request(ConstRequestFactoryPtr finishing_request); + + /// Get the request factory that will generate a finishing task + ConstRequestFactoryPtr finishing_request() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + class Assignment { public: @@ -139,30 +189,60 @@ class TaskPlanner /// Constructor /// /// \param[in] configuration - /// The configuration for the planner - TaskPlanner(const Configuration& configuration); - - /// Get the configuration of this task planner + /// The configuration for the planner + /// + /// \param[in] default_options + /// Default options for the task planner to use when solving for assignments. + /// These options can be overriden each time a plan is requested. + TaskPlanner( + Configuration configuration, + Options default_options); + + /// Get a const reference to configuration of this task planner const Configuration& configuration() const; - /// Get the greedy planner based assignments for a set of initial states and - /// requests - Result greedy_plan( + /// Get a const reference to the default planning options. + const Options& default_options() const; + + /// Get a mutable reference to the default planning options. + Options& default_options(); + + /// Generate assignments for requests among available agents. The default + /// Options of this TaskPlanner instance will be used. + /// + /// \param[in] time_now + /// The current time when this plan is requested + /// + /// \param[in] agents + /// The initial states of the agents/AGVs that can undertake the requests + /// + /// \param[in] requests + /// The set of requests that need to be assigned among the agents/AGVs + Result plan( rmf_traffic::Time time_now, std::vector agents, std::vector requests); - /// Get the optimal planner based assignments for a set of initial states and - /// requests - /// \note When the number of requests exceed 10 for the same start time - /// segment, this plan may take a while to be generated. Hence, it is - /// recommended to call greedy_plan() method and use the greedy solution for bidding. - /// If a bid is awarded, the optimal solution may be used for assignments. - Result optimal_plan( + /// Generate assignments for requests among available agents. Override the + /// default parameters + /// + /// \param[in] time_now + /// The current time when this plan is requested + /// + /// \param[in] agents + /// The initial states of the agents/AGVs that can undertake the requests + /// + /// \param[in] requests + /// The set of requests that need to be assigned among the agents/AGVs + /// + /// \param[in] options + /// The options to use for this plan. This overrides the default Options of + /// the TaskPlanner instance + Result plan( rmf_traffic::Time time_now, std::vector agents, std::vector requests, - std::function interrupter); + Options options); /// Compute the cost of a set of assignments double compute_cost(const Assignments& assignments) const; diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index aa235fc5..1171dbaf 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -36,18 +36,24 @@ namespace rmf_task { namespace requests { //============================================================================== +/// A class that generates a Request which requires an AGV to return to its +/// desginated charging_waypoint as specified in its agv::State and wait till +/// its battery charges up to the recharge_soc confugred in agv::Constraints class ChargeBattery { public: + // Forward declare the model for this request class Model; class Description : public Request::Description { public: + /// Generate the description for this request static DescriptionPtr make(); + /// Documentation inherited std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, const agv::Parameters& parameters) const final; @@ -58,9 +64,20 @@ class ChargeBattery rmf_utils::impl_ptr _pimpl; }; + /// Generate a chargebattery request + /// + /// \param[in] earliest_start_time + /// The desired start time for this request + /// + /// \param[in] priority + /// The priority for this request + /// + /// \param[in] automatic + /// True if this request is auto-generated static ConstRequestPtr make( rmf_traffic::Time earliest_start_time, - ConstPriorityPtr priority = nullptr); + ConstPriorityPtr priority = nullptr, + bool automatic = true); }; } // namespace requests diff --git a/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp b/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp new file mode 100644 index 00000000..3a4b0f62 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef RMF_TASK__REQUESTS__FACTORY__CHARGEBATTERYFACTORY_HPP +#define RMF_TASK__REQUESTS__FACTORY__CHARGEBATTERYFACTORY_HPP + +#include +#include + +#include + +namespace rmf_task { +namespace requests { + +//============================================================================== +/// The ChargeBatteryFactory will generate a ChargeBattery request which +/// requires an AGV to return to its desginated charging_waypoint as specified +/// in its agv::State and wait till its battery charges up to the recharge_soc +/// confugred in agv::Constraints recharge_soc specified in its agv::Constraints +class ChargeBatteryFactory : public RequestFactory +{ +public: + + ChargeBatteryFactory(); + + /// Documentation inherited + ConstRequestPtr make_request( + const agv::State& state) const final; + + class Implementation; + +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace requests +} // namespace rmf_task + +#endif // RMF_TASK__REQUESTS__FACTORY__CHARGEBATTERYFACTORY_HPP diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index 758459a5..eb92c482 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -37,21 +37,26 @@ namespace rmf_task { namespace requests { //============================================================================== +/// A class that generates a Request which requires an AGV to perform a cleaning +/// operation at a location class Clean { public: + // Forward declare the model for this request class Model; class Description : public Request::Description { public: + /// Generate the description for this request static DescriptionPtr make( std::size_t start_waypoint, std::size_t end_waypoint, const rmf_traffic::Trajectory& cleaning_path); + /// Documentation inherited std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, const agv::Parameters& parameters) const final; @@ -68,13 +73,40 @@ class Clean rmf_utils::impl_ptr _pimpl; }; + /// Generate a clean request + /// + /// \param[in] start_waypoint + /// The graph index for the location where the AGV should begin its cleaning + /// operation + /// + /// \param[in] end_waypoint + /// The graph index for the location where the AGV ends up after its cleaning + /// operation + /// + /// \param[in] cleaning_path + /// A trajectory that describes the motion of the AGV during the cleaning + /// operation. This is used to determine the process duration and expected + /// battery drain + /// + /// \param[in] id + /// A unique id for this request + /// + /// \param[in] earliest_start_time + /// The desired start time for this request + /// + /// \param[in] priority + /// The priority for this request + /// + /// \param[in] automatic + /// True if this request is auto-generated static ConstRequestPtr make( std::size_t start_waypoint, std::size_t end_waypoint, const rmf_traffic::Trajectory& cleaning_path, const std::string& id, rmf_traffic::Time earliest_start_time, - ConstPriorityPtr priority = nullptr); + ConstPriorityPtr priority = nullptr, + bool automatic = false); }; } // namespace requests diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 53cf85a0..10a74da3 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -36,10 +36,13 @@ namespace rmf_task { namespace requests { //============================================================================== +/// A class that generates a Request which requires an AGV to pickup items from +/// one location and deliver them to another class Delivery { public: + // Forward declare the Model for this request class Model; class Description : public Request::Description @@ -48,12 +51,14 @@ class Delivery using Start = rmf_traffic::agv::Planner::Start; + /// Generate the description for this request static DescriptionPtr make( std::size_t pickup_waypoint, rmf_traffic::Duration pickup_duration, std::size_t dropoff_waypoint, rmf_traffic::Duration dropoff_duration); + /// Documentation inherited std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, const agv::Parameters& parameters) const final; @@ -76,7 +81,33 @@ class Delivery rmf_utils::impl_ptr _pimpl; }; - + /// Generate a delivery request + /// + /// \param[in] pickup_waypoint + /// The graph index for the pickup location + /// + /// \param[in] pickup_wait + /// The expected duration the AGV has to wait at the pickup location for + /// the items to be loaded + /// + /// \param[in] dropoff_waypoint + /// The graph index for the dropoff location + /// + /// \param[in] dropoff_wait + /// The expected duration the AGV has to wait at the dropoff location for + /// the items to be unloaded + /// + /// \param[in] id + /// A unique id for this request + /// + /// \param[in] earliest_start_time + /// The desired start time for this request + /// + /// \param[in] priority + /// The priority for this request + /// + /// \param[in] automatic + /// True if this request is auto-generated static ConstRequestPtr make( std::size_t pickup_waypoint, rmf_traffic::Duration pickup_wait, @@ -84,7 +115,8 @@ class Delivery rmf_traffic::Duration dropoff_wait, const std::string& id, rmf_traffic::Time earliest_start_time, - ConstPriorityPtr priority = nullptr); + ConstPriorityPtr priority = nullptr, + bool automatic = false); }; } // namespace requests diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index 6cbe717a..33c74e53 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -38,21 +38,26 @@ namespace rmf_task { namespace requests { //============================================================================== +/// A class that generates a Request which requires an AGV to repeatedly travel +/// between two locations class Loop { public: + // Forward declare the Model for this request class Model; class Description : public Request::Description { public: + /// Generate the description for this request static DescriptionPtr make( std::size_t start_waypoint, std::size_t finish_waypoint, std::size_t num_loops); + /// Documentation inherited std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, const agv::Parameters& parameters) const final; @@ -73,13 +78,37 @@ class Loop rmf_utils::impl_ptr _pimpl; }; + /// Generate a loop request + /// + /// \param[in] start_waypoint + /// The graph index for the starting waypoint of the loop + /// + /// \param[in] finish_waypoint + /// The graph index for the finishing waypoint of the loop + /// + /// \param[in] num_loops + /// The number of times the AGV should loop between the start_waypoint and + /// finish_waypoint + /// + /// \param[in] id + /// A unique id for this request + /// + /// \param[in] earliest_start_time + /// The desired start time for this request + /// + /// \param[in] priority + /// The priority for this request + /// + /// \param[in] automatic + /// True if this request is auto-generated static ConstRequestPtr make( std::size_t start_waypoint, std::size_t finish_waypoint, std::size_t num_loops, const std::string& id, rmf_traffic::Time earliest_start_time, - ConstPriorityPtr priority = nullptr); + ConstPriorityPtr priority = nullptr, + bool automatic = false); }; } // namespace tasks diff --git a/rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp b/rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp new file mode 100644 index 00000000..254167b1 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef RMF_TASK__REQUESTS__FACTORY__PARKROBOTFACTORY_HPP +#define RMF_TASK__REQUESTS__FACTORY__PARKROBOTFACTORY_HPP + +#include +#include + +#include + +#include + +namespace rmf_task { +namespace requests { + +//============================================================================== +/// The ParkRobotFactory will generate a request for the AGV to return to +/// its desginated parking spot and remain idle there. This factory may be used +/// when AGVs should not remain idle at the location of their last task but +/// rather wait for new orders at their designated parking spots. +class ParkRobotFactory : public RequestFactory +{ +public: + + /// Constructor + /// + /// \param[in] parking_waypoint + /// The graph index of the waypoint assigned to this AGV for parking. + /// If nullopt, the AGV will return to its charging_waypoint and remain idle + /// there. It will not wait for its battery to charge up before undertaking + /// new tasks. + ParkRobotFactory( + std::optional parking_waypoint = std::nullopt); + + /// Documentation inherited + ConstRequestPtr make_request( + const agv::State& state) const final; + + class Implementation; + +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace requests +} // namespace rmf_task + +#endif // RMF_TASK__REQUESTS__FACTORY__PARKROBOTFACTORY_HPP diff --git a/rmf_task/src/rmf_task/Request.cpp b/rmf_task/src/rmf_task/Request.cpp index 75f9e6b7..9530c7c4 100644 --- a/rmf_task/src/rmf_task/Request.cpp +++ b/rmf_task/src/rmf_task/Request.cpp @@ -27,6 +27,7 @@ class Request::Implementation rmf_traffic::Time earliest_start_time; rmf_task::ConstPriorityPtr priority; DescriptionPtr description; + bool automatic; }; //============================================================================== @@ -34,13 +35,15 @@ Request::Request( const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - DescriptionPtr description) + DescriptionPtr description, + bool automatic) : _pimpl(rmf_utils::make_impl( Implementation { id, earliest_start_time, std::move(priority), - std::move(description) + std::move(description), + automatic })) { // Do nothing @@ -70,4 +73,10 @@ const Request::DescriptionPtr& Request::description() const return _pimpl->description; } -} // namespace rmf_task \ No newline at end of file +//============================================================================== +bool Request::automatic() const +{ + return _pimpl->automatic; +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 4cbb1879..575986f0 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -98,6 +98,72 @@ auto TaskPlanner::Configuration::cost_calculator( return *this; } +//============================================================================== +class TaskPlanner::Options::Implementation +{ +public: + + bool greedy; + std::function interrupter; + ConstRequestFactoryPtr finishing_request; +}; + +//============================================================================== +TaskPlanner::Options::Options( + bool greedy, + std::function interrupter, + ConstRequestFactoryPtr finishing_request) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(greedy), + std::move(interrupter), + std::move(finishing_request) + })) +{ + // Do nothing +} + +//============================================================================== +auto TaskPlanner::Options::greedy(bool value) -> Options& +{ + _pimpl->greedy = value; + return *this; +} + +//============================================================================== +bool TaskPlanner::Options::greedy() const +{ + return _pimpl->greedy; +} + +//============================================================================== +auto TaskPlanner::Options::interrupter(std::function interrupter) +-> Options& +{ + _pimpl->interrupter = std::move(interrupter); + return *this; +} + +//============================================================================== +const std::function& TaskPlanner::Options::interrupter() const +{ + return _pimpl->interrupter; +} + +//============================================================================== +auto TaskPlanner::Options::finishing_request( + ConstRequestFactoryPtr finishing_request) -> Options& +{ + _pimpl->finishing_request = std::move(finishing_request); + return *this; +} + +//============================================================================== +ConstRequestFactoryPtr TaskPlanner::Options::finishing_request() const +{ + return _pimpl->finishing_request; +} + //============================================================================== class TaskPlanner::Assignment::Implementation { @@ -306,6 +372,7 @@ class TaskPlanner::Implementation public: Configuration config; + Options default_options; std::shared_ptr estimate_cache; bool check_priority = false; ConstCostCalculatorPtr cost_calculator = nullptr; @@ -352,11 +419,91 @@ class TaskPlanner::Implementation return node; } + void append_finishing_request( + const RequestFactory& factory, + TaskPlanner::Assignments& complete_assignments) + { + for (auto& agent : complete_assignments) + { + if (agent.empty()) + { + continue; + } + + const auto& state = agent.back().state(); + const auto request = factory.make_request(state); + + // TODO(YV) Currently we are unable to recursively call complete_solve() + // here as the prune_assignments() function will remove any ChargeBattery + // requests at the back of the assignments. But the finishing factory + // could be a ChargeBattery request and hence this approach does not work. + // When we fix the logic with unnecessary ChargeBattery tasks, we should + // revist making this a recursive call. + auto model = request->description()->make_model( + state.finish_time(), + config.parameters()); + auto estimate = model->estimate_finish( + state, config.constraints(), *estimate_cache); + if (estimate.has_value()) + { + agent.push_back( + Assignment + { + request, + estimate.value().finish_state(), + estimate.value().wait_until() + }); + } + else + { + // Insufficient battery to perform the finishing request. We check if + // adding a ChargeBattery task before will allow for it to be performed + const auto charging_request = + make_charging_request(state.finish_time()); + const auto charge_battery_model = + charging_request->description()->make_model( + state.finish_time(), + config.parameters()); + const auto charge_battery_estimate = + charge_battery_model->estimate_finish( + state, config.constraints(), *estimate_cache); + if (charge_battery_estimate.has_value()) + { + model = request->description()->make_model( + charge_battery_estimate.value().finish_state().finish_time(), + config.parameters()); + estimate = model->estimate_finish( + charge_battery_estimate.value().finish_state(), + config.constraints(), + *estimate_cache); + if (estimate.has_value()) + { + // Append the ChargeBattery and finishing request + agent.push_back( + Assignment{ + charging_request, + charge_battery_estimate.value().finish_state(), + charge_battery_estimate.value().wait_until() + }); + agent.push_back( + Assignment + { + request, + estimate.value().finish_state(), + estimate.value().wait_until() + }); + } + } + } + } + } + Result complete_solve( rmf_traffic::Time time_now, std::vector& initial_states, const std::vector& requests, const std::function interrupter, + ConstRequestFactoryPtr finishing_request, bool greedy) { @@ -410,7 +557,13 @@ class TaskPlanner::Implementation if (node->unassigned_tasks.empty()) { - return prune_assignments(complete_assignments); + auto pruned_assignments = prune_assignments(complete_assignments); + if (finishing_request != nullptr) + { + append_finishing_request(*finishing_request, pruned_assignments); + } + + return pruned_assignments; } std::vector new_tasks; @@ -440,6 +593,13 @@ class TaskPlanner::Implementation initial_states = estimates; } + // If a finishing_request is present, accommodate the request at the end of + // the assignments for each agent + if (finishing_request != nullptr) + { + append_finishing_request(*finishing_request, complete_assignments); + } + return complete_assignments; } @@ -922,10 +1082,12 @@ class TaskPlanner::Implementation // ============================================================================ TaskPlanner::TaskPlanner( - const rmf_task::agv::TaskPlanner::Configuration& configuration) + Configuration configuration, + Options default_options) : _pimpl(rmf_utils::make_impl( Implementation{ configuration, + default_options, std::make_shared( configuration.parameters().planner()-> get_configuration().graph().num_waypoints()) @@ -935,7 +1097,7 @@ TaskPlanner::TaskPlanner( } // ============================================================================ -auto TaskPlanner::greedy_plan( +auto TaskPlanner::plan( rmf_traffic::Time time_now, std::vector agents, std::vector requests) -> Result @@ -944,23 +1106,25 @@ auto TaskPlanner::greedy_plan( time_now, agents, requests, - nullptr, - true); + _pimpl->default_options.interrupter(), + _pimpl->default_options.finishing_request(), + _pimpl->default_options.greedy()); } // ============================================================================ -auto TaskPlanner::optimal_plan( +auto TaskPlanner::plan( rmf_traffic::Time time_now, std::vector agents, std::vector requests, - std::function interrupter) -> Result + Options options) -> Result { return _pimpl->complete_solve( time_now, agents, requests, - interrupter, - false); + options.interrupter(), + options.finishing_request(), + options.greedy()); } // ============================================================================ @@ -988,5 +1152,17 @@ const return _pimpl->config; } +// ============================================================================ +auto TaskPlanner::default_options() const -> const Options& +{ + return _pimpl->default_options; +} + +// ============================================================================ +auto TaskPlanner::default_options() -> Options& +{ + return _pimpl->default_options; +} + } // namespace agv } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index ff9d0ab9..ac5b1ed3 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -226,14 +226,15 @@ std::shared_ptr ChargeBattery::Description::make_model( //============================================================================== ConstRequestPtr ChargeBattery::make( rmf_traffic::Time earliest_start_time, - ConstPriorityPtr priority) + ConstPriorityPtr priority, + bool automatic) { std::string id = "Charge" + generate_uuid(); const auto description = Description::make(); return std::make_shared( - id, earliest_start_time, priority, description); + id, earliest_start_time, priority, description, automatic); } diff --git a/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp new file mode 100644 index 00000000..b3219fc6 --- /dev/null +++ b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +namespace rmf_task { +namespace requests { + +//============================================================================== +class ChargeBatteryFactory::Implementation +{ + +}; + +//============================================================================== +ChargeBatteryFactory::ChargeBatteryFactory() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + +//============================================================================== +ConstRequestPtr ChargeBatteryFactory::make_request( + const agv::State& state) const +{ + return ChargeBattery::make(state.finish_time()); +} + +} // namespace requests +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index dfd8a748..386b668b 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -333,7 +333,8 @@ ConstRequestPtr Clean::make( const rmf_traffic::Trajectory& cleaning_path, const std::string& id, rmf_traffic::Time earliest_start_time, - ConstPriorityPtr priority) + ConstPriorityPtr priority, + bool automatic) { const auto description = Clean::Description::make( start_waypoint, @@ -341,7 +342,7 @@ ConstRequestPtr Clean::make( cleaning_path); return std::make_shared( - id, earliest_start_time, priority, description); + id, earliest_start_time, priority, description, automatic); } } // namespace requests diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index c1a356b8..5a7734ab 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -358,7 +358,8 @@ ConstRequestPtr Delivery::make( rmf_traffic::Duration dropoff_wait, const std::string& id, rmf_traffic::Time earliest_start_time, - ConstPriorityPtr priority) + ConstPriorityPtr priority, + bool automatic) { const auto description = Description::make( pickup_waypoint, @@ -367,7 +368,7 @@ ConstRequestPtr Delivery::make( dropoff_wait); return std::make_shared( - id, earliest_start_time, std::move(priority), description); + id, earliest_start_time, std::move(priority), description, automatic); } } // namespace requests diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 1cc1c8fe..b13978c5 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -347,7 +347,8 @@ ConstRequestPtr Loop::make( std::size_t num_loops, const std::string& id, rmf_traffic::Time earliest_start_time, - ConstPriorityPtr priority) + ConstPriorityPtr priority, + bool automatic) { const auto description = Description::make( start_waypoint, @@ -355,7 +356,7 @@ ConstRequestPtr Loop::make( num_loops); return std::make_shared( - id, earliest_start_time, std::move(priority), description); + id, earliest_start_time, std::move(priority), description, automatic); } diff --git a/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp new file mode 100644 index 00000000..98ccb43b --- /dev/null +++ b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include + +namespace rmf_task { +namespace requests { + +//============================================================================== +namespace { +std::string generate_uuid(const std::size_t length = 3) +{ + std::stringstream ss; + for (std::size_t i = 0; i < length; ++i) + { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dis(0, 255); + const auto random_char = dis(gen); + std::stringstream hexstream; + hexstream << std::hex << random_char; + auto hex = hexstream.str(); + ss << (hex.length() < 2 ? '0' + hex : hex); + } + return ss.str(); +} + +} // anonymous namespace + +//============================================================================== +class ParkRobotFactory::Implementation +{ +public: + + std::optional parking_waypoint; +}; + +//============================================================================== +ParkRobotFactory::ParkRobotFactory( + std::optional parking_waypoint) +: _pimpl(rmf_utils::make_impl( + Implementation{ + parking_waypoint + })) +{ + // Do nothing +} + +//============================================================================== +ConstRequestPtr ParkRobotFactory::make_request( + const agv::State& state) const +{ + std::string id = "ReturnToCharger" + generate_uuid(); + const auto start_waypoint = state.location().waypoint(); + const auto finish_waypoint = _pimpl->parking_waypoint.has_value() ? + _pimpl->parking_waypoint.value() : state.charging_waypoint(); + const auto request = Loop::make( + start_waypoint, + finish_waypoint, + 1, + id, + state.finish_time(), + nullptr, + true); + + return request; +} + +} // namespace requests +} // namespace rmf_task diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 82889ea4..14f9354e 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -23,6 +23,9 @@ #include #include +#include +#include + #include #include @@ -172,12 +175,12 @@ SCENARIO("Grid World") const rmf_traffic::agv::VehicleTraits traits( {1.0, 0.7}, {0.6, 0.5}, profile); rmf_traffic::schedule::Database database; - const auto default_options = rmf_traffic::agv::Planner::Options{ + const auto default_planner_options = rmf_traffic::agv::Planner::Options{ nullptr}; auto planner = std::make_shared( rmf_traffic::agv::Planner::Configuration{graph, traits}, - default_options); + default_planner_options); auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8); REQUIRE(battery_system_optional); @@ -210,6 +213,17 @@ SCENARIO("Grid World") constraints, cost_calculator}; + // But default we use the optimal solver + const auto default_options = TaskPlanner::Options{ + false, + nullptr, + nullptr}; + + const auto greedy_options = TaskPlanner::Options{ + true, + nullptr, + nullptr}; + // Duration for loading/unloading items for delivery tasks const auto delivery_wait = rmf_traffic::time::from_seconds(0); @@ -255,11 +269,11 @@ SCENARIO("Grid World") }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_result = task_planner.greedy_plan( - now, initial_states, requests); + const auto greedy_result = task_planner.plan( + now, initial_states, requests, greedy_options); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); @@ -276,10 +290,10 @@ SCENARIO("Grid World") // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another - task_planner = TaskPlanner(task_config); + task_planner = TaskPlanner(task_config, default_options); start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); @@ -401,11 +415,11 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(60000)) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_result = task_planner.greedy_plan( - now, initial_states, requests); + const auto greedy_result = task_planner.plan( + now, initial_states, requests, greedy_options); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); @@ -422,10 +436,10 @@ SCENARIO("Grid World") // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another - task_planner = TaskPlanner(task_config); + task_planner = TaskPlanner(task_config, default_options); start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); @@ -492,11 +506,11 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(50000)) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_result = task_planner.greedy_plan( - now, initial_states, requests); + const auto greedy_result = task_planner.plan( + now, initial_states, requests, greedy_options); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); @@ -513,10 +527,10 @@ SCENARIO("Grid World") // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another - task_planner = TaskPlanner(task_config); + task_planner = TaskPlanner(task_config, default_options); start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); @@ -648,11 +662,11 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(70000)) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_result = task_planner.greedy_plan( - now, initial_states, requests); + const auto greedy_result = task_planner.plan( + now, initial_states, requests, greedy_options); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); @@ -669,10 +683,10 @@ SCENARIO("Grid World") // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another - task_planner = TaskPlanner(task_config); + task_planner = TaskPlanner(task_config, default_options); start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); @@ -711,9 +725,9 @@ SCENARIO("Grid World") now) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); - const auto greedy_result = task_planner.greedy_plan( + const auto greedy_result = task_planner.plan( now, initial_states, requests); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); @@ -724,9 +738,9 @@ SCENARIO("Grid World") // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another - task_planner = TaskPlanner(task_config); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + task_planner = TaskPlanner(task_config, default_options); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE_FALSE(optimal_assignments); @@ -757,10 +771,10 @@ SCENARIO("Grid World") now) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); - const auto greedy_result = task_planner.greedy_plan( - now, initial_states, requests); + const auto greedy_result = task_planner.plan( + now, initial_states, requests, greedy_options); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE_FALSE(greedy_assignments); @@ -770,9 +784,9 @@ SCENARIO("Grid World") // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another - task_planner = TaskPlanner(task_config); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + task_planner = TaskPlanner(task_config, default_options); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE_FALSE(optimal_assignments); @@ -821,11 +835,11 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0)) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -857,10 +871,10 @@ SCENARIO("Grid World") } // Reset the planner cache - task_planner = TaskPlanner(task_config); + task_planner = TaskPlanner(task_config, default_options); start_time = std::chrono::steady_clock::now(); - const auto new_optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto new_optimal_result = task_planner.plan( + now, initial_states, requests); const auto new_optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&new_optimal_result); REQUIRE(new_optimal_assignments_ptr); @@ -923,11 +937,11 @@ SCENARIO("Grid World") rmf_task::BinaryPriorityScheme::make_high_priority()) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -996,11 +1010,11 @@ SCENARIO("Grid World") rmf_task::BinaryPriorityScheme::make_high_priority()) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -1078,11 +1092,11 @@ SCENARIO("Grid World") rmf_task::BinaryPriorityScheme::make_high_priority()) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -1160,11 +1174,11 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0)) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -1227,8 +1241,8 @@ SCENARIO("Grid World") }; auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -1305,11 +1319,11 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0)) }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -1381,8 +1395,8 @@ SCENARIO("Grid World") }; auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -1473,11 +1487,11 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(50000)) }; - TaskPlanner task_planner(new_task_config); + TaskPlanner task_planner(new_task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_result = task_planner.greedy_plan( - now, initial_states, requests); + const auto greedy_result = task_planner.plan( + now, initial_states, requests, greedy_options); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); @@ -1494,10 +1508,10 @@ SCENARIO("Grid World") // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another - task_planner = TaskPlanner(new_task_config); + task_planner = TaskPlanner(new_task_config, default_options); start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); @@ -1565,10 +1579,10 @@ SCENARIO("Grid World") start_time), }; - TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config, default_options); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE(optimal_assignments_ptr); @@ -1641,11 +1655,11 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(50000)) }; - TaskPlanner task_planner(new_task_config); + TaskPlanner task_planner(new_task_config, default_options); auto start_time = std::chrono::steady_clock::now(); - const auto optimal_result = task_planner.optimal_plan( - now, initial_states, requests, nullptr); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); auto finish_time = std::chrono::steady_clock::now(); const auto optimal_assignments_ptr = std::get_if< TaskPlanner::Assignments>(&optimal_result); @@ -1668,4 +1682,220 @@ SCENARIO("Grid World") } + WHEN("Planning without and with ChargeBatteryFactory") + { + 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}; + rmf_traffic::agv::Plan::Start second_location{now, 1, default_orientation}; + + std::vector initial_states = + { + rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::agv::State{second_location, 1, 1.0}, + }; + + std::vector requests = + { + rmf_task::requests::Loop::make( + 0, + 15, + 1, + "Loop1", + now), + rmf_task::requests::Loop::make( + 0, + 14, + 1, + "Loop2", + now), + rmf_task::requests::Loop::make( + 3, + 4, + 1, + "Loop3", + now), + }; + + TaskPlanner task_planner(task_config, default_options); + + THEN("When ChargeBatteryFactory is not supplied during planning") + { + auto start_time = std::chrono::steady_clock::now(); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); + const auto optimal_assignments_ptr = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + REQUIRE(optimal_assignments_ptr); + const auto& optimal_assignments = *optimal_assignments_ptr; + const double optimal_cost = + task_planner.compute_cost(optimal_assignments); + auto finish_time = std::chrono::steady_clock::now(); + CHECK_TIMES(optimal_assignments, now); + + if (display_solutions) + { + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Optimal", optimal_assignments, optimal_cost); + } + + // Check that the last assignment for each agent is not a ChargeBattery request + for (const auto& agent : optimal_assignments) + { + const auto last_assignment = agent.back(); + auto is_charge_request = + std::dynamic_pointer_cast< + const rmf_task::requests::ChargeBattery::Description>( + last_assignment.request()->description()); + CHECK_FALSE(is_charge_request); + } + } + + THEN("When ChargeBatteryFactory is supplied during planning") + { + const auto finishing_request = + std::make_shared(); + task_planner.default_options().finishing_request(finishing_request); + REQUIRE(task_planner.default_options().finishing_request() != nullptr); + + auto start_time = std::chrono::steady_clock::now(); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); + const auto optimal_assignments_ptr = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + REQUIRE(optimal_assignments_ptr); + const auto& optimal_assignments = *optimal_assignments_ptr; + const double optimal_cost = + task_planner.compute_cost(optimal_assignments); + auto finish_time = std::chrono::steady_clock::now(); + CHECK_TIMES(optimal_assignments, now); + + if (display_solutions) + { + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Optimal", optimal_assignments, optimal_cost); + } + + // Check that the last assignment for each agent is a ChargeBattery request + for (const auto& agent : optimal_assignments) + { + const auto last_assignment = agent.back(); + auto is_charge_request = + std::dynamic_pointer_cast< + const rmf_task::requests::ChargeBattery::Description>( + last_assignment.request()->description()); + CHECK(is_charge_request); + } + } + } + + WHEN("Planning without and with ParkRobotFactory") + { + 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}; + rmf_traffic::agv::Plan::Start second_location{now, 1, default_orientation}; + + std::vector initial_states = + { + rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::agv::State{second_location, 1, 1.0}, + }; + + std::vector requests = + { + rmf_task::requests::Loop::make( + 0, + 15, + 1, + "Loop1", + now), + rmf_task::requests::Loop::make( + 0, + 14, + 1, + "Loop2", + now), + rmf_task::requests::Loop::make( + 3, + 4, + 1, + "Loop3", + now), + }; + + TaskPlanner task_planner(task_config, default_options); + + THEN("When ParkRobotFactory is not supplied during planning") + { + auto start_time = std::chrono::steady_clock::now(); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); + const auto optimal_assignments_ptr = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + REQUIRE(optimal_assignments_ptr); + const auto& optimal_assignments = *optimal_assignments_ptr; + const double optimal_cost = + task_planner.compute_cost(optimal_assignments); + auto finish_time = std::chrono::steady_clock::now(); + CHECK_TIMES(optimal_assignments, now); + + if (display_solutions) + { + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Optimal", optimal_assignments, optimal_cost); + } + + // Check that the final location for each agent is not its charging waypoint + for (const auto& agent : optimal_assignments) + { + const auto last_assignment = agent.back(); + CHECK_FALSE(last_assignment.request()->automatic()); + const auto& state = last_assignment.state(); + CHECK_FALSE(state.location().waypoint() == state.charging_waypoint()); + } + } + + THEN("When ParkRobotFactory is supplied during planning") + { + const auto finishing_request = + std::make_shared(); + task_planner.default_options().finishing_request(finishing_request); + REQUIRE(task_planner.default_options().finishing_request() != nullptr); + + auto start_time = std::chrono::steady_clock::now(); + const auto optimal_result = task_planner.plan( + now, initial_states, requests); + const auto optimal_assignments_ptr = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + REQUIRE(optimal_assignments_ptr); + const auto& optimal_assignments = *optimal_assignments_ptr; + const double optimal_cost = + task_planner.compute_cost(optimal_assignments); + auto finish_time = std::chrono::steady_clock::now(); + CHECK_TIMES(optimal_assignments, now); + + if (display_solutions) + { + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Optimal", optimal_assignments, optimal_cost); + } + + // Check that the final location for each agent is its charging waypoint + for (const auto& agent : optimal_assignments) + { + const auto last_assignment = agent.back(); + CHECK(last_assignment.request()->automatic()); + const auto& state = last_assignment.state(); + CHECK(state.location().waypoint() == state.charging_waypoint()); + } + } + } + }