diff --git a/rmf_tasks/include/rmf_tasks/Estimate.hpp b/rmf_tasks/include/rmf_tasks/Estimate.hpp
index b57c8e79e..d0d41f0c9 100644
--- a/rmf_tasks/include/rmf_tasks/Estimate.hpp
+++ b/rmf_tasks/include/rmf_tasks/Estimate.hpp
@@ -29,14 +29,25 @@ class Estimate
 {
 public:
 
+  /// Constructor of an estimate of a task request.
+  ///
+  /// \param[in] finish_state
+  ///   Finish state of the robot once it completes the task request.
+  ///
+  /// \param[in] wait_until
+  ///   The ideal time the robot starts executing this task.
   Estimate(agv::State finish_state, rmf_traffic::Time wait_until);
 
+  /// Finish state of the robot once it completes the task request.
   agv::State finish_state() const;
 
+  /// Sets a new finish state for the robot.
   Estimate& finish_state(agv::State new_finish_state);
 
+  /// The ideal time the robot starts executing this task.
   rmf_traffic::Time wait_until() const;
 
+  /// Sets a new starting time for the robot to execute the task request.
   Estimate& wait_until(rmf_traffic::Time new_wait_until);
 
   class Implementation;
diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp
index 846fe89fa..c8fbbcf0a 100644
--- a/rmf_tasks/include/rmf_tasks/Request.hpp
+++ b/rmf_tasks/include/rmf_tasks/Request.hpp
@@ -29,26 +29,26 @@
 
 namespace rmf_tasks {
 
-/// Implement this for new tasks
+/// Implement this for new type of requests.
 class Request
 {
 public:
 
   using SharedPtr = std::shared_ptr<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;
diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp
index bd569874b..08eb3d747 100644
--- a/rmf_tasks/include/rmf_tasks/agv/State.hpp
+++ b/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] location
+  ///   Current state's location, which includes the time that the robot can
+  ///   feasibly take on a new task, 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 location,
     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 current state's location.
+  rmf_traffic::agv::Plan::Start location() const;
 
-  State& waypoint(std::size_t new_waypoint);
+  /// Sets the state's location.
+  State& location(rmf_traffic::agv::Plan::Start new_location);
 
+  /// 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;
diff --git a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp
index 87274040f..0c21cda49 100644
--- a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp
+++ b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp
@@ -30,10 +30,15 @@ class StateConfig
   /// Constructor
   ///
   /// \param[in] threshold_soc
+  ///   Minimum charge level the battery is allowed to deplete to. This
+  ///   value needs to be between 0.0 and 1.0.
   StateConfig(double threshold_soc);
 
+  /// Gets the battery state of charge threshold value.
   double threshold_soc() const;
 
+  /// Sets a new battery state of charge threshold value. This value needs to be
+  /// between 0.0 and 1.0.
   StateConfig& threshold_soc(double threshold_soc);
 
   class Implementation;
diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp
index 731258b8c..594bd844e 100644
--- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp
+++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp
@@ -127,6 +127,7 @@ class TaskPlanner
   /// Get the greedy planner based assignments for a set of initial states and 
   /// requests
   Assignments greedy_plan(
+    rmf_traffic::Time time_now,
     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 time_now,
     std::vector<State> initial_states,
     std::vector<StateConfig> state_configs,
     std::vector<Request::SharedPtr> requests,
diff --git a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp
index 759fbe335..53102f1df 100644
--- a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp
+++ b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp
@@ -49,8 +49,8 @@ class Clean : public rmf_tasks::Request
     std::shared_ptr<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;
 
diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp
index 16a2e8937..0bd9a3dc1 100644
--- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp
+++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp
@@ -46,8 +46,8 @@ class Delivery : public rmf_tasks::Request
     std::shared_ptr<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;
 
diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp
index accbd3112..9316ae563 100644
--- a/rmf_tasks/src/agv/State.cpp
+++ b/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 location,
     std::size_t charging_waypoint,
-    rmf_traffic::Time finish_time,
     double battery_soc)
-  : _waypoint(waypoint),
+  : _location(std::move(location)),
     _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 _location;
   std::size_t _charging_waypoint;
-  rmf_traffic::Time _finish_time;
   double _battery_soc;
 };
 
 //==============================================================================
 State::State(
-  std::size_t waypoint, 
+  rmf_traffic::agv::Plan::Start location,
   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(location), charging_waypoint, battery_soc)))
 {}
 
 //==============================================================================
-std::size_t State::waypoint() const
+rmf_traffic::agv::Plan::Start State::location() const
 {
-  return _pimpl->_waypoint;
+  return _pimpl->_location;
 }
 
 //==============================================================================
-State& State::waypoint(std::size_t new_waypoint)
+State& State::location(rmf_traffic::agv::Plan::Start new_location)
 {
-  _pimpl->_waypoint = new_waypoint;
+  _pimpl->_location = std::move(new_location);
   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->_location.waypoint();
 }
 
 //==============================================================================
-State& State::battery_soc(double new_battery_soc)
+State& State::waypoint(std::size_t new_waypoint)
 {
-  _pimpl->_battery_soc = new_battery_soc;
+  _pimpl->_location.waypoint(new_waypoint);
+  return *this;
+}
+
+//==============================================================================
+rmf_traffic::Time State::finish_time() const
+{
+  return _pimpl->_location.time();
+}
+
+//==============================================================================
+State& State::finish_time(rmf_traffic::Time new_finish_time)
+{
+  _pimpl->_location.time(new_finish_time);
   return *this;
 }
 
+//==============================================================================
 } // namespace agv
 } // namespace rmf_tasks
diff --git a/rmf_tasks/src/agv/StateConfig.cpp b/rmf_tasks/src/agv/StateConfig.cpp
index d5593cb72..b187fe0d8 100644
--- a/rmf_tasks/src/agv/StateConfig.cpp
+++ b/rmf_tasks/src/agv/StateConfig.cpp
@@ -15,6 +15,8 @@
  *
 */
 
+#include <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;
 }
diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp
index e1a25a692..e3bc44e8e 100644
--- a/rmf_tasks/src/agv/TaskPlanner.cpp
+++ b/rmf_tasks/src/agv/TaskPlanner.cpp
@@ -614,6 +614,7 @@ class TaskPlanner::Implementation
   }
   
   Assignments complete_solve(
+    rmf_traffic::Time time_now,
     std::vector<State>& initial_states,
     const std::vector<StateConfig>& state_configs,
     const std::vector<Request::SharedPtr>& requests,
@@ -622,8 +623,7 @@ class TaskPlanner::Implementation
   {
     assert(initial_states.size() == state_configs.size());
 
-    const rmf_traffic::Time start_time = std::chrono::steady_clock::now();
-    auto node = make_initial_node(initial_states, state_configs, requests, start_time);
+    auto node = make_initial_node(initial_states, state_configs, requests, time_now);
 
     Node::AssignedTasks complete_assignments;
     complete_assignments.resize(node->assigned_tasks.size());
@@ -631,9 +631,9 @@ class TaskPlanner::Implementation
     while (node)
     {
       if (greedy)
-        node = greedy_solve(node, initial_states, state_configs, start_time);
+        node = greedy_solve(node, initial_states, state_configs, time_now);
       else
-        node = solve(node, initial_states, state_configs, requests.size(), start_time, interrupter);
+        node = solve(node, initial_states, state_configs, requests.size(), time_now, interrupter);
 
       if (!node)
         return {};
@@ -659,17 +659,21 @@ class TaskPlanner::Implementation
 
       // copy final state estimates 
       std::vector<State> estimates;
-      estimates.resize(node->assigned_tasks.size());
+      rmf_traffic::agv::Plan::Start empty_new_location{
+        time_now, 0, 0.0};
+      estimates.resize(
+        node->assigned_tasks.size(),
+        State{empty_new_location, 0, 0.0});
       for (std::size_t i = 0; i < node->assigned_tasks.size(); ++i)
       {
         const auto& assignments = node->assigned_tasks[i];
         if (assignments.empty())
           estimates[i] = initial_states[i];
         else
-          estimates[i] = assignments.back().state();        
+          estimates[i] = assignments.back().state();
       }
 
-      node = make_initial_node(estimates, state_configs, new_tasks, start_time);
+      node = make_initial_node(estimates, state_configs, new_tasks, time_now);
       initial_states = estimates;
     }
 
@@ -681,7 +685,7 @@ class TaskPlanner::Implementation
     return compute_g(node.assigned_tasks);
   }
 
-  double compute_h(const Node& node, const rmf_traffic::Time relative_start_time)
+  double compute_h(const Node& node, const rmf_traffic::Time time_now)
   {
     std::vector<double> initial_queue_values;
     initial_queue_values.resize(
@@ -696,7 +700,7 @@ class TaskPlanner::Implementation
           u.second.candidates.best_finish_time()
           - u.second.request->invariant_duration();
       const double variant_value =
-        rmf_traffic::time::to_seconds(variant_time - relative_start_time);
+        rmf_traffic::time::to_seconds(variant_time - time_now);
 
       const auto& range = u.second.candidates.best_candidates();
       for (auto it = range.begin; it != range.end; ++it)
@@ -720,7 +724,7 @@ class TaskPlanner::Implementation
         else
           value =
             rmf_traffic::time::to_seconds(
-              assignments.back().state().finish_time() - relative_start_time);
+              assignments.back().state().finish_time() - time_now);
       }
     }
 
@@ -735,16 +739,16 @@ class TaskPlanner::Implementation
     return queue.compute_cost();
   }
 
-  double compute_f(const Node& n, const rmf_traffic::Time relative_start_time)
+  double compute_f(const Node& n, const rmf_traffic::Time time_now)
   {
-    return compute_g(n) + compute_h(n, relative_start_time);
+    return compute_g(n) + compute_h(n, time_now);
   }
 
   ConstNodePtr make_initial_node(
     std::vector<State> initial_states,
     std::vector<StateConfig> state_configs,
     std::vector<Request::SharedPtr> requests,
-    rmf_traffic::Time relative_start_time)
+    rmf_traffic::Time time_now)
   {
     auto initial_node = std::make_shared<Node>();
 
@@ -757,7 +761,7 @@ class TaskPlanner::Implementation
           PendingTask(initial_states, state_configs, request, config->charge_battery_request())
         });
 
-    initial_node->cost_estimate = compute_f(*initial_node, relative_start_time);
+    initial_node->cost_estimate = compute_f(*initial_node, time_now);
 
     initial_node->sort_invariants();
 
@@ -813,7 +817,7 @@ class TaskPlanner::Implementation
     const Node::UnassignedTasks::value_type& u,
     const ConstNodePtr& parent,
     Filter* filter,
-    rmf_traffic::Time relative_start_time,
+    rmf_traffic::Time time_now,
     const std::vector<StateConfig>& state_configs)
 
   {
@@ -916,7 +920,7 @@ class TaskPlanner::Implementation
     }
 
     // Update the cost estimate for new_node
-    new_node->cost_estimate = compute_f(*new_node, relative_start_time);
+    new_node->cost_estimate = compute_f(*new_node, time_now);
     new_node->latest_time = get_latest_time(*new_node);
 
     // Apply filter
@@ -934,7 +938,7 @@ class TaskPlanner::Implementation
     const std::size_t agent,
     const std::vector<State>& initial_states,
     const std::vector<StateConfig>& state_configs,
-    rmf_traffic::Time relative_start_time)
+    rmf_traffic::Time time_now)
   {
     auto new_node = std::make_shared<Node>(*parent);
      // Assign charging task to an agent
@@ -974,7 +978,7 @@ class TaskPlanner::Implementation
         }
       }
 
-      new_node->cost_estimate = compute_f(*new_node, relative_start_time);
+      new_node->cost_estimate = compute_f(*new_node, time_now);
       new_node->latest_time = get_latest_time(*new_node);
       return new_node;
     }
@@ -986,7 +990,7 @@ class TaskPlanner::Implementation
     ConstNodePtr node,
     const std::vector<State>& initial_states,
     const std::vector<StateConfig>& state_configs,
-    rmf_traffic::Time relative_start_time)
+    rmf_traffic::Time time_now)
   {
     while (!finished(*node))
     {
@@ -997,7 +1001,7 @@ class TaskPlanner::Implementation
         for (auto it = range.begin; it != range.end; ++it)
         {
           if (auto n = expand_candidate(
-            it, u, node, nullptr, relative_start_time, state_configs))
+            it, u, node, nullptr, time_now, state_configs))
           {
             if (!next_node || (n->cost_estimate < next_node->cost_estimate))
               {
@@ -1021,7 +1025,7 @@ class TaskPlanner::Implementation
                   it->second.candidate,
                   initial_states,
                   state_configs,
-                  relative_start_time);
+                  time_now);
                 if (new_charge_node)
                 {
                   next_node = std::move(new_charge_node);
@@ -1045,7 +1049,7 @@ class TaskPlanner::Implementation
     Filter& filter,
     const std::vector<State>& initial_states,
     const std::vector<StateConfig>& state_configs,
-    rmf_traffic::Time relative_start_time)
+    rmf_traffic::Time time_now)
   {
     std::vector<ConstNodePtr> new_nodes;
     new_nodes.reserve(
@@ -1056,7 +1060,7 @@ class TaskPlanner::Implementation
       for (auto it = range.begin; it!= range.end; it++)
       {
         if (auto new_node = expand_candidate(
-          it, u, parent, &filter, relative_start_time, state_configs))
+          it, u, parent, &filter, time_now, state_configs))
           new_nodes.push_back(std::move(new_node));
       }
     }
@@ -1065,7 +1069,7 @@ class TaskPlanner::Implementation
     for (std::size_t i = 0; i < parent->assigned_tasks.size(); ++i)
     {
       if (auto new_node = expand_charger(
-        parent, i, initial_states, state_configs, relative_start_time))
+        parent, i, initial_states, state_configs, time_now))
         new_nodes.push_back(new_node);
     }
 
@@ -1093,7 +1097,7 @@ class TaskPlanner::Implementation
     const std::vector<State>& initial_states,
     const std::vector<StateConfig>& state_configs,
     const std::size_t num_tasks,
-    rmf_traffic::Time relative_start_time,
+    rmf_traffic::Time time_now,
     std::function<bool()> interrupter)
   {
     using PriorityQueue = std::priority_queue<
@@ -1122,7 +1126,7 @@ class TaskPlanner::Implementation
 
       // Apply possible actions to expand the node
       const auto new_nodes = expand(
-        top, filter, initial_states, state_configs, relative_start_time);
+        top, filter, initial_states, state_configs, time_now);
 
       // Add copies and with a newly assigned task to queue
       for (const auto&n : new_nodes)
@@ -1145,11 +1149,13 @@ TaskPlanner::TaskPlanner(std::shared_ptr<Configuration> config)
 }
 
 auto TaskPlanner::greedy_plan(
+  rmf_traffic::Time time_now,
   std::vector<State> initial_states,
   std::vector<StateConfig> state_configs,
   std::vector<Request::SharedPtr> requests) -> Assignments
 {
   return _pimpl->complete_solve(
+    time_now,
     initial_states,
     state_configs,
     requests,
@@ -1158,12 +1164,14 @@ auto TaskPlanner::greedy_plan(
 }
 
 auto TaskPlanner::optimal_plan(
+  rmf_traffic::Time time_now,
   std::vector<State> initial_states,
   std::vector<StateConfig> state_configs,
   std::vector<Request::SharedPtr> requests,
   std::function<bool()> interrupter) -> Assignments
 {
   return _pimpl->complete_solve(
+    time_now,
     initial_states,
     state_configs,
     requests,
diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp
index 1c466b9f7..3a9b170f3 100644
--- a/rmf_tasks/src/requests/ChargeBattery.cpp
+++ b/rmf_tasks/src/requests/ChargeBattery.cpp
@@ -86,11 +86,14 @@ rmf_utils::optional<rmf_tasks::Estimate> ChargeBattery::estimate_finish(
     return rmf_utils::nullopt;
 
   // Compute time taken to reach charging waypoint from current location
-  agv::State state(
+  rmf_traffic::agv::Plan::Start final_plan_start{
+    initial_state.finish_time(),
     initial_state.charging_waypoint(),
+    initial_state.location().orientation()};
+  agv::State state{
+    std::move(final_plan_start),
     initial_state.charging_waypoint(),
-    initial_state.finish_time(),
-    initial_state.battery_soc());
+    initial_state.battery_soc()};
 
   const auto start_time = initial_state.finish_time();
 
diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp
index 2d3e6bb59..ff267b7eb 100644
--- a/rmf_tasks/src/requests/Clean.cpp
+++ b/rmf_tasks/src/requests/Clean.cpp
@@ -55,8 +55,8 @@ rmf_tasks::Request::SharedPtr Clean::make(
   std::shared_ptr<rmf_battery::DevicePowerSink> ambient_sink,
   std::shared_ptr<rmf_battery::DevicePowerSink> cleaning_sink,
   std::shared_ptr<rmf_traffic::agv::Planner> planner,
-  bool drain_battery,
-  rmf_traffic::Time start_time)
+  rmf_traffic::Time start_time,
+  bool drain_battery)
 {
   std::shared_ptr<Clean> clean(new Clean());
   clean->_pimpl->id = id;
@@ -112,11 +112,14 @@ rmf_utils::optional<rmf_tasks::Estimate> Clean::estimate_finish(
   const agv::State& initial_state,
   const agv::StateConfig& state_config) const
 {
-  agv::State state(
-    _pimpl->end_waypoint, 
-    initial_state.charging_waypoint(),
+  rmf_traffic::agv::Plan::Start final_plan_start{
     initial_state.finish_time(),
-    initial_state.battery_soc());
+    _pimpl->end_waypoint,
+    initial_state.location().orientation()};
+  agv::State state{
+    std::move(final_plan_start),
+    initial_state.charging_waypoint(),
+    initial_state.battery_soc()};
 
   rmf_traffic::Duration variant_duration(0);
 
diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp
index 1fbe10d86..798725abe 100644
--- a/rmf_tasks/src/requests/Delivery.cpp
+++ b/rmf_tasks/src/requests/Delivery.cpp
@@ -51,8 +51,8 @@ rmf_tasks::Request::SharedPtr Delivery::make(
   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,
-  rmf_traffic::Time start_time)
+  rmf_traffic::Time start_time,
+  bool drain_battery)
 {
   std::shared_ptr<Delivery> delivery(new Delivery());
   delivery->_pimpl->_id = id;
@@ -110,11 +110,14 @@ rmf_utils::optional<rmf_tasks::Estimate> Delivery::estimate_finish(
   const agv::State& initial_state,
   const agv::StateConfig& state_config) const
 {
-  agv::State state(
-    _pimpl->_dropoff_waypoint, 
-    initial_state.charging_waypoint(),
+  rmf_traffic::agv::Plan::Start final_plan_start{
     initial_state.finish_time(),
-    initial_state.battery_soc());
+    _pimpl->_dropoff_waypoint,
+    initial_state.location().orientation()};
+  agv::State state{
+    std::move(final_plan_start),
+    initial_state.charging_waypoint(),
+    initial_state.battery_soc()};
 
   rmf_traffic::Duration variant_duration(0);
 
diff --git a/rmf_tasks/test/unit/agv/test_State.cpp b/rmf_tasks/test/unit/agv/test_State.cpp
new file mode 100644
index 000000000..5b21cf471
--- /dev/null
+++ b/rmf_tasks/test/unit/agv/test_State.cpp
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2020 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 <chrono>
+#include <memory>
+
+#include <rmf_tasks/agv/State.hpp>
+
+#include <rmf_traffic/Time.hpp>
+#include <rmf_traffic/agv/Planner.hpp>
+
+#include <rmf_utils/catch.hpp>
+
+SCENARIO("Robot States")
+{
+  const rmf_traffic::agv::Plan::Start basic_start{
+    std::chrono::steady_clock::now(),
+    0,
+    0.0};
+  
+  std::unique_ptr<rmf_tasks::agv::State> basic_state;
+
+  WHEN("Empty battery")
+  {
+    CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{
+      basic_start,
+      0,
+      0.0}));
+  }
+
+  WHEN("Full battery")
+  {
+    CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{
+      basic_start,
+      0,
+      1.0}));
+  }
+
+  WHEN("Half battery")
+  {
+    CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{
+      basic_start,
+      0,
+      0.5}));
+  }
+
+  WHEN("Battery soc more than 1.0")
+  {
+    CHECK_THROWS(basic_state.reset(new rmf_tasks::agv::State{
+      basic_start,
+      0,
+      1.0 + 1e-4}));
+  }
+
+  WHEN("Battery soc less than 0.0")
+  {
+    CHECK_THROWS(basic_state.reset(new rmf_tasks::agv::State{
+      basic_start,
+      0,
+      0.0 - 1e-4}));
+  }
+}
diff --git a/rmf_tasks/test/unit/agv/test_StateConfig.cpp b/rmf_tasks/test/unit/agv/test_StateConfig.cpp
new file mode 100644
index 000000000..e205e1c98
--- /dev/null
+++ b/rmf_tasks/test/unit/agv/test_StateConfig.cpp
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2020 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 <chrono>
+#include <memory>
+
+#include <rmf_tasks/agv/StateConfig.hpp>
+
+#include <rmf_utils/catch.hpp>
+
+SCENARIO("Robot State Configs")
+{
+  std::unique_ptr<rmf_tasks::agv::StateConfig> basic_state_config;
+
+  WHEN("Minimum battery threshold")
+  {
+    CHECK_NOTHROW(
+      basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.0}));
+  }
+
+  WHEN("Maximum battery threshold")
+  {
+    CHECK_NOTHROW(
+      basic_state_config.reset(new rmf_tasks::agv::StateConfig{1.0}));
+  }
+
+  WHEN("Half battery threshold")
+  {
+    CHECK_NOTHROW(
+      basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.5}));
+  }
+
+  WHEN("Below minimum battery threshold")
+  {
+    CHECK_THROWS(
+      basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.0 - 1e-4}));
+  }
+
+  WHEN("Above maximum battery threshold")
+  {
+    CHECK_THROWS(
+      basic_state_config.reset(new rmf_tasks::agv::StateConfig{1.0 + 1e-4}));
+  }
+}
diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp
index 01f377307..0647ce473 100644
--- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp
+++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp
@@ -55,7 +55,8 @@ inline void display_solution(
     {
       const auto& s = a.state();
       const double start_seconds = a.earliest_start_time().time_since_epoch().count()/1e9;
-      const double finish_seconds = s.finish_time().time_since_epoch().count()/1e9;
+      const rmf_traffic::Time finish_time = s.finish_time();
+      const double finish_seconds = finish_time.time_since_epoch().count()/1e9;
       std::cout << "    <" << a.task_id() << ": " << start_seconds 
                 << ", "<< finish_seconds << ", " << 100* s.battery_soc() 
                 << "%>" << std::endl;
@@ -136,11 +137,15 @@ SCENARIO("Grid World")
   WHEN("Planning for 3 requests and 2 agents")
   {
     const auto now = std::chrono::steady_clock::now();
+    const double default_orientation = 0.0;
+
+    rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation};
+    rmf_traffic::agv::Plan::Start second_location{now, 2, default_orientation};
 
     std::vector<rmf_tasks::agv::State> initial_states =
     {
-      rmf_tasks::agv::State{13, 13},
-      rmf_tasks::agv::State{2, 2}
+      rmf_tasks::agv::State{first_location, 13, 1.0},
+      rmf_tasks::agv::State{second_location, 2, 1.0}
     };
 
     std::vector<rmf_tasks::agv::StateConfig> state_configs =
@@ -158,8 +163,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         2,
@@ -168,8 +173,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         3,
@@ -178,8 +183,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0))
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery)
     };
 
     std::shared_ptr<rmf_tasks::agv::TaskPlanner::Configuration>  task_config =
@@ -187,11 +192,11 @@ SCENARIO("Grid World")
     rmf_tasks::agv::TaskPlanner task_planner(task_config);
 
     const auto greedy_assignments = task_planner.greedy_plan(
-      initial_states, state_configs, requests);
+      now, initial_states, state_configs, requests);
     const double greedy_cost = task_planner.compute_cost(greedy_assignments);
 
     const auto optimal_assignments = task_planner.optimal_plan(
-      initial_states, state_configs, requests, nullptr);
+      now, initial_states, state_configs, requests, nullptr);
     const double optimal_cost = task_planner.compute_cost(optimal_assignments);
     
     display_solution("Greedy", greedy_assignments, greedy_cost);
@@ -203,11 +208,15 @@ SCENARIO("Grid World")
   WHEN("Planning for 11 requests and 2 agents")
   {
     const auto now = std::chrono::steady_clock::now();
+    const double default_orientation = 0.0;
+
+    rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation};
+    rmf_traffic::agv::Plan::Start second_location{now, 2, default_orientation};
 
     std::vector<rmf_tasks::agv::State> initial_states =
     {
-      rmf_tasks::agv::State{13, 13},
-      rmf_tasks::agv::State{2, 2}
+      rmf_tasks::agv::State{first_location, 13, 1.0},
+      rmf_tasks::agv::State{second_location, 2, 1.0}
     };
 
     std::vector<rmf_tasks::agv::StateConfig> state_configs =
@@ -225,8 +234,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         2,
@@ -235,8 +244,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         3,
@@ -245,8 +254,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         3,
@@ -255,8 +264,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         4,
@@ -265,8 +274,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(50000)),
+        now + rmf_traffic::time::from_seconds(50000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         5,
@@ -275,8 +284,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(50000)),
+        now + rmf_traffic::time::from_seconds(50000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         6,
@@ -285,8 +294,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         7,
@@ -295,8 +304,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         8,
@@ -305,8 +314,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         9,
@@ -315,8 +324,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         10,
@@ -325,8 +334,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         11,
@@ -335,8 +344,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000))
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery)
     };
 
     std::shared_ptr<rmf_tasks::agv::TaskPlanner::Configuration>  task_config =
@@ -344,11 +353,11 @@ SCENARIO("Grid World")
     rmf_tasks::agv::TaskPlanner task_planner(task_config);
 
     const auto greedy_assignments = task_planner.greedy_plan(
-      initial_states, state_configs, requests);
+      now, initial_states, state_configs, requests);
     const double greedy_cost = task_planner.compute_cost(greedy_assignments);
 
     const auto optimal_assignments = task_planner.optimal_plan(
-      initial_states, state_configs, requests, nullptr);
+      now, initial_states, state_configs, requests, nullptr);
     const double optimal_cost = task_planner.compute_cost(optimal_assignments);
   
     display_solution("Greedy", greedy_assignments, greedy_cost);