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..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 @@ -32,8 +32,6 @@ 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(); 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( - 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..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 @@ -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( - 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..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 @@ -33,7 +33,6 @@ void VoxelDistanceBasedStaticMapLoader::onMapCallback( pcl::fromROSMsg(*map, map_pcl); const auto map_pcl_ptr = pcl::make_shared>(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( - 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..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 @@ -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(); 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(); + 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("timer_interval_ms"); 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..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 @@ -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::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::SharedPtr sub_kinematic_state_; std::optional 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 getCurrentDownsampledMapPc() const + inline pcl::PointCloud getCurrentDownsampledMapPc() { pcl::PointCloud output; + std::lock_guard 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 getCurrentMapIDs() const + inline std::vector getCurrentMapIDs() { std::vector current_map_ids{}; + std::lock_guard 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()); + 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_) + @@ -256,14 +259,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( @@ -310,9 +311,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(); } };