From 9d844dfa8a6b9edb6a0ec43ab7b8ce64cff25dbf Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 4 Mar 2025 21:24:50 +0900 Subject: [PATCH 1/7] fix(compare_map_filter): deadlock bug fix Signed-off-by: badai-nguyen --- .../voxel_grid_map_loader.cpp | 61 ++++++++++++------- .../voxel_grid_map_loader.hpp | 13 +++- 2 files changed, 48 insertions(+), 26 deletions(-) diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index e93b2097e583e..1903f044b82ab 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -325,15 +325,18 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( } void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg) { + std::lock_guard lock(dynamic_map_loader_mutex_); current_position_ = msg->pose.pose.position; } bool VoxelGridDynamicMapLoader::is_close_to_next_map_grid( - const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold) + const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold, + const double origin_x, const double origin_y, const double map_grid_size_x, + const double map_grid_size_y, const int map_grids_x) { int neighbor_map_grid_index = static_cast( - std::floor((point.x - origin_x_) / map_grid_size_x_) + - map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); - + std::floor((point.x - origin_x) / map_grid_size_x) + + map_grids_x * std::floor((point.y - origin_y) / map_grid_size_y)); + std::lock_guard lock(dynamic_map_loader_mutex_); if ( static_cast(neighbor_map_grid_index) >= current_voxel_grid_array_.size() || neighbor_map_grid_index == current_map_grid_index || @@ -352,7 +355,10 @@ 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_); + dynamic_map_loader_mutex_.lock(); if (current_voxel_grid_dict_.size() == 0) { + dynamic_map_loader_mutex_.unlock(); return false; } @@ -363,6 +369,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map( map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + dynamic_map_loader_mutex_.unlock(); return false; } if ( @@ -370,31 +377,39 @@ bool VoxelGridDynamicMapLoader::is_close_to_map( is_close_to_neighbor_voxels( point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_pc_ptr, current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid)) { + dynamic_map_loader_mutex_.unlock(); return true; } + auto origin_x = origin_x_; + auto origin_y = origin_y_; + auto map_grid_size_x = map_grid_size_x_; + auto map_grid_size_y = map_grid_size_y_; + auto map_grids_x = map_grids_x_; + dynamic_map_loader_mutex_.unlock(); + // Compare point with the neighbor map cells if point close to map cell boundary if (is_close_to_next_map_grid( pcl::PointXYZ(point.x - distance_threshold, point.y, point.z), map_grid_index, - distance_threshold)) { + distance_threshold, origin_x, origin_y, map_grid_size_x, map_grid_size_y, map_grids_x)) { return true; } if (is_close_to_next_map_grid( pcl::PointXYZ(point.x + distance_threshold, point.y, point.z), map_grid_index, - distance_threshold)) { + distance_threshold, origin_x, origin_y, map_grid_size_x, map_grid_size_y, map_grids_x)) { return true; } if (is_close_to_next_map_grid( pcl::PointXYZ(point.x, point.y - distance_threshold, point.z), map_grid_index, - distance_threshold)) { + distance_threshold, origin_x, origin_y, map_grid_size_x, map_grid_size_y, map_grids_x)) { return true; } if (is_close_to_next_map_grid( pcl::PointXYZ(point.x, point.y + distance_threshold, point.z), map_grid_index, - distance_threshold)) { + distance_threshold, origin_x, origin_y, map_grid_size_x, map_grid_size_y, map_grids_x)) { return true; } @@ -402,27 +417,27 @@ bool VoxelGridDynamicMapLoader::is_close_to_map( } void VoxelGridDynamicMapLoader::timer_callback() { - if (current_position_ == std::nullopt) { - return; - } - if (last_updated_position_ == std::nullopt) { - request_update_map(current_position_.value()); - last_updated_position_ = current_position_; + dynamic_map_loader_mutex_.lock(); + auto current_position = current_position_; + dynamic_map_loader_mutex_.unlock(); + + if (current_position == std::nullopt) { return; } - - if (should_update_map()) { - last_updated_position_ = current_position_; - request_update_map((current_position_.value())); - last_updated_position_ = current_position_; + if ( + last_updated_position_ == std::nullopt || + should_update_map( + current_position.value(), last_updated_position_.value(), map_update_distance_threshold_)) { + request_update_map(current_position.value()); } + last_updated_position_ = current_position; } -bool VoxelGridDynamicMapLoader::should_update_map() const +bool VoxelGridDynamicMapLoader::should_update_map( + const geometry_msgs::msg::Point & current_point, const geometry_msgs::msg::Point & last_point, + const double map_update_distance_threshold) const { - if ( - distance2D(current_position_.value(), last_updated_position_.value()) > - map_update_distance_threshold_) { + if (distance2D(current_point, last_point) > map_update_distance_threshold) { return true; } return false; diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 3685008ffc4fe..6bc1e3fa2136a 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -190,12 +190,16 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg); void timer_callback(); - bool should_update_map() const; + bool should_update_map( + const geometry_msgs::msg::Point & current_point, const geometry_msgs::msg::Point & last_point, + const double map_update_distance_threshold) const; void request_update_map(const geometry_msgs::msg::Point & position); bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; /** \brief Check if point close to map pointcloud in the */ bool is_close_to_next_map_grid( - const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold); + const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold, + const double origin_x, const double origin_y, const double map_grid_size_x, + const double map_grid_size_y, const int map_grids_x); inline pcl::PointCloud getCurrentDownsampledMapPc() { @@ -235,6 +239,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader /** Update loaded map grid array for fast searching*/ virtual inline void updateVoxelGridArray() { + std::lock_guard lock(dynamic_map_loader_mutex_); origin_x_ = std::floor((current_position_.value().x - map_loader_radius_) / map_grid_size_x_) * map_grid_size_x_ + origin_x_remainder_; @@ -253,7 +258,6 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader 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_) + @@ -275,9 +279,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader virtual inline void addMapCellAndFilter( const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) { + dynamic_map_loader_mutex_.lock(); map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; if (map_grid_size_x_ > max_map_grid_size_ || map_grid_size_y_ > max_map_grid_size_) { + dynamic_map_loader_mutex_.unlock(); RCLCPP_ERROR( logger_, "Map was not split or split map grid size is too large. Split map with grid size smaller " @@ -287,6 +293,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_); origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_); + dynamic_map_loader_mutex_.unlock(); pcl::PointCloud map_cell_pc_tmp; pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); From 52c545995edd9420279073b6c9fca756a5a0e9b4 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 6 Mar 2025 01:48:39 +0900 Subject: [PATCH 2/7] fix: change to lock_guard Signed-off-by: badai-nguyen --- .../node.cpp | 1 + .../node.cpp | 1 + .../node.cpp | 1 + .../voxel_grid_map_loader.cpp | 67 +++++++++---------- .../voxel_grid_map_loader.hpp | 29 ++++---- 5 files changed, 50 insertions(+), 49 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 91f7add844c74..c26172bd4bf6c 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 @@ -76,6 +76,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; } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index bfb0027b4e0a0..ffd5d454d1d34 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,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; } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index a94ae92671caf..6e3e4462797d3 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -80,6 +80,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; } 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 1903f044b82ab..f805385573302 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 @@ -355,39 +355,36 @@ 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_); - dynamic_map_loader_mutex_.lock(); - if (current_voxel_grid_dict_.size() == 0) { - dynamic_map_loader_mutex_.unlock(); - return false; - } - - // Compare point with map grid that point belong to - - int 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_)); - - if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { - dynamic_map_loader_mutex_.unlock(); - return false; - } - if ( - current_voxel_grid_array_.at(map_grid_index) != nullptr && - is_close_to_neighbor_voxels( - point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_pc_ptr, - current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid)) { - dynamic_map_loader_mutex_.unlock(); - return true; + double origin_x, origin_y, map_grid_size_x, map_grid_size_y; + int map_grids_x, map_grid_index; + { + std::lock_guard lock(dynamic_map_loader_mutex_); + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + origin_x = origin_x_; + origin_y = origin_y_; + map_grid_size_x = map_grid_size_x_; + map_grid_size_y = map_grid_size_y_; + map_grids_x = map_grids_x_; + // Compare point with map grid that point belong to + + map_grid_index = static_cast( + std::floor((point.x - origin_x) / map_grid_size_x) + + map_grids_x * std::floor((point.y - origin_y) / map_grid_size_y)); + + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if ( + current_voxel_grid_array_.at(map_grid_index) != nullptr && + is_close_to_neighbor_voxels( + point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_pc_ptr, + current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid)) { + return true; + } } - auto origin_x = origin_x_; - auto origin_y = origin_y_; - auto map_grid_size_x = map_grid_size_x_; - auto map_grid_size_y = map_grid_size_y_; - auto map_grids_x = map_grids_x_; - dynamic_map_loader_mutex_.unlock(); - // Compare point with the neighbor map cells if point close to map cell boundary if (is_close_to_next_map_grid( @@ -417,9 +414,11 @@ bool VoxelGridDynamicMapLoader::is_close_to_map( } void VoxelGridDynamicMapLoader::timer_callback() { - dynamic_map_loader_mutex_.lock(); - auto current_position = current_position_; - dynamic_map_loader_mutex_.unlock(); + std::optional current_position = std::nullopt; + { + std::lock_guard lock(dynamic_map_loader_mutex_); + current_position = current_position_; + } if (current_position == std::nullopt) { return; 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 6bc1e3fa2136a..dbc1dbd6b6227 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 @@ -279,22 +279,21 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader virtual inline void addMapCellAndFilter( const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) { - dynamic_map_loader_mutex_.lock(); - map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; - map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; - if (map_grid_size_x_ > max_map_grid_size_ || map_grid_size_y_ > max_map_grid_size_) { - dynamic_map_loader_mutex_.unlock(); - RCLCPP_ERROR( - logger_, - "Map was not split or split map grid size is too large. Split map with grid size smaller " - "than %f", - max_map_grid_size_); - } - - origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_); - origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_); - dynamic_map_loader_mutex_.unlock(); + { + std::lock_guard lock(dynamic_map_loader_mutex_); + map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; + map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + if (map_grid_size_x_ > max_map_grid_size_ || map_grid_size_y_ > max_map_grid_size_) { + RCLCPP_ERROR( + logger_, + "Map was not split or split map grid size is too large. Split map with grid size smaller " + "than %f", + max_map_grid_size_); + } + origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_); + origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_); + } pcl::PointCloud map_cell_pc_tmp; pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); From 4cf4db052788aa46bdc4ad184a3a0e1c8ead1474 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 6 Mar 2025 16:39:19 +0900 Subject: [PATCH 3/7] fix: CI error Signed-off-by: badai-nguyen --- .../src/voxel_grid_map_loader/voxel_grid_map_loader.cpp | 4 ++-- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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 f805385573302..99a58eace1002 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -414,7 +414,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map( } void VoxelGridDynamicMapLoader::timer_callback() { - std::optional current_position = std::nullopt; + std::optional current_position; { std::lock_guard lock(dynamic_map_loader_mutex_); current_position = current_position_; @@ -434,7 +434,7 @@ void VoxelGridDynamicMapLoader::timer_callback() bool VoxelGridDynamicMapLoader::should_update_map( const geometry_msgs::msg::Point & current_point, const geometry_msgs::msg::Point & last_point, - const double map_update_distance_threshold) const + const double map_update_distance_threshold) { if (distance2D(current_point, last_point) > map_update_distance_threshold) { 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 dbc1dbd6b6227..642cefe610e49 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -190,9 +190,9 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg); void timer_callback(); - bool should_update_map( + static bool should_update_map( const geometry_msgs::msg::Point & current_point, const geometry_msgs::msg::Point & last_point, - const double map_update_distance_threshold) const; + const double map_update_distance_threshold); void request_update_map(const geometry_msgs::msg::Point & position); bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; /** \brief Check if point close to map pointcloud in the */ From 7d8359a6b08b90ad1fdfd455ed8562232517245b Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 7 Mar 2025 04:55:43 +0900 Subject: [PATCH 4/7] reduce scope of mutex Signed-off-by: badai-nguyen --- .../node.cpp | 49 ++++++++++--------- .../node.cpp | 41 +++++++++------- .../node.cpp | 34 +++++++------ .../voxel_grid_map_loader.hpp | 3 ++ 4 files changed, 71 insertions(+), 56 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 c26172bd4bf6c..7db994c890a86 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,40 +72,41 @@ bool DistanceBasedStaticMapLoader::is_close_to_map( } return true; } - 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; - } - if (!isFinite(point)) { - return false; - } + int map_grid_index; + std::shared_ptr> map_cell_kdtree; + { + std::lock_guard lock(dynamic_map_loader_mutex_); + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + if (!isFinite(point)) { + return false; + } - const int map_grid_index = static_cast( - std::floor((point.x - origin_x_) / map_grid_size_x_) + - map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_)); + 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_)); - if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { - return false; - } - if (current_voxel_grid_array_.at(map_grid_index) != nullptr) { - if (current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree == nullptr) { + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { return false; } - std::vector nn_indices(1); - std::vector nn_distances(1); - if (!current_voxel_grid_array_.at(map_grid_index) - ->map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) { + auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index); + if (current_voxel_grid == nullptr) { return false; } - if (nn_distances[0] <= distance_threshold) { - return true; - } + map_cell_kdtree = current_voxel_grid->map_cell_kdtree; } - return false; + + std::vector nn_indices(1); + std::vector nn_distances(1); + if (!map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) { + return false; + } + + return nn_distances[0] <= distance_threshold; } DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index ffd5d454d1d34..499f5b41ac550 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,28 +45,33 @@ 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; - } + VoxelGridPointXYZ map_cell_voxel_grid; + { + std::lock_guard lock(dynamic_map_loader_mutex_); + if (current_voxel_grid_dict_.size() == 0) { + return false; + } - const int 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_)); + const int 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_)); - if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { - return false; - } - if (current_voxel_grid_array_.at(map_grid_index) != nullptr) { - const int index = current_voxel_grid_array_.at(map_grid_index) - ->map_cell_voxel_grid.getCentroidIndexAt( - current_voxel_grid_array_.at(map_grid_index) - ->map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z)); - if (index == -1) { + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { return false; - } else { - return true; } + auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index); + if (current_voxel_grid == nullptr) { + return false; + } + map_cell_voxel_grid = current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid; + } + + const int index = map_cell_voxel_grid.getCentroidIndexAt( + map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z)); + if (index == -1) { + return false; + } else { + return true; } 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 6e3e4462797d3..35aa093e24e13 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -80,23 +80,29 @@ 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; - } + std::shared_ptr> map_cell_kdtree; + VoxelGridPointXYZ map_cell_voxel_grid; + { + std::lock_guard lock(dynamic_map_loader_mutex_); + if (current_voxel_grid_dict_.size() == 0) { + return false; + } - const int 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_)); + const int 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_)); - if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { - return false; + if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { + return false; + } + if (current_voxel_grid_array_.at(map_grid_index) == nullptr) { + return false; + } + map_cell_voxel_grid = current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid; + map_cell_kdtree = current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree; } - if ( - current_voxel_grid_array_.at(map_grid_index) != nullptr && - is_close_to_neighbor_voxels( - point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid, - current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree)) { + if (is_close_to_neighbor_voxels( + point, distance_threshold, map_cell_voxel_grid, map_cell_kdtree)) { return true; } return false; diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 642cefe610e49..69a1846677f4d 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -206,6 +206,9 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader pcl::PointCloud output; std::lock_guard lock(dynamic_map_loader_mutex_); for (const auto & kv : current_voxel_grid_dict_) { + if (kv.second.map_cell_pc_ptr == nullptr) { + continue; + } output = output + *(kv.second.map_cell_pc_ptr); } return output; From c558c3f15ecf3e308fb0cc5c1e20da75d0d1eb87 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 7 Mar 2025 05:39:13 +0900 Subject: [PATCH 5/7] refactor Signed-off-by: badai-nguyen --- .../voxel_based_approximate_compare_map_filter/node.cpp | 7 +------ .../src/voxel_distance_based_compare_map_filter/node.cpp | 7 ++----- 2 files changed, 3 insertions(+), 11 deletions(-) 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 499f5b41ac550..16953a3d4a7f6 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 @@ -68,12 +68,7 @@ bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map( const int index = map_cell_voxel_grid.getCentroidIndexAt( map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z)); - if (index == -1) { - return false; - } else { - return true; - } - return false; + return (index != -1); } VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapFilterComponent( diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index 35aa093e24e13..2bcd4ba7c2fc2 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -101,11 +101,8 @@ bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map( map_cell_voxel_grid = current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid; map_cell_kdtree = current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree; } - if (is_close_to_neighbor_voxels( - point, distance_threshold, map_cell_voxel_grid, map_cell_kdtree)) { - return true; - } - return false; + return is_close_to_neighbor_voxels( + point, distance_threshold, map_cell_voxel_grid, map_cell_kdtree); } VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterComponent( From 869223f252feacf8a067f85f742dbe5834e9cc29 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 7 Mar 2025 05:45:23 +0900 Subject: [PATCH 6/7] chore: refactor Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.cpp | 5 ++--- .../src/voxel_based_approximate_compare_map_filter/node.cpp | 2 +- 2 files changed, 3 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 7db994c890a86..f27bf982978f2 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -75,7 +75,6 @@ bool DistanceBasedStaticMapLoader::is_close_to_map( bool DistanceBasedDynamicMapLoader::is_close_to_map( const pcl::PointXYZ & point, const double distance_threshold) { - int map_grid_index; std::shared_ptr> map_cell_kdtree; { std::lock_guard lock(dynamic_map_loader_mutex_); @@ -86,14 +85,14 @@ bool DistanceBasedDynamicMapLoader::is_close_to_map( return false; } - map_grid_index = static_cast( + const int 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_)); if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { return false; } - auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index); + const auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index); if (current_voxel_grid == nullptr) { 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 16953a3d4a7f6..ec20a3c952d0c 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -59,7 +59,7 @@ bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map( if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) { return false; } - auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index); + const auto & current_voxel_grid = current_voxel_grid_array_.at(map_grid_index); if (current_voxel_grid == nullptr) { return false; } From dc5f72d97d4668e4945bbecf805f8b62a1571c74 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 7 Mar 2025 14:20:13 +0900 Subject: [PATCH 7/7] fix: add missing mutex for map_grid_size_x Signed-off-by: badai-nguyen --- .../src/distance_based_compare_map_filter/node.hpp | 7 +++++-- .../src/voxel_distance_based_compare_map_filter/node.hpp | 7 +++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp index 33c9d9ff3c788..1b2b2dfe1294f 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp @@ -65,8 +65,11 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader inline void addMapCellAndFilter( const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override { - map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; - map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + { + std::lock_guard lock(dynamic_map_loader_mutex_); + map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; + map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + } pcl::PointCloud map_cell_pc_tmp; pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp index 9fb81aa675655..25b30efb1b2c9 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp @@ -71,8 +71,11 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader inline void addMapCellAndFilter( const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override { - map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; - map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + { + std::lock_guard lock(dynamic_map_loader_mutex_); + map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x; + map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y; + } pcl::PointCloud map_cell_pc_tmp; pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);