fix(compare_map_filter): deadlock bug fix

[badai-nguyen]

Description

• To add missing mutex for current_voxel_grid_dict_ and current_voxel_grid_array_ in dynamic_map_loader mode such as

  autoware.universe/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp

  Lines 355 to 374 in b3caa5c

  	if (current_voxel_grid_dict_.size() == 0) {
  	return false;
  	}
  	// Compare point with map grid that point belong to
  	int map_grid_index = static_cast<int>(
  	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<size_t>(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;
  	}

Related links

- TIER IV internal link

Parent Issue:

• Link

How was this PR tested?

• Testing on Evaluator (wip)
• Confirm by logging_simulator, and the processing_time is similar with before update

Before	After

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[veqcc]

@badai-nguyen
Thank you for your quick work!!

Requests from me:

• could you clarify where is the critical section and which is the shared resource?
• could you avoid writing naive lock() and unlock() ? Instead, use scoped lock as much as possible.

[badai-nguyen]

@veqcc

@badai-nguyen Thank you for your quick work!!

Requests from me:

• could you clarify where is the critical section and which is the shared resource?
• could you avoid writing naive lock() and unlock() ? Instead, use scoped lock as much as possible.

Thanks for your comment, I fixed here 52c5459

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 74 lines in your changes missing coverage. Please review.

Project coverage is 26.32%. Comparing base (92da629) to head (dc5f72d).
Report is 7 commits behind head on main.

Files with missing lines	Patch %	Lines
...rc/voxel_grid_map_loader/voxel_grid_map_loader.cpp	0.00%	29 Missing ⚠️
...ion/src/distance_based_compare_map_filter/node.cpp	0.00%	10 Missing ⚠️
...oxel_based_approximate_compare_map_filter/node.cpp	0.00%	10 Missing ⚠️
...rc/voxel_grid_map_loader/voxel_grid_map_loader.hpp	0.00%	9 Missing and 1 partial ⚠️
...c/voxel_distance_based_compare_map_filter/node.cpp	0.00%	9 Missing ⚠️
...ion/src/distance_based_compare_map_filter/node.hpp	0.00%	3 Missing ⚠️
...c/voxel_distance_based_compare_map_filter/node.hpp	0.00%	3 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #10222      +/-   ##
==========================================
+ Coverage   26.25%   26.32%   +0.06%     
==========================================
  Files        1381     1384       +3     
  Lines      107503   107525      +22     
  Branches    41411    41385      -26     
==========================================
+ Hits        28229    28305      +76     
+ Misses      76459    76397      -62     
- Partials     2815     2823       +8     

Flag	Coverage Δ		*Carryforward flag
differential	6.99% <0.00%> (?)
differential-cuda	6.99% <0.00%> (?)
total	26.34% <ø> (+0.08%)	⬆️	Carriedforward from 1065feb

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[veqcc]

@badai-nguyen

Thank you.
Then please pass required CI checks

[veqcc]

@badai-nguyen

Are you missing mutex for map_grid_size_x_ and map_grid_size_y_ ?

  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;

    pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
    pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);

    auto map_cell_voxel_input_tmp_ptr =
      std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_cell_pc_tmp);

    MapGridVoxelInfo current_voxel_grid_list_item;
    current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x;
    current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y;
    current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x;
    current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y;

    // add kdtree
    pcl::search::Search<pcl::PointXYZ>::Ptr tree_tmp;
    if (!tree_tmp) {
      if (map_cell_voxel_input_tmp_ptr->isOrganized()) {
        tree_tmp.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
      } else {
        tree_tmp.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
      }
    }
    tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr);
    current_voxel_grid_list_item.map_cell_kdtree = tree_tmp;

    // add
    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
    current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
  }

[badai-nguyen]

@badai-nguyen

Are you missing mutex for map_grid_size_x_ and map_grid_size_y_ ?

  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;

    pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
    pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);

    auto map_cell_voxel_input_tmp_ptr =
      std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_cell_pc_tmp);

    MapGridVoxelInfo current_voxel_grid_list_item;
    current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x;
    current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y;
    current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x;
    current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y;

    // add kdtree
    pcl::search::Search<pcl::PointXYZ>::Ptr tree_tmp;
    if (!tree_tmp) {
      if (map_cell_voxel_input_tmp_ptr->isOrganized()) {
        tree_tmp.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
      } else {
        tree_tmp.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
      }
    }
    tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr);
    current_voxel_grid_list_item.map_cell_kdtree = tree_tmp;

    // add
    std::lock_guard<std::mutex> lock(dynamic_map_loader_mutex_);
    current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
  }

@veqcc Thank you for your pointing out.
I fixed by dc5f72d

[veqcc]

LGTM about mutex.
I cannot review the logic of compare_map_filter.