From bb6580ff7d3ed5697788fd51321f27dcc42a7523 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Muhammed=20Yavuz=20K=C3=B6seo=C4=9Flu?= Date: Wed, 27 Sep 2023 07:16:55 +0300 Subject: [PATCH] feat(ndt_scan_matcher): adding exe time to diagnostics for checking runtime_monitor (#4916) * adding exe time to diagnostics * Update localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> * order of the parameters in constructor corrected * spell check * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> * Update localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml Co-authored-by: Motz <83898149+Motsu-san@users.noreply.github.com> * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Co-authored-by: Motz <83898149+Motsu-san@users.noreply.github.com> * rebase branch to main * the actual execution time added to diagnostics message * execution_time parameter corrected * integration with header file * style(pre-commit): autofix * parameter naming --------- Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Motz <83898149+Motsu-san@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/ndt_scan_matcher.param.yaml | 3 +++ .../ndt_scan_matcher/ndt_scan_matcher_core.hpp | 3 +++ .../src/ndt_scan_matcher_core.cpp | 18 ++++++++++++++++-- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index c8afa9ea1a916..ecd6c65fa11df 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -85,3 +85,6 @@ # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 + + # The execution time which means probably NDT cannot matches scans properly + critical_upper_bound_exe_time_ms: 100 # [ms] diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index ed3b99019f7d9..011cda6ba3356 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -208,6 +208,9 @@ class NDTScanMatcher : public rclcpp::Node double z_margin_for_ground_removal_; bool use_dynamic_map_loading_; + + // The execution time which means probably NDT cannot matches scans properly + int critical_upper_bound_exe_time_ms_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ 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 7bb2b79e88f33..1d1b829bd5262 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -99,8 +99,9 @@ NDTScanMatcher::NDTScanMatcher() output_pose_covariance_(), regularization_enabled_(declare_parameter("regularization_enabled")), estimate_scores_for_degrounded_scan_( - declare_parameter("estimate_scores_for_degrounded_scan")), - z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal")) + declare_parameter("estimate_scores_for_degrounded_scan", false)), + z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal", 0.8)), + critical_upper_bound_exe_time_ms_(100) { (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; @@ -140,6 +141,11 @@ NDTScanMatcher::NDTScanMatcher() this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); + critical_upper_bound_exe_time_ms_ = + this->declare_parameter("critical_upper_bound_exe_time_ms", critical_upper_bound_exe_time_ms_); + + initial_pose_timeout_sec_ = + this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); @@ -292,6 +298,13 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT score is unreliably low. "; } + if ( + state_ptr_->count("execution_time") && + std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += + "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; + } // Ignore local optimal solution if ( state_ptr_->count("is_local_optimal_solution_oscillation") && @@ -528,6 +541,7 @@ void NDTScanMatcher::callback_sensor_points( } else { (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0"; } + (*state_ptr_)["execution_time"] = std::to_string(exe_time); } void NDTScanMatcher::transform_sensor_measurement(