fix(compare_map_segmentation): add missing mutex lock (#9097)

* fix(compare_map_segmentation): missing mutux

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

* chore: rename mutex_

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

* fix: remove unnecessary mutex

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

* fix: typos

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

* chore: minimize mutex scope

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

* chore: change to lock_guard

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

* fix: check tree initialization

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

* fix: memory ordering

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

* fix: replace all static_map_loader_mutex_

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

---------

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

Author: badai-nguyen

perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp

@@ -32,8 +32,6 @@ void DistanceBasedStaticMapLoader::onMapCallback(
   pcl::PointCloud<pcl::PointXYZ> map_pcl;
   pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
   const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);
-
-  (*mutex_ptr_).lock();
   map_ptr_ = map_pcl_ptr;
   *tf_map_input_frame_ = map_ptr_->header.frame_id;
   if (!tree_) {
@@ -44,13 +42,15 @@ void DistanceBasedStaticMapLoader::onMapCallback(
     }
   }
   tree_->setInputCloud(map_ptr_);
-
-  (*mutex_ptr_).unlock();
+  is_initialized_.store(true, std::memory_order_release);
 }
 
 bool DistanceBasedStaticMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
+  if (!is_initialized_.load(std::memory_order_acquire)) {
+    return false;
+  }
   if (map_ptr_ == NULL) {
     return false;
   }
@@ -126,10 +126,10 @@ DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent(
     rclcpp::CallbackGroup::SharedPtr main_callback_group;
     main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
     distance_based_map_loader_ = std::make_unique<DistanceBasedDynamicMapLoader>(
-      this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group);
+      this, distance_threshold_, &tf_input_frame_, main_callback_group);
   } else {
-    distance_based_map_loader_ = std::make_unique<DistanceBasedStaticMapLoader>(
-      this, distance_threshold_, &tf_input_frame_, &mutex_);
+    distance_based_map_loader_ =
+      std::make_unique<DistanceBasedStaticMapLoader>(this, distance_threshold_, &tf_input_frame_);
   }
 }
 

perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp

@@ -41,8 +41,8 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
 
 public:
   DistanceBasedStaticMapLoader(
-    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
-  : VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex)
+    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame)
+  : VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame)
   {
     RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n");
   }
@@ -55,9 +55,9 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
 {
 public:
   DistanceBasedDynamicMapLoader(
-    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
+    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame,
     rclcpp::CallbackGroup::SharedPtr main_callback_group)
-  : VoxelGridDynamicMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex, main_callback_group)
+  : VoxelGridDynamicMapLoader(node, leaf_size, 1.0, tf_map_input_frame, main_callback_group)
   {
     RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n");
   }
@@ -94,9 +94,8 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
     current_voxel_grid_list_item.map_cell_kdtree = tree_tmp;
 
     // add
-    (*mutex_ptr_).lock();
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
     current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
-    (*mutex_ptr_).unlock();
   }
 };
 

perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp

@@ -95,11 +95,10 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF
     rclcpp::CallbackGroup::SharedPtr main_callback_group;
     main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
     voxel_based_approximate_map_loader_ = std::make_unique<VoxelBasedApproximateDynamicMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
-      main_callback_group);
+      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, main_callback_group);
   } else {
     voxel_based_approximate_map_loader_ = std::make_unique<VoxelBasedApproximateStaticMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
+      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_);
   }
 }
 

perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp

@@ -33,8 +33,8 @@ class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader
 public:
   explicit VoxelBasedApproximateStaticMapLoader(
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-    std::string * tf_map_input_frame, std::mutex * mutex)
-  : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+    std::string * tf_map_input_frame)
+  : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame)
   {
     RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n");
   }
@@ -46,10 +46,9 @@ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader
 public:
   VoxelBasedApproximateDynamicMapLoader(
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-    std::string * tf_map_input_frame, std::mutex * mutex,
-    rclcpp::CallbackGroup::SharedPtr main_callback_group)
+    std::string * tf_map_input_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group)
   : VoxelGridDynamicMapLoader(
-      node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group)
+      node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, main_callback_group)
   {
     RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n");
   }

perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp

@@ -54,11 +54,10 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
     rclcpp::CallbackGroup::SharedPtr main_callback_group;
     main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
     voxel_grid_map_loader_ = std::make_unique<VoxelGridDynamicMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
-      main_callback_group);
+      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, main_callback_group);
   } else {
     voxel_grid_map_loader_ = std::make_unique<VoxelGridStaticMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
+      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_);
   }
   tf_input_frame_ = *(voxel_grid_map_loader_->tf_map_input_frame_);
   RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str());

perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp

@@ -33,7 +33,6 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback(
   pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
   const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);
 
-  (*mutex_ptr_).lock();
   map_ptr_ = map_pcl_ptr;
   *tf_map_input_frame_ = map_ptr_->header.frame_id;
   // voxel
@@ -53,12 +52,15 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback(
     }
   }
   tree_->setInputCloud(map_ptr_);
-  (*mutex_ptr_).unlock();
+  is_initialized_.store(true, std::memory_order_release);
 }
 
 bool VoxelDistanceBasedStaticMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
+  if (!is_initialized_.load(std::memory_order_acquire)) {
+    return false;
+  }
   if (voxel_map_ptr_ == NULL) {
     return false;
   }
@@ -124,11 +126,10 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC
     rclcpp::CallbackGroup::SharedPtr main_callback_group;
     main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
     voxel_distance_based_map_loader_ = std::make_unique<VoxelDistanceBasedDynamicMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
-      main_callback_group);
+      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, main_callback_group);
   } else {
     voxel_distance_based_map_loader_ = std::make_unique<VoxelDistanceBasedStaticMapLoader>(
-      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
+      this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_);
   }
 }
 

perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp

@@ -43,8 +43,8 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
 public:
   explicit VoxelDistanceBasedStaticMapLoader(
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-    std::string * tf_map_input_frame, std::mutex * mutex)
-  : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+    std::string * tf_map_input_frame)
+  : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame)
   {
     RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n");
   }
@@ -61,10 +61,9 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
 public:
   explicit VoxelDistanceBasedDynamicMapLoader(
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-    std::string * tf_map_input_frame, std::mutex * mutex,
-    rclcpp::CallbackGroup::SharedPtr main_callback_group)
+    std::string * tf_map_input_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group)
   : VoxelGridDynamicMapLoader(
-      node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group)
+      node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, main_callback_group)
   {
     RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n");
   }
@@ -117,9 +116,8 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
     current_voxel_grid_list_item.map_cell_kdtree = tree_tmp;
 
     // add
-    (*mutex_ptr_).lock();
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
     current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
-    (*mutex_ptr_).unlock();
   }
 };
 

perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp

@@ -18,13 +18,12 @@ namespace autoware::compare_map_segmentation
 {
 VoxelGridMapLoader::VoxelGridMapLoader(
   rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-  std::string * tf_map_input_frame, std::mutex * mutex)
+  std::string * tf_map_input_frame)
 : logger_(node->get_logger()),
   voxel_leaf_size_(leaf_size),
   downsize_ratio_z_axis_(downsize_ratio_z_axis)
 {
   tf_map_input_frame_ = tf_map_input_frame;
-  mutex_ptr_ = mutex;
 
   downsampled_map_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(
     "debug/downsampled_map/pointcloud", rclcpp::QoS{1}.transient_local());
@@ -245,8 +244,8 @@ bool VoxelGridMapLoader::is_in_voxel(
 
 VoxelGridStaticMapLoader::VoxelGridStaticMapLoader(
   rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-  std::string * tf_map_input_frame, std::mutex * mutex)
-: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+  std::string * tf_map_input_frame)
+: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame)
 {
   voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_;
   sub_map_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
@@ -262,13 +261,12 @@ void VoxelGridStaticMapLoader::onMapCallback(
   pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
   const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);
   *tf_map_input_frame_ = map_pcl_ptr->header.frame_id;
-  (*mutex_ptr_).lock();
   voxel_map_ptr_.reset(new pcl::PointCloud<pcl::PointXYZ>);
   voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_z_);
   voxel_grid_.setInputCloud(map_pcl_ptr);
   voxel_grid_.setSaveLeafLayout(true);
   voxel_grid_.filter(*voxel_map_ptr_);
-  (*mutex_ptr_).unlock();
+  is_initialized_.store(true, std::memory_order_release);
 
   if (debug_) {
     publish_downsampled_map(*voxel_map_ptr_);
@@ -277,6 +275,9 @@ void VoxelGridStaticMapLoader::onMapCallback(
 bool VoxelGridStaticMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
+  if (!is_initialized_.load(std::memory_order_acquire)) {
+    return false;
+  }
   if (is_close_to_neighbor_voxels(point, distance_threshold, voxel_map_ptr_, voxel_grid_)) {
     return true;
   }
@@ -285,9 +286,8 @@ bool VoxelGridStaticMapLoader::is_close_to_map(
 
 VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
   rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-  std::string * tf_map_input_frame, std::mutex * mutex,
-  rclcpp::CallbackGroup::SharedPtr main_callback_group)
-: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+  std::string * tf_map_input_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group)
+: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame)
 {
   voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_;
   auto timer_interval_ms = node->declare_parameter<int>("timer_interval_ms");

perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp

@@ -26,6 +26,7 @@
 #include <pcl/search/pcl_search.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <atomic>
 #include <map>
 #include <memory>
 #include <string>
@@ -85,7 +86,6 @@ class VoxelGridMapLoader
 {
 protected:
   rclcpp::Logger logger_;
-  std::mutex * mutex_ptr_;
   double voxel_leaf_size_;
   double voxel_leaf_size_z_{};
   double downsize_ratio_z_axis_;
@@ -98,7 +98,7 @@ class VoxelGridMapLoader
   typedef typename PointCloud::Ptr PointCloudPtr;
   explicit VoxelGridMapLoader(
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-    std::string * tf_map_input_frame, std::mutex * mutex);
+    std::string * tf_map_input_frame);
 
   virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0;
   static bool is_close_to_neighbor_voxels(
@@ -121,11 +121,12 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_map_;
   VoxelGridPointXYZ voxel_grid_;
   PointCloudPtr voxel_map_ptr_;
+  std::atomic_bool is_initialized_{false};
 
 public:
   explicit VoxelGridStaticMapLoader(
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-    std::string * tf_map_input_frame, std::mutex * mutex);
+    std::string * tf_map_input_frame);
   virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map);
   bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
 };
@@ -145,6 +146,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
 
   /** \brief Map to hold loaded map grid id and it's voxel filter */
   VoxelGridDict current_voxel_grid_dict_;
+  std::mutex dynamic_map_loader_mutex_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_kinematic_state_;
 
   std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
@@ -182,8 +184,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
 public:
   explicit VoxelGridDynamicMapLoader(
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
-    std::string * tf_map_input_frame, std::mutex * mutex,
-    rclcpp::CallbackGroup::SharedPtr main_callback_group);
+    std::string * tf_map_input_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group);
   void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg);
 
   void timer_callback();
@@ -194,17 +195,19 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   bool is_close_to_next_map_grid(
     const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold);
 
-  inline pcl::PointCloud<pcl::PointXYZ> getCurrentDownsampledMapPc() const
+  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_) {
       output = output + *(kv.second.map_cell_pc_ptr);
     }
     return output;
   }
-  inline std::vector<std::string> getCurrentMapIDs() const
+  inline std::vector<std::string> getCurrentMapIDs()
   {
     std::vector<std::string> current_map_ids{};
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
     for (auto & kv : current_voxel_grid_dict_) {
       current_map_ids.push_back(kv.first);
     }
@@ -243,9 +246,9 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
       return;
     }
 
-    (*mutex_ptr_).lock();
     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_) +
@@ -256,14 +259,12 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
       }
       current_voxel_grid_array_.at(index) = std::make_shared<MapGridVoxelInfo>(kv.second);
     }
-    (*mutex_ptr_).unlock();
   }
 
   inline void removeMapCell(const std::string & map_cell_id_to_remove)
   {
-    (*mutex_ptr_).lock();
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
     current_voxel_grid_dict_.erase(map_cell_id_to_remove);
-    (*mutex_ptr_).unlock();
   }
 
   virtual inline void addMapCellAndFilter(
@@ -310,9 +311,8 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
     current_voxel_grid_list_item.map_cell_pc_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>);
     current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp);
     // add
-    (*mutex_ptr_).lock();
+    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
     current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
-    (*mutex_ptr_).unlock();
   }
 };