feat(ndt_scan_matcher): warn if the lidar scan has gone out of the range of the loaded map

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -52,6 +52,8 @@ class MapUpdateModule
     rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr,
     HyperParameters::DynamicMapLoading param);
 
+  bool out_of_map_range(const geometry_msgs::msg::Point & position);
+
 private:
   friend class NDTScanMatcher;
 
@@ -62,6 +64,7 @@ class MapUpdateModule
   [[nodiscard]] bool should_update_map(
     const geometry_msgs::msg::Point & position,
     std::unique_ptr<DiagnosticsModule> & diagnostics_ptr);
+
   void update_map(
     const geometry_msgs::msg::Point & position,
     std::unique_ptr<DiagnosticsModule> & diagnostics_ptr);

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -107,6 +107,20 @@
   return distance > param_.update_distance;
 }
 
+bool MapUpdateModule::out_of_map_range(const geometry_msgs::msg::Point & position)
+{
+  if (last_update_position_ == std::nullopt) {
+    return true;
+  }
+
+  const double dx = position.x - last_update_position_.value().x;
+  const double dy = position.y - last_update_position_.value().y;
+  const double distance = std::hypot(dx, dy);
+
+  // check distance_last_update_position_to_current_position
+  return (distance + param_.lidar_radius > param_.map_radius);
+}
+
 void MapUpdateModule::update_map(
   const geometry_msgs::msg::Point & position, std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
 {

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -439,6 +439,18 @@
     add_regularization_pose(sensor_ros_time);
   }
 
+  // Warn if the lidar has gone out of the map range
+  if (map_update_module_->out_of_map_range(
+        interpolation_result.interpolated_pose.pose.pose.position)) {
+    std::stringstream msg;
+
+    msg << "Lidar has gone out of the map range";
+    diagnostics_scan_points_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, msg.str());
+
+    RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, msg.str());
+  }
+
   // check is_set_map_points
   const bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr);
   diagnostics_scan_points_->add_key_value("is_set_map_points", is_set_map_points);