feat(ndt_scan_matcher): adding exe time to diagnostics for checking r…

…untime_monitor (autowarefoundation#4916) * adding exe time to diagnostics * Update localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml Co-authored-by: kminoda <[email protected]> * order of the parameters in constructor corrected * spell check * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Co-authored-by: kminoda <[email protected]> * Update localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml Co-authored-by: Motz <[email protected]> * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Co-authored-by: Motz <[email protected]> * 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 <[email protected]> Co-authored-by: Motz <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Nov 16, 2023 · db0987a · db0987a
1 parent 9b8e666
commit db0987a
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 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
@@ -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
@@ -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
@@ -99,8 +99,9 @@ NDTScanMatcher::NDTScanMatcher()
   output_pose_covariance_(),
   regularization_enabled_(declare_parameter<bool>("regularization_enabled")),
   estimate_scores_for_degrounded_scan_(
-    declare_parameter<bool>("estimate_scores_for_degrounded_scan")),
-  z_margin_for_ground_removal_(declare_parameter<double>("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<double>("converged_param_nearest_voxel_transformation_likelihood");
 
   lidar_topic_timeout_sec_ = this->declare_parameter<double>("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<double>("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(