Add API to clear planner inner cache (#123)

* add API to clear planner inner cache

Signed-off-by: Charly Wu <charlywu@medinno.com>

* removed debug print

Signed-off-by: Charly Wu <charlywu@medinno.com>

* Revert whitespace changes

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

* Revert more whitespace

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

* Do not use exception for unimplemented method

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

* Revert more whitespace changes

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

---------

Signed-off-by: Charly Wu <charlywu@medinno.com>
Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Co-authored-by: Michael X. Grey <mxgrey@intrinsic.ai>
Co-authored-by: Luca Della Vedova <lucadv@intrinsic.ai>

Author: 3 people

rmf_traffic/include/rmf_traffic/agv/Planner.hpp

@@ -618,6 +618,8 @@ class Planner
   /// It is advisable to not clear the cache more than once per minute.
   void clear_differential_drive_cache() const;
 
+  void clear_inner_cache() const;
+
   class Implementation;
   class Debug;
 private:

rmf_traffic/src/rmf_traffic/agv/Planner.cpp

@@ -1063,6 +1063,12 @@ void Planner::clear_differential_drive_cache() const
   _pimpl->interface->clear_cache();
 }
 
+//==============================================================================
+void Planner::clear_inner_cache() const
+{
+  _pimpl->interface->clear_inner_cache();
+}
+
 //==============================================================================
 const Eigen::Vector3d& Plan::Waypoint::position() const
 {

rmf_traffic/src/rmf_traffic/agv/internal_planning.hpp

@@ -131,6 +131,8 @@ class Interface
 
   virtual void clear_cache() const = 0;
 
+  virtual void clear_inner_cache() const = 0;
+
   virtual ~Interface() = default;
 };
 

rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDriveHeuristic.cpp

@@ -707,6 +707,12 @@ ConstForestSolutionPtr DifferentialDriveHeuristic::inner_heuristic(
   return _heuristic->get(start, finish);
 }
 
+//==============================================================================
+void DifferentialDriveHeuristic::clear_inner_heuristic() const
+{
+  _heuristic->clear_cache();
+}
+
 //==============================================================================
 CacheManagerPtr<DifferentialDriveHeuristic>
 DifferentialDriveHeuristic::make_manager(

rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDriveHeuristic.hpp

@@ -57,6 +57,8 @@ class DifferentialDriveHeuristic : public Generator<DifferentialDriveMap>
   ConstForestSolutionPtr inner_heuristic(
       std::size_t start, std::size_t finish) const;
 
+  void clear_inner_heuristic() const;
+
   static CacheManagerPtr<DifferentialDriveHeuristic> make_manager(
     std::shared_ptr<const Supergraph> graph,
     ConstChildHeuristicPtr child_heuristic);

rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp

@@ -2930,6 +2930,12 @@ void DifferentialDrivePlanner::clear_cache() const
   _cache->get().clear();
 }
 
+//==============================================================================
+void DifferentialDrivePlanner::clear_inner_cache() const
+{
+  _cache->inner()->clear_inner_heuristic();
+}
+
 } // namespace planning
 } // namespace agv
 } // namespace rmf_traffic

rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.hpp

@@ -64,6 +64,8 @@ class DifferentialDrivePlanner : public Interface
 
   void clear_cache() const final;
 
+  void clear_inner_cache() const final;
+
   std::optional<double> compute_heuristic(const Planner::Start& start) const;
 
 private:

rmf_traffic/src/rmf_traffic/agv/planning/Tree.hpp

@@ -242,8 +242,12 @@ class BidirectionalForest
 
   std::size_t cache_size() const;
 
+  void clear_cache() const;
+
   std::size_t heuristic_cache_size() const;
 
+  void clear_heuristic_cache() const;
+
   ~BidirectionalForest();
 
 private:

rmf_traffic/src/rmf_traffic/agv/planning/impl_Tree.hpp

@@ -376,14 +376,22 @@ std::optional<double> BidirectionalForest<T>::get_cost(
 template<typename T>
 std::size_t BidirectionalForest<T>::cache_size() const
 {
-    SpinLock lock(_solutions_mutex);
-    std::size_t count = 0;
-    for (const auto& [_, goals] : _solutions)
-    {
-      count += goals.size();
-    }
+  SpinLock lock(_solutions_mutex);
+  std::size_t count = 0;
+  for (const auto& [_, goals] : _solutions)
+  {
+    count += goals.size();
+  }
 
-    return count;
+  return count;
+}
+
+//==============================================================================
+template<typename T>
+void BidirectionalForest<T>::clear_cache() const
+{
+  SpinLock lock(_solutions_mutex);
+  _solutions.clear();
 }
 
 //==============================================================================
@@ -393,6 +401,14 @@ std::size_t BidirectionalForest<T>::heuristic_cache_size() const
   return _heuristic_cache->net_size();
 }
 
+//==============================================================================
+template<typename T>
+void BidirectionalForest<T>::clear_heuristic_cache() const
+{
+  // TODO: This is not implemented yet, but there should not be any negative
+  // impact on behavior by doing nothing here.
+}
+
 //==============================================================================
 template<typename T>
 BidirectionalForest<T>::~BidirectionalForest()