feat(ndt_scan_matcher): detect loss PCD

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -72,6 +72,13 @@ class MapUpdateModule
 
   std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
 
+  // Metadata of segmented PCDs
+  std::unordered_set<std::string> all_pcds_;
+  std::unordered_set<std::string> cur_pcds_;
+  double pcd_res_x_, pcd_res_y_;
+  double pcd_lower_x_, pcd_lower_y_;
+  std::string pcd_tag_;
+
   HyperParameters::DynamicMapLoading param_;
 
   // Indicate if there is a prefetch thread waiting for being collected

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -146,18 +146,33 @@
     request,
     [](rclcpp::Client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::SharedFuture) {})};
 
+  // Wait for maximum 10 milliseconds
+  std::chrono::milliseconds timeout(10);
+  auto start = std::chrono::system_clock::now();
+
   std::future_status status = result.wait_for(std::chrono::seconds(0));
   while (status != std::future_status::ready) {
     RCLCPP_INFO(logger_, "waiting response");
     if (!rclcpp::ok()) {
       return false;  // No update
     }
+
+    auto cur = std::chrono::system_clock::now();
+
+    // Report an error if wait for too long
+    if (cur - start >= timeout) {
+      RCLCPP_ERROR_STREAM_THROTTLE(
+        logger_, *clock_, 1000, "Waited for incoming PCDs for too long. Abandon NDT update.");
+      return false;
+    }
+
     status = result.wait_for(std::chrono::seconds(1));
   }
 
   auto & maps_to_add = result.get()->new_pointcloud_with_ids;
   auto & map_ids_to_remove = result.get()->ids_to_remove;
 
+  // Check
   RCLCPP_INFO(
     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()) {