Allow task planner to append a finishing task when robot goes idle (#28)

Signed-off-by: Yadunund <yadunund@openrobotics.org>

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"
 )
 

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<Priority> 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<Priority> make_high_priority();
 
   /// Use this to give the appropriate cost calculator to the task planner

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<CostCalculator>;

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:

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<Priority>;
 using ConstPriorityPtr = std::shared_ptr<const Priority>;

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 <memory>
 
@@ -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<Model> make_model(
       rmf_traffic::Time earliest_start_time,
       const agv::Parameters& parameters) const = 0;
@@ -69,20 +82,23 @@ 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
   ///   requests that do not have any priority this is a nullptr.
   ///
   /// \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<Implementation> _pimpl;

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 <rmf_task/Request.hpp>
+
+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<RequestFactory>;
+using ConstRequestFactoryPtr = std::shared_ptr<const RequestFactory>;
+
+} // namespace rmf_task
+
+#endif // RMF_TASK__REQUESTFACTORY_HPP

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:

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:

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:

rmf_task/include/rmf_task/agv/TaskPlanner.hpp

@@ -19,6 +19,7 @@
 #define RMF_TASK__AGV__TASKPLANNER_HPP
 
 #include <rmf_task/Request.hpp>
+#include <rmf_task/RequestFactory.hpp>
 #include <rmf_task/CostCalculator.hpp>
 #include <rmf_task/agv/Constraints.hpp>
 #include <rmf_task/agv/Parameters.hpp>
@@ -83,6 +84,55 @@ class TaskPlanner
     rmf_utils::impl_ptr<Implementation> _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<bool()> 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<bool()> interrupter);
+
+    /// Get the interrupter that will be used in this Options
+    const std::function<bool()>& 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<Implementation> _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<State> agents,
     std::vector<ConstRequestPtr> 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<State> agents,
     std::vector<ConstRequestPtr> requests,
-    std::function<bool()> interrupter);
+    Options options);
 
   /// Compute the cost of a set of assignments
   double compute_cost(const Assignments& assignments) const;