fix: change to lock_guard

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp

@@ -76,6 +76,7 @@ bool DistanceBasedStaticMapLoader::is_close_to_map(
 bool DistanceBasedDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
+  std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
   if (current_voxel_grid_dict_.size() == 0) {
     return false;
   }

perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp

@@ -45,6 +45,7 @@ bool VoxelBasedApproximateStaticMapLoader::is_close_to_map(
 bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold)
 {
+  std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
   if (current_voxel_grid_dict_.size() == 0) {
     return false;
   }

perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp

@@ -80,6 +80,7 @@ bool VoxelDistanceBasedStaticMapLoader::is_close_to_map(
 bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
+  std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
   if (current_voxel_grid_dict_.size() == 0) {
     return false;
   }

perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp

@@ -355,39 +355,36 @@ bool VoxelGridDynamicMapLoader::is_close_to_next_map_grid(
 bool VoxelGridDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
-  // std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
-  dynamic_map_loader_mutex_.lock();
-  if (current_voxel_grid_dict_.size() == 0) {
-    dynamic_map_loader_mutex_.unlock();
-    return false;
-  }
-
-  // Compare point with map grid that point belong to
-
-  int map_grid_index = static_cast<int>(
-    std::floor((point.x - origin_x_) / map_grid_size_x_) +
-    map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
-
-  if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
-    dynamic_map_loader_mutex_.unlock();
-    return false;
-  }
-  if (
-    current_voxel_grid_array_.at(map_grid_index) != nullptr &&
-    is_close_to_neighbor_voxels(
-      point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_pc_ptr,
-      current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid)) {
-    dynamic_map_loader_mutex_.unlock();
-    return true;
+  double origin_x, origin_y, map_grid_size_x, map_grid_size_y;
+  int map_grids_x, map_grid_index;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    if (current_voxel_grid_dict_.size() == 0) {
+      return false;
+    }
+    origin_x = origin_x_;
+    origin_y = origin_y_;
+    map_grid_size_x = map_grid_size_x_;
+    map_grid_size_y = map_grid_size_y_;
+    map_grids_x = map_grids_x_;
+    // Compare point with map grid that point belong to
+
+    map_grid_index = static_cast<int>(
+      std::floor((point.x - origin_x) / map_grid_size_x) +
+      map_grids_x * std::floor((point.y - origin_y) / map_grid_size_y));
+
+    if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
+      return false;
+    }
+    if (
+      current_voxel_grid_array_.at(map_grid_index) != nullptr &&
+      is_close_to_neighbor_voxels(
+        point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_pc_ptr,
+        current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid)) {
+      return true;
+    }
   }
 
-  auto origin_x = origin_x_;
-  auto origin_y = origin_y_;
-  auto map_grid_size_x = map_grid_size_x_;
-  auto map_grid_size_y = map_grid_size_y_;
-  auto map_grids_x = map_grids_x_;
-  dynamic_map_loader_mutex_.unlock();
-
   // Compare point with the neighbor map cells if point close to map cell boundary
 
   if (is_close_to_next_map_grid(
@@ -417,9 +414,11 @@ bool VoxelGridDynamicMapLoader::is_close_to_map(
 }
 void VoxelGridDynamicMapLoader::timer_callback()
 {
-  dynamic_map_loader_mutex_.lock();
-  auto current_position = current_position_;
-  dynamic_map_loader_mutex_.unlock();
+  std::optional<geometry_msgs::msg::Point> current_position = std::nullopt;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    current_position = current_position_;
+  }
 
   if (current_position == std::nullopt) {
     return;

perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp

@@ -279,22 +279,21 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   virtual inline void addMapCellAndFilter(
     const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add)
   {
-    dynamic_map_loader_mutex_.lock();
-    map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
-    map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
-    if (map_grid_size_x_ > max_map_grid_size_ || map_grid_size_y_ > max_map_grid_size_) {
-      dynamic_map_loader_mutex_.unlock();
-      RCLCPP_ERROR(
-        logger_,
-        "Map was not split or split map grid size is too large. Split map with grid size smaller "
-        "than %f",
-        max_map_grid_size_);
-    }
-
-    origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_);
-    origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_);
-    dynamic_map_loader_mutex_.unlock();
+    {
+      std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+      map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
+      map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+      if (map_grid_size_x_ > max_map_grid_size_ || map_grid_size_y_ > max_map_grid_size_) {
+        RCLCPP_ERROR(
+          logger_,
+          "Map was not split or split map grid size is too large. Split map with grid size smaller "
+          "than %f",
+          max_map_grid_size_);
+      }
 
+      origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_);
+      origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_);
+    }
     pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
     pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);