planner_cspace: reduce remember_updates calculation cost (#742)

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
+

planner_cspace/test/include/planner_cspace/planner_status.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2024, the neonavigation authors
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PLANNER_CSPACE_PLANNER_STATUS_H
+#define PLANNER_CSPACE_PLANNER_STATUS_H
+
+#include <ostream>
+#include <planner_cspace_msgs/PlannerStatus.h>
+
+namespace planner_cspace_msgs
+{
+std::ostream& operator<<(std::ostream& os, const PlannerStatus::ConstPtr& msg)
+{
+  if (!msg)
+  {
+    os << "nullptr";
+  }
+  else
+  {
+    os << std::endl
+       << "  header: " << msg->header.stamp << " " << msg->header.frame_id << std::endl
+       << "  status: " << static_cast<int>(msg->status) << std::endl
+       << "  error: " << static_cast<int>(msg->error);
+  }
+  return os;
+}
+}  // namespace planner_cspace_msgs
+
+#endif  // PLANNER_CSPACE_PLANNER_STATUS_H

planner_cspace/test/src/test_navigate.cpp

@@ -45,26 +45,9 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_listener.h>
 
-#include <gtest/gtest.h>
+#include <planner_cspace/planner_status.h>
 
-namespace planner_cspace_msgs
-{
-std::ostream& operator<<(std::ostream& os, const PlannerStatus::ConstPtr& msg)
-{
-  if (!msg)
-  {
-    os << "nullptr";
-  }
-  else
-  {
-    os << std::endl
-       << "  header: " << msg->header.stamp << " " << msg->header.frame_id << std::endl
-       << "  status: " << static_cast<int>(msg->status) << std::endl
-       << "  error: " << static_cast<int>(msg->error);
-  }
-  return os;
-}
-}  // namespace planner_cspace_msgs
+#include <gtest/gtest.h>
 
 class Navigate : public ::testing::Test
 {