diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp
index 91f7add844c74..f27bf982978f2 100644
--- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp
+++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp
@@ -72,39 +72,40 @@ bool DistanceBasedStaticMapLoader::is_close_to_map(
   }
   return true;
 }
-
 bool DistanceBasedDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
-  if (current_voxel_grid_dict_.size() == 0) {
-    return false;
-  }
-  if (!isFinite(point)) {
-    return false;
-  }
+  std::shared_ptr<pcl::search::Search<pcl::PointXYZ>> map_cell_kdtree;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    if (current_voxel_grid_dict_.size() == 0) {
+      return false;
+    }
+    if (!isFinite(point)) {
+      return false;
+    }
 
-  const 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_));
+    const 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()) {
-    return false;
-  }
-  if (current_voxel_grid_array_.at(map_grid_index) != nullptr) {
-    if (current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree == nullptr) {
+    if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
       return false;
     }
-    std::vector<int> nn_indices(1);
-    std::vector<float> nn_distances(1);
-    if (!current_voxel_grid_array_.at(map_grid_index)
-           ->map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) {
+    const auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index);
+    if (current_voxel_grid == nullptr) {
       return false;
     }
-    if (nn_distances[0] <= distance_threshold) {
-      return true;
-    }
+    map_cell_kdtree = current_voxel_grid->map_cell_kdtree;
   }
-  return false;
+
+  std::vector<int> nn_indices(1);
+  std::vector<float> nn_distances(1);
+  if (!map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) {
+    return false;
+  }
+
+  return nn_distances[0] <= distance_threshold;
 }
 
 DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent(
diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp
index 33c9d9ff3c788..1b2b2dfe1294f 100644
--- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp
+++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp
@@ -65,8 +65,11 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   inline void addMapCellAndFilter(
     const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
   {
-    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;
+    {
+      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;
+    }
 
     pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
     pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp
index bfb0027b4e0a0..ec20a3c952d0c 100644
--- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp
+++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp
@@ -45,29 +45,30 @@ bool VoxelBasedApproximateStaticMapLoader::is_close_to_map(
 bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold)
 {
-  if (current_voxel_grid_dict_.size() == 0) {
-    return false;
-  }
+  VoxelGridPointXYZ map_cell_voxel_grid;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    if (current_voxel_grid_dict_.size() == 0) {
+      return false;
+    }
 
-  const 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_));
+    const 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()) {
-    return false;
-  }
-  if (current_voxel_grid_array_.at(map_grid_index) != nullptr) {
-    const int index = current_voxel_grid_array_.at(map_grid_index)
-                        ->map_cell_voxel_grid.getCentroidIndexAt(
-                          current_voxel_grid_array_.at(map_grid_index)
-                            ->map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z));
-    if (index == -1) {
+    if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
       return false;
-    } else {
-      return true;
     }
+    const auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index);
+    if (current_voxel_grid == nullptr) {
+      return false;
+    }
+    map_cell_voxel_grid = current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid;
   }
-  return false;
+
+  const int index = map_cell_voxel_grid.getCentroidIndexAt(
+    map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z));
+  return (index != -1);
 }
 
 VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapFilterComponent(
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp
index a94ae92671caf..2bcd4ba7c2fc2 100644
--- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp
+++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp
@@ -80,25 +80,29 @@ bool VoxelDistanceBasedStaticMapLoader::is_close_to_map(
 bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
-  if (current_voxel_grid_dict_.size() == 0) {
-    return false;
-  }
+  std::shared_ptr<pcl::search::Search<pcl::PointXYZ>> map_cell_kdtree;
+  VoxelGridPointXYZ map_cell_voxel_grid;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    if (current_voxel_grid_dict_.size() == 0) {
+      return false;
+    }
 
-  const 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_));
+    const 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()) {
-    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_voxel_grid,
-      current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree)) {
-    return true;
+    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) {
+      return false;
+    }
+    map_cell_voxel_grid = current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid;
+    map_cell_kdtree = current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree;
   }
-  return false;
+  return is_close_to_neighbor_voxels(
+    point, distance_threshold, map_cell_voxel_grid, map_cell_kdtree);
 }
 
 VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterComponent(
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp
index 9fb81aa675655..25b30efb1b2c9 100644
--- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp
+++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp
@@ -71,8 +71,11 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   inline void addMapCellAndFilter(
     const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
   {
-    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;
+    {
+      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;
+    }
 
     pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
     pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp
index e93b2097e583e..99a58eace1002 100644
--- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp
+++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp
@@ -325,15 +325,18 @@ 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)
+  const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold,
+  const double origin_x, const double origin_y, const double map_grid_size_x,
+  const double map_grid_size_y, const int map_grids_x)
 {
   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_));
-
+    std::floor((point.x - origin_x) / map_grid_size_x) +
+    map_grids_x * std::floor((point.y - origin_y) / map_grid_size_y));
+  std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
   if (
     static_cast<size_t>(neighbor_map_grid_index) >= current_voxel_grid_array_.size() ||
     neighbor_map_grid_index == current_map_grid_index ||
@@ -352,49 +355,58 @@ bool VoxelGridDynamicMapLoader::is_close_to_next_map_grid(
 bool VoxelGridDynamicMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
-  if (current_voxel_grid_dict_.size() == 0) {
-    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()) {
-    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;
+  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;
+    }
   }
 
   // Compare point with the neighbor map cells if point close to map cell boundary
 
   if (is_close_to_next_map_grid(
         pcl::PointXYZ(point.x - distance_threshold, point.y, point.z), map_grid_index,
-        distance_threshold)) {
+        distance_threshold, origin_x, origin_y, map_grid_size_x, map_grid_size_y, map_grids_x)) {
     return true;
   }
 
   if (is_close_to_next_map_grid(
         pcl::PointXYZ(point.x + distance_threshold, point.y, point.z), map_grid_index,
-        distance_threshold)) {
+        distance_threshold, origin_x, origin_y, map_grid_size_x, map_grid_size_y, map_grids_x)) {
     return true;
   }
 
   if (is_close_to_next_map_grid(
         pcl::PointXYZ(point.x, point.y - distance_threshold, point.z), map_grid_index,
-        distance_threshold)) {
+        distance_threshold, origin_x, origin_y, map_grid_size_x, map_grid_size_y, map_grids_x)) {
     return true;
   }
   if (is_close_to_next_map_grid(
         pcl::PointXYZ(point.x, point.y + distance_threshold, point.z), map_grid_index,
-        distance_threshold)) {
+        distance_threshold, origin_x, origin_y, map_grid_size_x, map_grid_size_y, map_grids_x)) {
     return true;
   }
 
@@ -402,27 +414,29 @@ bool VoxelGridDynamicMapLoader::is_close_to_map(
 }
 void VoxelGridDynamicMapLoader::timer_callback()
 {
-  if (current_position_ == std::nullopt) {
-    return;
+  std::optional<geometry_msgs::msg::Point> current_position;
+  {
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
+    current_position = current_position_;
   }
-  if (last_updated_position_ == std::nullopt) {
-    request_update_map(current_position_.value());
-    last_updated_position_ = current_position_;
+
+  if (current_position == std::nullopt) {
     return;
   }
-
-  if (should_update_map()) {
-    last_updated_position_ = current_position_;
-    request_update_map((current_position_.value()));
-    last_updated_position_ = current_position_;
+  if (
+    last_updated_position_ == std::nullopt ||
+    should_update_map(
+      current_position.value(), last_updated_position_.value(), map_update_distance_threshold_)) {
+    request_update_map(current_position.value());
   }
+  last_updated_position_ = current_position;
 }
 
-bool VoxelGridDynamicMapLoader::should_update_map() const
+bool VoxelGridDynamicMapLoader::should_update_map(
+  const geometry_msgs::msg::Point & current_point, const geometry_msgs::msg::Point & last_point,
+  const double map_update_distance_threshold)
 {
-  if (
-    distance2D(current_position_.value(), last_updated_position_.value()) >
-    map_update_distance_threshold_) {
+  if (distance2D(current_point, last_point) > map_update_distance_threshold) {
     return true;
   }
   return false;
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp
index 3685008ffc4fe..69a1846677f4d 100644
--- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp
+++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp
@@ -190,18 +190,25 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg);
 
   void timer_callback();
-  bool should_update_map() const;
+  static bool should_update_map(
+    const geometry_msgs::msg::Point & current_point, const geometry_msgs::msg::Point & last_point,
+    const double map_update_distance_threshold);
   void request_update_map(const geometry_msgs::msg::Point & position);
   bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
   /** \brief Check if point close to map pointcloud in the */
   bool is_close_to_next_map_grid(
-    const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold);
+    const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold,
+    const double origin_x, const double origin_y, const double map_grid_size_x,
+    const double map_grid_size_y, const int map_grids_x);
 
   inline pcl::PointCloud<pcl::PointXYZ> getCurrentDownsampledMapPc()
   {
     pcl::PointCloud<pcl::PointXYZ> output;
     std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
     for (const auto & kv : current_voxel_grid_dict_) {
+      if (kv.second.map_cell_pc_ptr == nullptr) {
+        continue;
+      }
       output = output + *(kv.second.map_cell_pc_ptr);
     }
     return output;
@@ -235,6 +242,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 +261,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,19 +282,21 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   virtual inline void addMapCellAndFilter(
     const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add)
   {
-    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_);
+    {
+      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);