From 0d6d565d384ae43290ad2e3b05e99181990e854d Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 17 Oct 2024 10:40:01 +0900 Subject: [PATCH 1/9] fix(compare_map_segmentation): missing mutux Signed-off-by: badai-nguyen --- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 4 ++++ 1 file changed, 4 insertions(+) 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 f860e7e6c87fd..b597ce509076f 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 @@ -197,17 +197,21 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader inline pcl::PointCloud getCurrentDownsampledMapPc() const { pcl::PointCloud output; + (*mutex_ptr_).lock(); for (const auto & kv : current_voxel_grid_dict_) { output = output + *(kv.second.map_cell_pc_ptr); } + (*mutex_ptr_).unlock(); return output; } inline std::vector getCurrentMapIDs() const { std::vector current_map_ids{}; + (*mutex_ptr_).lock(); for (auto & kv : current_voxel_grid_dict_) { current_map_ids.push_back(kv.first); } + (*mutex_ptr_).unlock(); return current_map_ids; } inline void updateDifferentialMapCells( From 462efefb57e2a61038c6bdad706bbddfa4bec523 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 17 Oct 2024 14:45:09 +0900 Subject: [PATCH 2/9] chore: rename mutex_ Signed-off-by: badai-nguyen --- .../node.cpp | 12 ++++---- .../node.hpp | 11 ++++--- .../node.cpp | 7 +++-- .../node.hpp | 9 +++--- .../voxel_based_compare_map_filter/node.cpp | 5 ++-- .../node.cpp | 9 +++--- .../node.hpp | 12 ++++---- .../voxel_grid_map_loader.cpp | 17 +++++------ .../voxel_grid_map_loader.hpp | 29 ++++++++----------- 9 files changed, 49 insertions(+), 62 deletions(-) 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 e5ff8da4e4da9..775f6cfc80d4f 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 @@ -32,8 +32,7 @@ void DistanceBasedStaticMapLoader::onMapCallback( pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - - (*mutex_ptr_).lock(); + std::lock_guard lock(stastic_map_loader_mutex_); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; if (!tree_) { @@ -44,8 +43,6 @@ void DistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); - - (*mutex_ptr_).unlock(); } bool DistanceBasedStaticMapLoader::is_close_to_map( @@ -75,6 +72,7 @@ bool DistanceBasedStaticMapLoader::is_close_to_map( bool DistanceBasedDynamicMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) { + std::lock_guard lock(dynamic_map_loader_mutex_); if (current_voxel_grid_dict_.size() == 0) { return false; } @@ -126,10 +124,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( - 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( - this, distance_threshold_, &tf_input_frame_, &mutex_); + distance_based_map_loader_ = + std::make_unique(this, distance_threshold_, &tf_input_frame_); } } 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 d0cb5b3c62992..509303c3a4e0e 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 @@ -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 lock(dynamic_map_loader_mutex_); current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item}); - (*mutex_ptr_).unlock(); } }; 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 3a04c3dc20902..45557de12ae9a 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 @@ -29,6 +29,7 @@ namespace autoware::compare_map_segmentation bool VoxelBasedApproximateStaticMapLoader::is_close_to_map( const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold) { + std::lock_guard lock(stastic_map_loader_mutex_); if (voxel_map_ptr_ == NULL) { return false; } @@ -44,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 lock(dynamic_map_loader_mutex_); if (current_voxel_grid_dict_.size() == 0) { return false; } @@ -95,11 +97,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( - 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( - this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp index 0466d20403ca3..c8ad7c16f59cb 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp +++ b/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"); } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index a3eb866d77e31..e024d2b538e51 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/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( - 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( - 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()); 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 7fdeb5d8ea163..e3737e01fd08b 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 @@ -33,7 +33,7 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - (*mutex_ptr_).lock(); + std::lock_guard lock(stastic_map_loader_mutex_); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; // voxel @@ -53,7 +53,6 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); - (*mutex_ptr_).unlock(); } bool VoxelDistanceBasedStaticMapLoader::is_close_to_map( @@ -77,6 +76,7 @@ bool VoxelDistanceBasedStaticMapLoader::is_close_to_map( bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) { + std::lock_guard lock(dynamic_map_loader_mutex_); if (current_voxel_grid_dict_.size() == 0) { return false; } @@ -124,11 +124,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( - 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( - this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_); } } 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 15ed2a32ff213..015ee52793768 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 @@ -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 lock(dynamic_map_loader_mutex_); current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item}); - (*mutex_ptr_).unlock(); } }; 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 a36372efe4428..e77953c377699 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 @@ -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( "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( @@ -262,13 +261,12 @@ void VoxelGridStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); *tf_map_input_frame_ = map_pcl_ptr->header.frame_id; - (*mutex_ptr_).lock(); + std::lock_guard lock(stastic_map_loader_mutex_); voxel_map_ptr_.reset(new pcl::PointCloud); 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(); if (debug_) { publish_downsampled_map(*voxel_map_ptr_); @@ -285,9 +283,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("timer_interval_ms"); @@ -324,6 +321,7 @@ void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(nav_msgs::msg::Odometry: 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 lock(dynamic_map_loader_mutex_); int neighbor_map_grid_index = static_cast( std::floor((point.x - origin_x_) / map_grid_size_x_) + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); @@ -346,6 +344,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 lock(dynamic_map_loader_mutex_); if (current_voxel_grid_dict_.size() == 0) { 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 b597ce509076f..e963098424047 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 @@ -85,7 +85,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 +97,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 +120,12 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader rclcpp::Subscription::SharedPtr sub_map_; VoxelGridPointXYZ voxel_grid_; PointCloudPtr voxel_map_ptr_; + std::mutex stastic_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 @@ 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::SharedPtr sub_kinematic_state_; std::optional current_position_ = std::nullopt; @@ -182,8 +183,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,24 +194,22 @@ 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 getCurrentDownsampledMapPc() const + inline pcl::PointCloud getCurrentDownsampledMapPc() { pcl::PointCloud output; - (*mutex_ptr_).lock(); + std::lock_guard lock(dynamic_map_loader_mutex_); for (const auto & kv : current_voxel_grid_dict_) { output = output + *(kv.second.map_cell_pc_ptr); } - (*mutex_ptr_).unlock(); return output; } - inline std::vector getCurrentMapIDs() const + inline std::vector getCurrentMapIDs() { std::vector current_map_ids{}; - (*mutex_ptr_).lock(); + std::lock_guard lock(dynamic_map_loader_mutex_); for (auto & kv : current_voxel_grid_dict_) { current_map_ids.push_back(kv.first); } - (*mutex_ptr_).unlock(); return current_map_ids; } inline void updateDifferentialMapCells( @@ -247,7 +245,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader return; } - (*mutex_ptr_).lock(); + std::lock_guard lock(dynamic_map_loader_mutex_); current_voxel_grid_array_.assign( map_grids_x_ * map_grid_size_y_, std::make_shared()); for (const auto & kv : current_voxel_grid_dict_) { @@ -260,14 +258,12 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader } current_voxel_grid_array_.at(index) = std::make_shared(kv.second); } - (*mutex_ptr_).unlock(); } inline void removeMapCell(const std::string & map_cell_id_to_remove) { - (*mutex_ptr_).lock(); + std::lock_guard lock(dynamic_map_loader_mutex_); current_voxel_grid_dict_.erase(map_cell_id_to_remove); - (*mutex_ptr_).unlock(); } virtual inline void addMapCellAndFilter( @@ -314,9 +310,8 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader current_voxel_grid_list_item.map_cell_pc_ptr.reset(new pcl::PointCloud); current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp); // add - (*mutex_ptr_).lock(); + std::lock_guard lock(dynamic_map_loader_mutex_); current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item}); - (*mutex_ptr_).unlock(); } }; From 810003e961234dcbe7bed50b646487b78809c7be Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 17 Oct 2024 18:03:48 +0900 Subject: [PATCH 3/9] fix: remove unnecessary mutex Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.cpp | 1 - .../src/voxel_based_approximate_compare_map_filter/node.cpp | 1 - .../src/voxel_distance_based_compare_map_filter/node.cpp | 1 - .../src/voxel_grid_map_loader/voxel_grid_map_loader.cpp | 2 -- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 2 +- 5 files changed, 1 insertion(+), 6 deletions(-) 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 775f6cfc80d4f..9bb2adffaf7e8 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,7 +72,6 @@ bool DistanceBasedStaticMapLoader::is_close_to_map( bool DistanceBasedDynamicMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) { - std::lock_guard lock(dynamic_map_loader_mutex_); if (current_voxel_grid_dict_.size() == 0) { return false; } 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 45557de12ae9a..1fe1ba9e4a2f3 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,7 +45,6 @@ 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 lock(dynamic_map_loader_mutex_); if (current_voxel_grid_dict_.size() == 0) { return false; } 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 e3737e01fd08b..0b23e7b93b08e 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 @@ -76,7 +76,6 @@ bool VoxelDistanceBasedStaticMapLoader::is_close_to_map( bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) { - std::lock_guard lock(dynamic_map_loader_mutex_); if (current_voxel_grid_dict_.size() == 0) { return false; } 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 e77953c377699..81b012f615525 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 @@ -321,7 +321,6 @@ void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(nav_msgs::msg::Odometry: 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 lock(dynamic_map_loader_mutex_); int neighbor_map_grid_index = static_cast( std::floor((point.x - origin_x_) / map_grid_size_x_) + map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); @@ -344,7 +343,6 @@ 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 lock(dynamic_map_loader_mutex_); if (current_voxel_grid_dict_.size() == 0) { 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 e963098424047..47e47332a7d03 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 @@ -245,9 +245,9 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader return; } - std::lock_guard lock(dynamic_map_loader_mutex_); current_voxel_grid_array_.assign( map_grids_x_ * map_grid_size_y_, std::make_shared()); + std::lock_guard lock(dynamic_map_loader_mutex_); for (const auto & kv : current_voxel_grid_dict_) { int index = static_cast( std::floor((kv.second.min_b_x - origin_x_) / map_grid_size_x_) + From cd70d8d135de6556d406e215d52a1b61e5ec8e9c Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 17 Oct 2024 18:04:46 +0900 Subject: [PATCH 4/9] fix: typos Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.cpp | 2 +- .../src/voxel_based_approximate_compare_map_filter/node.cpp | 2 +- .../src/voxel_distance_based_compare_map_filter/node.cpp | 2 +- .../src/voxel_grid_map_loader/voxel_grid_map_loader.cpp | 2 +- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) 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 9bb2adffaf7e8..f9bba0e6442b8 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 @@ -32,7 +32,7 @@ void DistanceBasedStaticMapLoader::onMapCallback( pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::lock_guard lock(stastic_map_loader_mutex_); + std::lock_guard lock(static_map_loader_mutex_); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; if (!tree_) { 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 1fe1ba9e4a2f3..f177bdf1c58db 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 @@ -29,7 +29,7 @@ namespace autoware::compare_map_segmentation bool VoxelBasedApproximateStaticMapLoader::is_close_to_map( const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold) { - std::lock_guard lock(stastic_map_loader_mutex_); + std::lock_guard lock(static_map_loader_mutex_); if (voxel_map_ptr_ == NULL) { return false; } 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 0b23e7b93b08e..e0b5adcb1c3cd 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 @@ -33,7 +33,7 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::lock_guard lock(stastic_map_loader_mutex_); + std::lock_guard lock(static_map_loader_mutex_); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; // voxel 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 81b012f615525..03cb715407b64 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 @@ -261,7 +261,7 @@ void VoxelGridStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); *tf_map_input_frame_ = map_pcl_ptr->header.frame_id; - std::lock_guard lock(stastic_map_loader_mutex_); + std::lock_guard lock(static_map_loader_mutex_); voxel_map_ptr_.reset(new pcl::PointCloud); voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_z_); voxel_grid_.setInputCloud(map_pcl_ptr); 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 47e47332a7d03..2d0dff436d924 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 @@ -120,7 +120,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader rclcpp::Subscription::SharedPtr sub_map_; VoxelGridPointXYZ voxel_grid_; PointCloudPtr voxel_map_ptr_; - std::mutex stastic_map_loader_mutex_; + std::mutex static_map_loader_mutex_; public: explicit VoxelGridStaticMapLoader( From 061359ea9a96060962b93e9e53af4c5f95337dd3 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Sun, 20 Oct 2024 20:11:05 +0900 Subject: [PATCH 5/9] chore: minimize mutex scope Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.cpp | 3 ++- .../src/voxel_based_approximate_compare_map_filter/node.cpp | 1 - .../src/voxel_distance_based_compare_map_filter/node.cpp | 3 ++- .../src/voxel_grid_map_loader/voxel_grid_map_loader.cpp | 3 ++- 4 files changed, 6 insertions(+), 4 deletions(-) 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 f9bba0e6442b8..b078810713035 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 @@ -32,7 +32,7 @@ void DistanceBasedStaticMapLoader::onMapCallback( pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::lock_guard lock(static_map_loader_mutex_); + std::unique_lock lock(static_map_loader_mutex_); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; if (!tree_) { @@ -43,6 +43,7 @@ void DistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); + lock.unlock(); } bool DistanceBasedStaticMapLoader::is_close_to_map( 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 f177bdf1c58db..7ffb825910f64 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 @@ -29,7 +29,6 @@ namespace autoware::compare_map_segmentation bool VoxelBasedApproximateStaticMapLoader::is_close_to_map( const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold) { - std::lock_guard lock(static_map_loader_mutex_); if (voxel_map_ptr_ == NULL) { return false; } 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 e0b5adcb1c3cd..01b39fa38dffd 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 @@ -33,7 +33,7 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::lock_guard lock(static_map_loader_mutex_); + std::unique_lock lock(static_map_loader_mutex_); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; // voxel @@ -53,6 +53,7 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); + lock.unlock(); } bool VoxelDistanceBasedStaticMapLoader::is_close_to_map( 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 03cb715407b64..d933d0289def7 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 @@ -261,12 +261,13 @@ void VoxelGridStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); *tf_map_input_frame_ = map_pcl_ptr->header.frame_id; - std::lock_guard lock(static_map_loader_mutex_); + std::unique_lock lock(static_map_loader_mutex_); voxel_map_ptr_.reset(new pcl::PointCloud); 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_); + lock.unlock(); if (debug_) { publish_downsampled_map(*voxel_map_ptr_); From 01070934d56860d2c3636ac680f3f02cc413f489 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Sun, 20 Oct 2024 22:37:33 +0900 Subject: [PATCH 6/9] chore: change to lock_guard Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.cpp | 3 +-- .../src/voxel_distance_based_compare_map_filter/node.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) 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 b078810713035..e73444570ce43 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 @@ -32,9 +32,9 @@ void DistanceBasedStaticMapLoader::onMapCallback( pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::unique_lock lock(static_map_loader_mutex_); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; + std::lock_guard lock(static_map_loader_mutex_); if (!tree_) { if (map_ptr_->isOrganized()) { tree_.reset(new pcl::search::OrganizedNeighbor()); @@ -43,7 +43,6 @@ void DistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); - lock.unlock(); } bool DistanceBasedStaticMapLoader::is_close_to_map( 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 01b39fa38dffd..c1dbe90807c5c 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 @@ -33,10 +33,10 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - std::unique_lock lock(static_map_loader_mutex_); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; // voxel + std::lock_guard lock(static_map_loader_mutex_); voxel_map_ptr_.reset(new pcl::PointCloud); voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); voxel_grid_.setInputCloud(map_pcl_ptr); @@ -53,7 +53,6 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); - lock.unlock(); } bool VoxelDistanceBasedStaticMapLoader::is_close_to_map( From 6824d74f5231550b59ede624a91e088b43111121 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 21 Oct 2024 10:58:35 +0900 Subject: [PATCH 7/9] fix: check tree initialization Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.cpp | 4 ++++ .../src/distance_based_compare_map_filter/node.hpp | 1 + .../src/voxel_distance_based_compare_map_filter/node.cpp | 4 ++++ .../src/voxel_distance_based_compare_map_filter/node.hpp | 1 + 4 files changed, 10 insertions(+) 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 e73444570ce43..d6d9365f001b6 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 @@ -43,11 +43,15 @@ void DistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); + is_tree_initialized_ = true; } bool DistanceBasedStaticMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) { + if (!is_tree_initialized_) { + return false; + } if (map_ptr_ == NULL) { return false; } 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 509303c3a4e0e..5f65d443c1a09 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 @@ -38,6 +38,7 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader private: PointCloudConstPtr map_ptr_; pcl::search::Search::Ptr tree_; + std::atomic_bool is_tree_initialized_{false}; public: DistanceBasedStaticMapLoader( 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 c1dbe90807c5c..e6a6b6a4c3839 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 @@ -53,11 +53,15 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); + is_tree_initialized_ = true; } bool VoxelDistanceBasedStaticMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) { + if (!is_tree_initialized_) { + return false; + } if (voxel_map_ptr_ == NULL) { return false; } 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 015ee52793768..0d564ee1457b9 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 @@ -39,6 +39,7 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader private: PointCloudConstPtr map_ptr_; pcl::search::Search::Ptr tree_; + std::atomic_bool is_tree_initialized_{false}; public: explicit VoxelDistanceBasedStaticMapLoader( From 54e1da6da763d84df04326338d6879588d6fab18 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 21 Oct 2024 11:52:48 +0900 Subject: [PATCH 8/9] fix: memory ordering Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.cpp | 4 ++-- .../src/distance_based_compare_map_filter/node.hpp | 1 + .../src/voxel_distance_based_compare_map_filter/node.cpp | 4 ++-- .../src/voxel_distance_based_compare_map_filter/node.hpp | 1 + 4 files changed, 6 insertions(+), 4 deletions(-) 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 d6d9365f001b6..61316861e5ee5 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 @@ -43,13 +43,13 @@ void DistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); - is_tree_initialized_ = true; + 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_) { + if (!is_tree_initialized_.load(std::memory_order_acquire)) { return false; } if (map_ptr_ == NULL) { 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 5f65d443c1a09..27525b56c813d 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 @@ -22,6 +22,7 @@ #include #include +#include #include #include #include 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 e6a6b6a4c3839..a600ea778d07d 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 @@ -53,13 +53,13 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); - is_tree_initialized_ = true; + 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_) { + if (!is_tree_initialized_.load(std::memory_order_acquire)) { return false; } if (voxel_map_ptr_ == NULL) { 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 0d564ee1457b9..ca42946a0bd5c 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 @@ -21,6 +21,7 @@ #include #include +#include #include #include #include From 8ecc1395033acdf9dda3583f165f2146d2dd3354 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 21 Oct 2024 13:13:07 +0900 Subject: [PATCH 9/9] fix: replace all static_map_loader_mutex_ Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.cpp | 5 ++--- .../src/distance_based_compare_map_filter/node.hpp | 2 -- .../src/voxel_distance_based_compare_map_filter/node.cpp | 5 ++--- .../src/voxel_distance_based_compare_map_filter/node.hpp | 2 -- .../src/voxel_grid_map_loader/voxel_grid_map_loader.cpp | 6 ++++-- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 3 ++- 6 files changed, 10 insertions(+), 13 deletions(-) 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 61316861e5ee5..8b44d1661da48 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 @@ -34,7 +34,6 @@ void DistanceBasedStaticMapLoader::onMapCallback( const auto map_pcl_ptr = pcl::make_shared>(map_pcl); map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; - std::lock_guard lock(static_map_loader_mutex_); if (!tree_) { if (map_ptr_->isOrganized()) { tree_.reset(new pcl::search::OrganizedNeighbor()); @@ -43,13 +42,13 @@ void DistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); - is_tree_initialized_.store(true, std::memory_order_release); + is_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)) { + if (!is_initialized_.load(std::memory_order_acquire)) { return false; } if (map_ptr_ == NULL) { 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 27525b56c813d..509303c3a4e0e 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 @@ -22,7 +22,6 @@ #include #include -#include #include #include #include @@ -39,7 +38,6 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader private: PointCloudConstPtr map_ptr_; pcl::search::Search::Ptr tree_; - std::atomic_bool is_tree_initialized_{false}; public: DistanceBasedStaticMapLoader( 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 a600ea778d07d..62f126c0039f4 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 @@ -36,7 +36,6 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( map_ptr_ = map_pcl_ptr; *tf_map_input_frame_ = map_ptr_->header.frame_id; // voxel - std::lock_guard lock(static_map_loader_mutex_); voxel_map_ptr_.reset(new pcl::PointCloud); voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); voxel_grid_.setInputCloud(map_pcl_ptr); @@ -53,13 +52,13 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( } } tree_->setInputCloud(map_ptr_); - is_tree_initialized_.store(true, std::memory_order_release); + is_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)) { + if (!is_initialized_.load(std::memory_order_acquire)) { return false; } if (voxel_map_ptr_ == NULL) { 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 ca42946a0bd5c..015ee52793768 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 @@ -21,7 +21,6 @@ #include #include -#include #include #include #include @@ -40,7 +39,6 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader private: PointCloudConstPtr map_ptr_; pcl::search::Search::Ptr tree_; - std::atomic_bool is_tree_initialized_{false}; public: explicit VoxelDistanceBasedStaticMapLoader( 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 d933d0289def7..2627bbff64866 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 @@ -261,13 +261,12 @@ void VoxelGridStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(map_pcl); *tf_map_input_frame_ = map_pcl_ptr->header.frame_id; - std::unique_lock lock(static_map_loader_mutex_); voxel_map_ptr_.reset(new pcl::PointCloud); 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_); - lock.unlock(); + is_initialized_.store(true, std::memory_order_release); if (debug_) { publish_downsampled_map(*voxel_map_ptr_); @@ -276,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; } 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 2d0dff436d924..f50d23d4af6e8 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 @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -120,7 +121,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader rclcpp::Subscription::SharedPtr sub_map_; VoxelGridPointXYZ voxel_grid_; PointCloudPtr voxel_map_ptr_; - std::mutex static_map_loader_mutex_; + std::atomic_bool is_initialized_{false}; public: explicit VoxelGridStaticMapLoader(