fix(ndt_scan_matcher): add a bool return indicating whether ndt has b…

…een updated in update_ndt (autowarefoundation#6502) Fixed update_ndt Signed-off-by: Shintaro Sakoda <[email protected]>
Interplai · Jun 3, 2024 · b10d91e · b10d91e
1 parent c61ee0c
commit b10d91e
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 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
@@ -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
@@ -88,7 +88,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_;
@@ -110,7 +114,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>();
 
@@ -132,7 +136,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));
   }
@@ -144,7 +148,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();
@@ -174,6 +178,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()