planner_cspace: reduce remember_updates calculation cost

planner_cspace/include/planner_cspace/planner_3d/costmap_bbf.h

@@ -49,6 +49,8 @@ class CostmapBBF
   BlockMemGridmap<bbf::BinaryBayesFilter, 2, 2, 0x20> cm_hist_bbf_;
   BlockMemGridmap<char, 2, 2, 0x80> cm_hist_;
   Vec size_;
+  VecInternal updated_min_;
+  VecInternal updated_max_;
 
 public:
   inline CostmapBBF()
@@ -60,11 +62,14 @@ class CostmapBBF
     size_ = size;
     cm_hist_bbf_.reset(VecInternal(size[0], size[1]));
     cm_hist_.reset(VecInternal(size[0], size[1]));
+    clear();
   }
   inline void clear()
   {
     cm_hist_bbf_.clear(bbf::BinaryBayesFilter(bbf::MIN_ODDS));
     cm_hist_.clear(0);
+    updated_min_ = VecInternal(0, 0);
+    updated_max_ = VecInternal(size_[0] - 1, size_[1] - 1);
   }
   inline char getCost(const Vec& p) const
   {

planner_cspace/src/costmap_bbf.cpp

@@ -27,6 +27,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <algorithm>
 #include <cmath>
 
 #include <planner_cspace/bbf.h>
@@ -39,21 +40,29 @@ namespace planner_3d
 {
 void CostmapBBF::updateCostmap()
 {
-  cm_hist_.clear(0);
-  for (VecInternal p(0, 0); p[1] < size_[1]; p[1]++)
+  for (VecInternal p = updated_min_; p[1] <= updated_max_[1]; p[1]++)
   {
-    for (p[0] = 0; p[0] < size_[0]; p[0]++)
+    for (p[0] = updated_min_[0]; p[0] <= updated_max_[0]; p[0]++)
     {
       cm_hist_[p] = std::lround(cm_hist_bbf_[p].getNormalizedProbability() * 100.0);
     }
   }
+  updated_min_ = VecInternal(size_[0], size_[1]);
+  updated_max_ = VecInternal(-1, -1);
 }
 void CostmapBBF::remember(
     const BlockMemGridmapBase<char, 3, 2>* costmap,
     const Vec& center,
     const float remember_hit_odds, const float remember_miss_odds,
     const int range_min, const int range_max)
 {
+  updated_min_ = VecInternal(
+      std::min(updated_min_[0], std::max(0, center[0] - range_max)),
+      std::min(updated_min_[1], std::max(0, center[1] - range_max)));
+  updated_max_ = VecInternal(
+      std::max(updated_max_[0], std::min(size_[0] - 1, center[0] + range_max)),
+      std::max(updated_max_[1], std::min(size_[1] - 1, center[1] + range_max)));
+
   const size_t width = size_[0];
   const size_t height = size_[1];
   const int range_min_sq = range_min * range_min;

planner_cspace/src/planner_3d.cpp

@@ -851,12 +851,16 @@ class Planner3dNode
 
     if (remember_updates_)
     {
+      const auto ts = boost::chrono::high_resolution_clock::now();
       bbf_costmap_.remember(
           &cm_updates_, s,
           remember_hit_odds_, remember_miss_odds_,
           hist_ignore_range_, hist_ignore_range_max_);
       publishRememberedMap();
       bbf_costmap_.updateCostmap();
+      const auto tnow = boost::chrono::high_resolution_clock::now();
+      const float dur = boost::chrono::duration<float>(tnow - ts).count();
+      ROS_DEBUG("Remembered costmap updated (%0.4f sec.)", dur);
     }
     if (!has_goal_)
       return;

planner_cspace/test/CMakeLists.txt

@@ -97,6 +97,13 @@ add_rostest(test/navigation_rostest.test
   DEPENDENCIES test_navigate
 )
 
+add_rostest_gtest(test_navigate_remember
+  test/navigation_remember_rostest.test
+  src/test_navigate_remember.cpp
+)
+target_link_libraries(test_navigate_remember ${catkin_LIBRARIES})
+add_dependencies(test_navigate_remember planner_3d patrol)
+
 add_rostest(test/navigation_compat_rostest.test
   DEPENDENCIES test_navigate
 )

planner_cspace/test/data/global_map_remember.yaml

@@ -0,0 +1,7 @@
+image: global_map_remember.png
+resolution: 0.1
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

planner_cspace/test/data/local_map_remember.yaml

@@ -0,0 +1,7 @@
+image: local_map_remember.png
+resolution: 0.1
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+