From 3f47d516305551183e098408cff682e79671d733 Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Tue, 16 Jul 2024 14:08:52 +0700 Subject: [PATCH] feat(ndt_scan_matcher): warn if the lidar scan has gone out of the range 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 * style(pre-commit): autofix * Testing Signed-off-by: Anh Nguyen * 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 * style(pre-commit): autofix * Remove last_update_pos_ and use mutex for last_update_position_ Signed-off-by: Anh Nguyen * style(pre-commit): autofix --------- Signed-off-by: Anh Nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ndt_scan_matcher/map_update_module.hpp | 5 +++ .../src/map_update_module.cpp | 38 ++++++++++++++++++- .../src/ndt_scan_matcher_core.cpp | 12 ++++++ 3 files changed, 54 insertions(+), 1 deletion(-) 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 ac913b128b5f1..e09ff5c782e7d 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 @@ -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 & diagnostics_ptr); + void update_map( const geometry_msgs::msg::Point & position, std::unique_ptr & 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_ diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 32e5a0f2a7c3c..012c72e6b3a46 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/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 & 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 & 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(); diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 6d34f666b997b..b394a8f863ceb 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/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);