Using rmf_traffic::agv::Plan::Start for holding waypoint and time information

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;

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<Request>;
 
-  // 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> 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;
 
   virtual ~Request() = default;

rmf_tasks/include/rmf_tasks/agv/State.hpp

@@ -24,45 +24,67 @@
 #include <rmf_utils/optional.hpp>
 
 #include <rmf_traffic/Time.hpp>
+#include <rmf_traffic/agv/Planner.hpp>
 
 namespace rmf_tasks {
 namespace agv {
 
+/// This state representation is used for task planning.
 class State
 {
 public:
 
   /// Constructor
   ///
-  /// \param[in] waypoint
+  /// \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
-  /// \param[in] finish_time
+  ///   The charging waypoint index of this robot.
+  ///
   /// \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, 
+    rmf_traffic::agv::Plan::Start plan_start,
     std::size_t charging_waypoint,
-    rmf_traffic::Time finish_time = std::chrono::steady_clock::now(),
-    double battery_soc = 1.0);
-
-  State();
+    double battery_soc);
 
-  std::size_t waypoint() const;
+  /// The next plan start based on the current state.
+  rmf_traffic::agv::Plan::Start plan_start() const;
 
-  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;
 
+  /// Sets the charging waypoint index.
   State& charging_waypoint(std::size_t new_charging_waypoint);
 
-  rmf_traffic::Time finish_time() const;
-
-  State& finish_time(rmf_traffic::Time new_finish_time);
-
+  /// 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);
 
+  /// 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<Implementation> _pimpl;

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;

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<State> initial_states,
     std::vector<StateConfig> state_configs,
     std::vector<Request::SharedPtr> requests);
@@ -138,6 +139,7 @@ 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<State> initial_states,
     std::vector<StateConfig> state_configs,
     std::vector<Request::SharedPtr> requests,

rmf_tasks/include/rmf_tasks/requests/Clean.hpp

@@ -49,8 +49,8 @@ class Clean : public rmf_tasks::Request
     std::shared_ptr<rmf_battery::DevicePowerSink> ambient_sink,
     std::shared_ptr<rmf_battery::DevicePowerSink> cleaning_sink,
     std::shared_ptr<rmf_traffic::agv::Planner> 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;
 

rmf_tasks/include/rmf_tasks/requests/Delivery.hpp

@@ -46,8 +46,8 @@ class Delivery : public rmf_tasks::Request
     std::shared_ptr<rmf_battery::MotionPowerSink> motion_sink,
     std::shared_ptr<rmf_battery::DevicePowerSink> device_sink,
     std::shared_ptr<rmf_traffic::agv::Planner> 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;
 

rmf_tasks/src/agv/State.cpp

@@ -15,6 +15,8 @@
  *
 */
 
+#include <stdexcept>
+
 #include <rmf_tasks/agv/State.hpp>
 
 namespace rmf_tasks {
@@ -26,49 +28,42 @@ class State::Implementation
 public:
 
   Implementation(
-    std::size_t waypoint, 
+    rmf_traffic::agv::Plan::Start plan_start,
     std::size_t charging_waypoint,
-    rmf_traffic::Time finish_time,
     double battery_soc)
-  : _waypoint(waypoint),
+  : _plan_start(std::move(plan_start)),
     _charging_waypoint(charging_waypoint),
-    _finish_time(finish_time),
     _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;
+  rmf_traffic::agv::Plan::Start _plan_start;
   std::size_t _charging_waypoint;
-  rmf_traffic::Time _finish_time;
   double _battery_soc;
 };
 
 //==============================================================================
 State::State(
-  std::size_t waypoint, 
+  rmf_traffic::agv::Plan::Start plan_start,
   std::size_t charging_waypoint,
-  rmf_traffic::Time finish_time,
   double battery_soc)
 : _pimpl(rmf_utils::make_impl<Implementation>(
-    Implementation(
-      waypoint, charging_waypoint, finish_time, battery_soc)))
-{}
-
-//==============================================================================
-State::State()
-: _pimpl(rmf_utils::make_impl<Implementation>(
-    Implementation(0, 0, std::chrono::steady_clock::now(), 0.0)))
+    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,30 +81,48 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint)
 }
 
 //==============================================================================
-rmf_traffic::Time State::finish_time() const
+double State::battery_soc() const
 {
-  return _pimpl->_finish_time;
+  return _pimpl->_battery_soc;
 }
 
 //==============================================================================
-State& State::finish_time(rmf_traffic::Time new_finish_time)
+State& State::battery_soc(double new_battery_soc)
 {
-  _pimpl->_finish_time = new_finish_time;
+  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)
 {
-  _pimpl->_battery_soc = new_battery_soc;
+  _pimpl->_plan_start.waypoint(new_waypoint);
+  return *this;
+}
+
+//==============================================================================
+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

rmf_tasks/src/agv/StateConfig.cpp

@@ -15,6 +15,8 @@
  *
 */
 
+#include <stdexcept>
+
 #include <rmf_tasks/agv/StateConfig.hpp>
 
 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;
 }