fix(compare_map_segmentation): add missing mutex lock

perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp

@@ -32,10 +32,9 @@ 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;
+  std::lock_guard<std::mutex> lock(static_map_loader_mutex_);
   if (!tree_) {
     if (map_ptr_->isOrganized()) {
       tree_.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
@@ -44,13 +43,15 @@ void DistanceBasedStaticMapLoader::onMapCallback(
     }
   }
   tree_->setInputCloud(map_ptr_);
-
-  (*mutex_ptr_).unlock();
+  is_tree_initialized_.store(true, std::memory_order_release);
 }
 
 bool DistanceBasedStaticMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
+  if (!is_tree_initialized_.load(std::memory_order_acquire)) {
+    return false;
+  }
   if (map_ptr_ == NULL) {
     return false;
   }
@@ -126,10 +127,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

@@ -22,6 +22,7 @@
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/search/pcl_search.h>
 
+#include <atomic>
 #include <memory>
 #include <string>
 #include <vector>
@@ -38,11 +39,12 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
 private:
   PointCloudConstPtr map_ptr_;
   pcl::search::Search<pcl::PointXYZ>::Ptr tree_;
+  std::atomic_bool is_tree_initialized_{false};
 
 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 +57,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 +96,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,10 +33,10 @@ 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
+  std::lock_guard<std::mutex> lock(static_map_loader_mutex_);
   voxel_map_ptr_.reset(new pcl::PointCloud<pcl::PointXYZ>);
   voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
   voxel_grid_.setInputCloud(map_pcl_ptr);
@@ -53,12 +53,15 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback(
     }
   }
   tree_->setInputCloud(map_ptr_);
-  (*mutex_ptr_).unlock();
+  is_tree_initialized_.store(true, std::memory_order_release);
 }
 
 bool VoxelDistanceBasedStaticMapLoader::is_close_to_map(
   const pcl::PointXYZ & point, const double distance_threshold)
 {
+  if (!is_tree_initialized_.load(std::memory_order_acquire)) {
+    return false;
+  }
   if (voxel_map_ptr_ == NULL) {
     return false;
   }
@@ -124,11 +127,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

@@ -21,6 +21,7 @@
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/search/pcl_search.h>
 
+#include <atomic>
 #include <map>
 #include <memory>
 #include <string>
@@ -39,12 +40,13 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
 private:
   PointCloudConstPtr map_ptr_;
   pcl::search::Search<pcl::PointXYZ>::Ptr tree_;
+  std::atomic_bool is_tree_initialized_{false};
 
 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 +63,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 +118,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 @@
 {
 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 @@
 
 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,13 @@
   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();
+  std::unique_lock<std::mutex> lock(static_map_loader_mutex_);
   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();
+  lock.unlock();
 
   if (debug_) {
     publish_downsampled_map(*voxel_map_ptr_);
@@ -285,9 +284,8 @@
 
 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

@@ -85,7 +85,6 @@
 {
 protected:
   rclcpp::Logger logger_;
-  std::mutex * mutex_ptr_;
   double voxel_leaf_size_;
   double voxel_leaf_size_z_{};
   double downsize_ratio_z_axis_;
@@ -98,7 +97,7 @@
   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 +120,12 @@
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_map_;
   VoxelGridPointXYZ voxel_grid_;
   PointCloudPtr voxel_map_ptr_;
+  std::mutex static_map_loader_mutex_;
 
 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 +145,7 @@
 
   /** \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 +183,7 @@
 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 +194,19 @@
   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 +245,9 @@
       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 +258,12 @@
       }
       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 +310,8 @@
     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();
   }
 };