fix(compare_map_filter): deadlock bug fix

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

perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp

@@ -325,11 +325,13 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
 }
 void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg)
 {
+  std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
   current_position_ = msg->pose.pose.position;
 }
 bool VoxelGridDynamicMapLoader::is_close_to_next_map_grid(
   const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold)
 {
+  std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
   int neighbor_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_));
@@ -352,6 +354,7 @@ 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_);
   if (current_voxel_grid_dict_.size() == 0) {
     return false;
   }
@@ -402,6 +405,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map(
 }
 void VoxelGridDynamicMapLoader::timer_callback()
 {
+  std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
   if (current_position_ == std::nullopt) {
     return;
   }
@@ -420,6 +424,7 @@ void VoxelGridDynamicMapLoader::timer_callback()
 
 bool VoxelGridDynamicMapLoader::should_update_map() const
 {
+  std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
   if (
     distance2D(current_position_.value(), last_updated_position_.value()) >
     map_update_distance_threshold_) {

perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp

@@ -235,6 +235,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   /** Update loaded map grid array for fast searching*/
   virtual inline void updateVoxelGridArray()
   {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
     origin_x_ = std::floor((current_position_.value().x - map_loader_radius_) / map_grid_size_x_) *
                   map_grid_size_x_ +
                 origin_x_remainder_;
@@ -253,7 +254,6 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
 
     current_voxel_grid_array_.assign(
       map_grids_x_ * map_grid_size_y_, std::make_shared<MapGridVoxelInfo>());
-    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
     for (const auto & kv : current_voxel_grid_dict_) {
       int index = static_cast<int>(
         std::floor((kv.second.min_b_x - origin_x_) / map_grid_size_x_) +
@@ -275,9 +275,11 @@ 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 "
@@ -287,6 +289,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
 
     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();
 
     pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
     pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);