From 6e7fb22f55d1992070df7c6f2e6f5c5b59d54d3f Mon Sep 17 00:00:00 2001
From: Atsushi Watanabe <atsushi.w@ieee.org>
Date: Wed, 4 Dec 2024 15:30:42 +0900
Subject: [PATCH] Add debug log for remember updates

---
 planner_cspace/src/planner_3d.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
index eee4b91b..0504dc03 100644
--- a/planner_cspace/src/planner_3d.cpp
+++ b/planner_cspace/src/planner_3d.cpp
@@ -851,12 +851,15 @@ 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 float dur = boost::chrono::duration<float>(tnow - ts).count();
+      ROS_DEBUG("Remembered costmap updated (%0.4f sec.)", dur);
     }
     if (!has_goal_)
       return;