Cache estimated values to speed up TaskPlanner

rmf_task/include/rmf_task/Estimate.hpp

@@ -18,6 +18,9 @@
 #ifndef INCLUDE__RMF_TASK__ESTIMATE_HPP
 #define INCLUDE__RMF_TASK__ESTIMATE_HPP
 
+#include <optional>
+#include <utility>
+
 #include <rmf_task/agv/State.hpp>
 #include <rmf_traffic/Time.hpp>
 #include <rmf_utils/impl_ptr.hpp>
@@ -55,6 +58,34 @@ class Estimate
   rmf_utils::impl_ptr<Implementation> _pimpl;
 };
 
+/// Stores computed estimates between pairs of waypoints
+class EstimateCache
+{
+public:
+  /// Constructs an empty EstimateCache
+  EstimateCache();
+
+  /// Struct containing the estimated duration and charge required to travel between
+  /// a waypoint pair.
+  struct CacheElement
+  {
+    rmf_traffic::Duration duration;
+    double dsoc; // Positive if charge is consumed
+  };
+
+  /// Returns the saved estimate values for the path between the supplied waypoints,
+  /// if present.
+  std::optional<CacheElement> get(std::pair<size_t, size_t> waypoints) const;
+
+  /// Saves the estimated duration and change in charge between the supplied waypoints.
+  void set(std::pair<size_t, size_t> waypoints,
+    rmf_traffic::Duration duration, double dsoc);
+
+  class Implementation;
+private:
+  rmf_utils::impl_ptr<Implementation> _pimpl;
+};
+
 } // namespace rmf_task
 
 #endif // INCLUDE__RMF_TASK__ESTIMATE_HPP

rmf_task/include/rmf_task/Request.hpp

@@ -43,7 +43,8 @@ class Request
   /// 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;
+    const agv::StateConfig& state_config,
+    const std::shared_ptr<EstimateCache> estimate_cache) const = 0;
 
   /// Estimate the invariant component of the task's duration
   virtual rmf_traffic::Duration invariant_duration() const = 0;

rmf_task/include/rmf_task/agv/TaskPlanner.hpp

@@ -174,6 +174,8 @@ class TaskPlanner
 
   double compute_cost(const Assignments& assignments);
 
+  const std::shared_ptr<EstimateCache> estimate_cache() const;
+
   class Implementation;
 
 private:

rmf_task/include/rmf_task/requests/ChargeBattery.hpp

@@ -52,7 +52,8 @@ class ChargeBattery : public rmf_task::Request
 
   rmf_utils::optional<rmf_task::Estimate> estimate_finish(
     const agv::State& initial_state,
-    const agv::StateConfig& state_config) const final;
+    const agv::StateConfig& state_config,
+    const std::shared_ptr<EstimateCache> estimate_cache) const final;
 
   rmf_traffic::Duration invariant_duration() const final;
 

rmf_task/include/rmf_task/requests/Clean.hpp

@@ -57,7 +57,8 @@ class Clean : public rmf_task::Request
 
   rmf_utils::optional<rmf_task::Estimate> estimate_finish(
     const agv::State& initial_state,
-    const agv::StateConfig& state_config) const final;
+    const agv::StateConfig& state_config,
+    const std::shared_ptr<EstimateCache> estimate_cache) const final;
 
   rmf_traffic::Duration invariant_duration() const final;
 

rmf_task/include/rmf_task/requests/Delivery.hpp

@@ -62,7 +62,8 @@ class Delivery : public rmf_task::Request
 
   rmf_utils::optional<rmf_task::Estimate> estimate_finish(
     const agv::State& initial_state,
-    const agv::StateConfig& state_config) const final;
+    const agv::StateConfig& state_config,
+    const std::shared_ptr<EstimateCache> estimate_cache) const final;
 
   rmf_traffic::Duration invariant_duration() const final;
 

rmf_task/include/rmf_task/requests/Loop.hpp

@@ -19,6 +19,7 @@
 #define INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP
 
 #include <chrono>
+#include <string>
 
 #include <rmf_traffic/Time.hpp>
 #include <rmf_traffic/agv/Planner.hpp>
@@ -57,7 +58,8 @@ class Loop : public rmf_task::Request
 
   rmf_utils::optional<rmf_task::Estimate> estimate_finish(
     const agv::State& initial_state,
-    const agv::StateConfig& state_config) const final;
+    const agv::StateConfig& state_config,
+    const std::shared_ptr<EstimateCache> estimate_cache) const final;
 
   rmf_traffic::Duration invariant_duration() const final;
 

rmf_task/src/rmf_task/Estimate.cpp

@@ -14,6 +14,7 @@
  * limitations under the License.
  *
 */
+#include <unordered_map>
 
 #include <rmf_task/Estimate.hpp>
 
@@ -66,4 +67,44 @@ Estimate& Estimate::wait_until(rmf_traffic::Time new_wait_until)
 }
 
 //==============================================================================
+class EstimateCache::Implementation
+{
+  public:
+    struct PairHash
+    {
+      size_t operator()(const std::pair<size_t,size_t>& p) const
+      {
+        return std::hash<size_t>()(p.first) ^ std::hash<size_t>()(p.second);
+      }
+    };
+
+    using Cache = std::unordered_map<std::pair<size_t,size_t>,
+      CacheElement, PairHash>;
+    Cache _cache;
+};
+
+//==============================================================================
+EstimateCache::EstimateCache()
+  : _pimpl(rmf_utils::make_impl<Implementation>())
+{}
+
+//==============================================================================
+std::optional<EstimateCache::CacheElement> EstimateCache::get(
+  std::pair<size_t, size_t> waypoints) const
+{
+  auto it = _pimpl->_cache.find(waypoints);
+  if (it != _pimpl->_cache.end())
+  {
+    return it->second;
+  }
+  return std::nullopt;
+}
+
+//==============================================================================
+void EstimateCache::set(std::pair<size_t, size_t> waypoints,
+  rmf_traffic::Duration duration, double dsoc)
+{
+  _pimpl->_cache[waypoints] = CacheElement{duration, dsoc};
+}
+
 } // namespace rmf_task

rmf_task/src/rmf_task/agv/TaskPlanner.cpp

@@ -197,7 +197,8 @@ class Candidates
     const std::vector<State>& initial_states,
     const std::vector<StateConfig>& state_configs,
     const rmf_task::Request& request,
-    const rmf_task::Request& charge_battery_request);
+    const rmf_task::Request& charge_battery_request,
+    const std::shared_ptr<EstimateCache> estimate_cache);
 
   Candidates(const Candidates& other)
   {
@@ -286,14 +287,16 @@ Candidates Candidates::make(
   const std::vector<State>& initial_states,
   const std::vector<StateConfig>& state_configs,
   const rmf_task::Request& request,
-  const rmf_task::Request& charge_battery_request)
+  const rmf_task::Request& charge_battery_request,
+  const std::shared_ptr<EstimateCache> estimate_cache)
 {
   Map initial_map;
   for (std::size_t i = 0; i < initial_states.size(); ++i)
   {
     const auto& state = initial_states[i];
     const auto& state_config = state_configs[i];
-    const auto finish = request.estimate_finish(state, state_config);
+    const auto finish = request.estimate_finish(
+      state, state_config, estimate_cache);
     if (finish.has_value())
     {
       initial_map.insert({
@@ -308,11 +311,12 @@ Candidates Candidates::make(
     else
     {
       auto battery_estimate =
-        charge_battery_request.estimate_finish(state, state_config);
+        charge_battery_request.estimate_finish(
+          state, state_config, estimate_cache);
       if (battery_estimate.has_value())
       {
         auto new_finish = request.estimate_finish(
-          battery_estimate.value().finish_state(), state_config);
+          battery_estimate.value().finish_state(), state_config, estimate_cache);
         assert(new_finish.has_value());
         initial_map.insert(
           {new_finish.value().finish_state().finish_time(),
@@ -343,10 +347,11 @@ struct PendingTask
       std::vector<rmf_task::agv::State>& initial_states,
       std::vector<rmf_task::agv::StateConfig>& state_configs,
       rmf_task::ConstRequestPtr request_,
-      rmf_task::ConstRequestPtr charge_battery_request)
+      rmf_task::ConstRequestPtr charge_battery_request,
+      std::shared_ptr<EstimateCache> estimate_cache)
     : request(std::move(request_)),
-      candidates(Candidates::make(
-        initial_states, state_configs, *request, *charge_battery_request)),
+      candidates(Candidates::make(initial_states, state_configs,
+        *request, *charge_battery_request, estimate_cache)),
       earliest_start_time(request->earliest_start_time())
   {
     // Do nothing
@@ -665,6 +670,7 @@ class TaskPlanner::Implementation
 public:
 
   std::shared_ptr<Configuration> config;
+  std::shared_ptr<EstimateCache> estimate_cache;
 
   ConstRequestPtr make_charging_request(rmf_traffic::Time start_time)
   {
@@ -868,7 +874,7 @@ class TaskPlanner::Implementation
       initial_node->unassigned_tasks.insert(
         {
           internal_id,
-          PendingTask(initial_states, state_configs, request, charge_battery)
+          PendingTask(initial_states, state_configs, request, charge_battery, estimate_cache)
         });
     }
 
@@ -953,7 +959,8 @@ class TaskPlanner::Implementation
           assignments.back().assignment.request()))
       {
         auto charge_battery = make_charging_request(entry.previous_state.finish_time());
-        auto battery_estimate = charge_battery->estimate_finish(entry.previous_state, state_config);
+        auto battery_estimate = charge_battery->estimate_finish(
+          entry.previous_state, state_config, estimate_cache);
         if (battery_estimate.has_value())
         {
           assignments.push_back(
@@ -983,7 +990,7 @@ class TaskPlanner::Implementation
     {
       const auto finish =
         new_u.second.request->estimate_finish(
-          entry.state, state_config);
+          entry.state, state_config, estimate_cache);
 
       if (finish.has_value())
       {
@@ -1025,7 +1032,8 @@ class TaskPlanner::Implementation
     if (add_charger)
     {
       auto charge_battery = make_charging_request(entry.state.finish_time());
-      auto battery_estimate = charge_battery->estimate_finish(entry.state, state_config);
+      auto battery_estimate = charge_battery->estimate_finish(
+        entry.state, state_config, estimate_cache);
       if (battery_estimate.has_value())
       {
         new_node->assigned_tasks[entry.candidate].push_back(
@@ -1039,7 +1047,8 @@ class TaskPlanner::Implementation
         for (auto& new_u : new_node->unassigned_tasks)
         {
           const auto finish =
-            new_u.second.request->estimate_finish(battery_estimate.value().finish_state(), state_config);
+            new_u.second.request->estimate_finish(battery_estimate.value().finish_state(),
+              state_config, estimate_cache);
           if (finish.has_value())
           {
             new_u.second.candidates.update_candidate(
@@ -1096,7 +1105,7 @@ class TaskPlanner::Implementation
 
     auto charge_battery = make_charging_request(state.finish_time());
     auto estimate = charge_battery->estimate_finish(
-      state, state_configs[agent]);
+      state, state_configs[agent], estimate_cache);
     if (estimate.has_value())
     {
       new_node->assigned_tasks[agent].push_back(
@@ -1113,8 +1122,8 @@ class TaskPlanner::Implementation
       for (auto& new_u : new_node->unassigned_tasks)
       {
         const auto finish =
-          new_u.second.request->estimate_finish(
-            estimate.value().finish_state(), state_configs[agent]);
+          new_u.second.request->estimate_finish(estimate.value().finish_state(),
+            state_configs[agent], estimate_cache);
         if (finish.has_value())
         {
           new_u.second.candidates.update_candidate(
@@ -1294,7 +1303,8 @@ class TaskPlanner::Implementation
 TaskPlanner::TaskPlanner(std::shared_ptr<Configuration> config)
 : _pimpl(rmf_utils::make_impl<Implementation>(
       Implementation{
-        std::move(config)
+        std::move(config),
+        std::make_shared<EstimateCache>()
       }))
 {
   // Do nothing
@@ -1336,7 +1346,10 @@ auto TaskPlanner::compute_cost(const Assignments& assignments) -> double
   return _pimpl->compute_g(assignments);
 }
 
-
+const std::shared_ptr<EstimateCache> TaskPlanner::estimate_cache() const
+{
+  return _pimpl->estimate_cache;
+}
 
 
 } // namespace agv

rmf_task/src/rmf_task/requests/ChargeBattery.cpp

@@ -84,7 +84,8 @@ std::string ChargeBattery::id() const
 //==============================================================================
 rmf_utils::optional<rmf_task::Estimate> ChargeBattery::estimate_finish(
   const agv::State& initial_state,
-  const agv::StateConfig& state_config) const
+  const agv::StateConfig& state_config,
+  const std::shared_ptr<EstimateCache> estimate_cache) const
 {
   if (abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3)
     return rmf_utils::nullopt;
@@ -102,30 +103,42 @@ rmf_utils::optional<rmf_task::Estimate> ChargeBattery::estimate_finish(
   const auto start_time = initial_state.finish_time();
 
   double battery_soc = initial_state.battery_soc();
+  double dSOC_motion = 0.0;
+  double dSOC_device = 0.0;
   rmf_traffic::Duration variant_duration(0);
 
   if (initial_state.waypoint() != initial_state.charging_waypoint())
   {
-    // Compute plan to charging waypoint along with battery drain
-    rmf_traffic::agv::Planner::Start start{
-      start_time,
-      initial_state.waypoint(),
-      0.0};
-
-    rmf_traffic::agv::Planner::Goal goal{initial_state.charging_waypoint()};
-
-    const auto result = _pimpl->_planner->plan(start, goal);
-    const auto& trajectory = result->get_itinerary().back().trajectory();
-    const auto& finish_time = *trajectory.finish_time();
-    variant_duration = finish_time - start_time;
-
-    if (_pimpl->_drain_battery)
+    const auto endpoints = std::make_pair(initial_state.waypoint(),
+      initial_state.charging_waypoint());
+    const auto& cache_result = estimate_cache->get(endpoints);
+    // Use memoized values if possible
+    if (cache_result)
+    {
+      variant_duration = cache_result->duration;
+      battery_soc = battery_soc - cache_result->dsoc;
+    }
+    else
     {
-      const double dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(
-        trajectory);
-      const double dSOC_device = _pimpl->_device_sink->compute_change_in_charge(
-        rmf_traffic::time::to_seconds(variant_duration));
-      battery_soc = battery_soc - dSOC_motion - dSOC_device;
+      // Compute plan to charging waypoint along with battery drain
+      rmf_traffic::agv::Planner::Goal goal{endpoints.second};
+      const auto result = _pimpl->_planner->plan(
+        initial_state.location(), goal);
+      const auto& trajectory = result->get_itinerary().back().trajectory();
+      const auto& finish_time = *trajectory.finish_time();
+      variant_duration = finish_time - start_time;
+
+      if (_pimpl->_drain_battery)
+      {
+        dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(
+          trajectory);
+        dSOC_device = _pimpl->_device_sink->compute_change_in_charge(
+          rmf_traffic::time::to_seconds(variant_duration));
+        battery_soc = battery_soc - dSOC_motion - dSOC_device;
+      }
+
+      estimate_cache->set(endpoints, variant_duration,
+        dSOC_motion + dSOC_device);
     }
 
     // If a robot cannot reach its charging dock given its initial battery soc