Removing test code

Signed-off-by: anhnv3991 <anh.nguyen.2@tier4.jp>

Author: anhnv3991

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -18,7 +18,7 @@
 #define timeDiff(start, end) ((end.tv_sec - start.tv_sec) * 1000000 + end.tv_usec - start.tv_usec)
 #endif
 
-// #define DEBUG_FLAG
+#define DEBUG_FLAG
 
 MapUpdateModule::MapUpdateModule(
   rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
@@ -135,23 +135,23 @@ void MapUpdateModule::prefetch_map(const geometry_msgs::msg::Point& position, Nd
 #ifdef DEBUG_FLAG
   // If the current position is super far from the previous loading position,
   // lock and rebuild ndt_ptr_
-  // if (rebuild_)
-  // {
-  //   ndt_ptr_mutex_->lock();
-  //   prefetch_map(position, ndt_ptr_);  
-  //   ndt_ptr_mutex_->unlock();
-  //   rebuild_ = false;
-  // }
-  // else
-  // {
+  if (rebuild_)
+  {
+    ndt_ptr_mutex_->lock();
+    prefetch_map(position, ndt_ptr_);  
+    ndt_ptr_mutex_->unlock();
+    rebuild_ = false;
+  }
+  else
+  {
     // Load map to the secondary_ndt_ptr, which does not require a mutex lock
     // Since the update of the secondary ndt ptr and the NDT align (done on 
     // the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly.
     // If the updating is done the main ndt_ptr_, either the update or the NDT 
     // align will be blocked by the other.
-    // ndt_ptr_mutex_->lock();
-    // prefetch_map(position, ndt_ptr_);
-    // ndt_ptr_mutex_->unlock();
+    prefetch_map(position, secondary_ndt_ptr_);
+    
+    auto tmp = ndt_ptr_;
 
   //   // Now lock and update the main ndt_ptr_ by the secondary_ndt_ptr_
   //   ndt_ptr_mutex_->lock();