From 145734c76e12dcc0513e481090b57288b2df7d86 Mon Sep 17 00:00:00 2001
From: Anh Nguyen <anh.nguyen.2@tier4.jp>
Date: Wed, 28 Feb 2024 05:21:28 +0700
Subject: [PATCH 1/2] feat(ndt_scan_matcher): zero mutex blocking for ndt map
update (#6480)
Added a dummy pointer to delay the destructor of NDT when switching ndt ptr
Signed-off-by: anhnv3991 <anh.nguyen.2@tier4.jp>
---
localization/ndt_scan_matcher/src/map_update_module.cpp | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
index 17bafcfedfe80..ce34bab809997 100644
--- a/localization/ndt_scan_matcher/src/map_update_module.cpp
+++ b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -70,6 +70,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
// lock and rebuild ndt_ptr_
if (need_rebuild_) {
ndt_ptr_mutex_->lock();
+
auto param = ndt_ptr_->getParams();
ndt_ptr_.reset(new NdtType);
@@ -88,10 +89,13 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
update_ndt(position, *secondary_ndt_ptr_);
ndt_ptr_mutex_->lock();
+ auto dummy_ptr = ndt_ptr_;
auto input_source = ndt_ptr_->getInputSource();
ndt_ptr_ = secondary_ndt_ptr_;
ndt_ptr_->setInputSource(input_source);
ndt_ptr_mutex_->unlock();
+
+ dummy_ptr.reset();
}
secondary_ndt_ptr_.reset(new NdtType);
From 91616495bbdad51954bb634ee4be3415aa660b9b Mon Sep 17 00:00:00 2001
From: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Date: Tue, 27 Feb 2024 17:08:00 +0900
Subject: [PATCH 2/2] Fixed update_ndt
Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
---
.../include/ndt_scan_matcher/map_update_module.hpp | 2 +-
.../ndt_scan_matcher/src/map_update_module.cpp | 13 +++++++++----
2 files changed, 10 insertions(+), 5 deletions(-)
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
index 38b5a932ae91d..04c762ae950a4 100644
--- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
+++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
@@ -55,7 +55,7 @@ class MapUpdateModule
friend class NDTScanMatcher;
// Update the specified NDT
- void update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
+ bool update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
void update_map(const geometry_msgs::msg::Point & position);
[[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position);
void publish_partial_pcd_map();
diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
index ce34bab809997..5711dbfbedd54 100644
--- a/localization/ndt_scan_matcher/src/map_update_module.cpp
+++ b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -86,7 +86,11 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
// the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly.
// If the updating is done the main ndt_ptr_, either the update or the NDT
// align will be blocked by the other.
- update_ndt(position, *secondary_ndt_ptr_);
+ const bool updated = update_ndt(position, *secondary_ndt_ptr_);
+ if (!updated) {
+ last_update_position_ = position;
+ return;
+ }
ndt_ptr_mutex_->lock();
auto dummy_ptr = ndt_ptr_;
@@ -108,7 +112,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
publish_partial_pcd_map();
}
-void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt)
+bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt)
{
auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
@@ -130,7 +134,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
while (status != std::future_status::ready) {
RCLCPP_INFO(logger_, "waiting response");
if (!rclcpp::ok()) {
- return;
+ return false; // No update
}
status = result.wait_for(std::chrono::seconds(1));
}
@@ -142,7 +146,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size());
if (maps_to_add.empty() && map_ids_to_remove.empty()) {
RCLCPP_INFO(logger_, "Skip map update");
- return;
+ return false; // No update
}
const auto exe_start_time = std::chrono::system_clock::now();
@@ -172,6 +176,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
+ return true; // Updated
}
void MapUpdateModule::publish_partial_pcd_map()