From e7bc713a521dd2e018afe880c1f5f11cb2bbcdc7 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 26 Dec 2023 17:28:48 +0900 Subject: [PATCH] fix(ndt_scan_matcher): fixed a lock scope in update_ndt (#5951) Fixed the lock scope in update_ndt Signed-off-by: Shintaro SAKODA --- .../src/map_update_module.cpp | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 18aac53227589..783368aac813a 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -144,21 +144,30 @@ void MapUpdateModule::update_ndt( } const auto exe_start_time = std::chrono::system_clock::now(); - NormalDistributionsTransform backup_ndt = *ndt_ptr_; + const size_t add_size = maps_to_add.size(); + + // Perform heavy processing outside of the lock scope + std::vector>> points_pcl(add_size); + for (size_t i = 0; i < add_size; i++) { + points_pcl[i] = pcl::make_shared>(); + pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); + } + + (*ndt_ptr_mutex_).lock(); // Add pcd - for (const auto & map_to_add : maps_to_add) { - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); - backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + for (size_t i = 0; i < add_size; i++) { + ndt_ptr_->addTarget(points_pcl[i], maps_to_add[i].cell_id); } // Remove pcd for (const std::string & map_id_to_remove : map_ids_to_remove) { - backup_ndt.removeTarget(map_id_to_remove); + ndt_ptr_->removeTarget(map_id_to_remove); } - backup_ndt.createVoxelKdtree(); + ndt_ptr_->createVoxelKdtree(); + + (*ndt_ptr_mutex_).unlock(); const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = @@ -166,14 +175,6 @@ void MapUpdateModule::update_ndt( const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); - // swap - (*ndt_ptr_mutex_).lock(); - // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should - // ideally be avoided. But I will leave this for now since I cannot come up with a solution other - // than using pointer of pointer. - *ndt_ptr_ = backup_ndt; - (*ndt_ptr_mutex_).unlock(); - publish_partial_pcd_map(); }