feat(ndt_scan_matcher): warn if the lidar scan has gone out of the ra…

…nge of the loaded map (#7612)

* Warn if the initial NDT pose of the lidar has gone out of the map range

Signed-off-by: Anh Nguyen <[email protected]>

* style(pre-commit): autofix

* Testing

Signed-off-by: Anh Nguyen <[email protected]>

* style(pre-commit): autofix

* Add mutex and a secondary last_update_position to map_update_module to avoid race condition when checking map range

Signed-off-by: Anh Nguyen <[email protected]>

* style(pre-commit): autofix

* Remove last_update_pos_ and use mutex for last_update_position_

Signed-off-by: Anh Nguyen <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Anh Nguyen <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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);
@@ -88,6 +91,8 @@ class MapUpdateModule
   // Indicate if there is a prefetch thread waiting for being collected
   NdtPtrType secondary_ndt_ptr_;
   bool need_rebuild_;
+  // Keep the last_update_position_ unchanged while checking map range
+  std::mutex last_update_position_mtx_;
 };
 
 #endif  // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -82,7 +82,11 @@ void MapUpdateModule::callback_timer(
 bool MapUpdateModule::should_update_map(
   const geometry_msgs::msg::Point & position, std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
 {
+  last_update_position_mtx_.lock();
+
   if (last_update_position_ == std::nullopt) {
+    last_update_position_mtx_.unlock();
+
     need_rebuild_ = true;
     return true;
   }
@@ -91,6 +95,8 @@ bool MapUpdateModule::should_update_map(
   const double dy = position.y - last_update_position_.value().y;
   const double distance = std::hypot(dx, dy);
 
+  last_update_position_mtx_.unlock();
+
   // check distance_last_update_position_to_current_position
   diagnostics_ptr->add_key_value("distance_last_update_position_to_current_position", distance);
   if (distance + param_.lidar_radius > param_.map_radius) {
@@ -107,6 +113,27 @@ bool MapUpdateModule::should_update_map(
   return distance > param_.update_distance;
 }
 
+bool MapUpdateModule::out_of_map_range(const geometry_msgs::msg::Point & position)
+{
+  last_update_position_mtx_.lock();
+
+  if (last_update_position_ == std::nullopt) {
+    last_update_position_mtx_.unlock();
+
+    return true;
+  }
+
+  const double dx = position.x - last_update_position_.value().x;
+  const double dy = position.y - last_update_position_.value().y;
+
+  last_update_position_mtx_.unlock();
+
+  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)
 {
@@ -140,8 +167,12 @@ void MapUpdateModule::update_map(
       diagnostics_ptr->update_level_and_message(
         diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
       RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str());
-      last_update_position_ = position;
       ndt_ptr_mutex_->unlock();
+
+      last_update_position_mtx_.lock();
+      last_update_position_ = position;
+      last_update_position_mtx_.unlock();
+
       return;
     }
 
@@ -159,7 +190,10 @@ void MapUpdateModule::update_map(
     // check is_updated_map
     diagnostics_ptr->add_key_value("is_updated_map", updated);
     if (!updated) {
+      last_update_position_mtx_.lock();
       last_update_position_ = position;
+      last_update_position_mtx_.unlock();
+
       return;
     }
 
@@ -179,7 +213,9 @@ void MapUpdateModule::update_map(
   *secondary_ndt_ptr_ = *ndt_ptr_;
 
   // Memorize the position of the last update
+  last_update_position_mtx_.lock();
   last_update_position_ = position;
+  last_update_position_mtx_.unlock();
 
   // Publish the new ndt maps
   publish_partial_pcd_map();

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -440,6 +440,18 @@ bool NDTScanMatcher::callback_sensor_points_main(
     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);