fix(ndt_scan_matcher): fix type of critical_upper_bound_exe_time_ms (a…

…utowarefoundation#6161) * fix type of critical_upper_bound_exe_time_ms Signed-off-by: Yamato Ando <[email protected]> * fix order Signed-off-by: Yamato Ando <[email protected]> --------- Signed-off-by: Yamato Ando <[email protected]>
Interplai · Jun 3, 2024 · f5fc5f8 · f5fc5f8
1 parent 260be59
commit f5fc5f8
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 7 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
@@ -54,6 +54,9 @@
     # Tolerance of distance difference between two initial poses used for linear interpolation. [m]
     initial_pose_distance_tolerance_m: 10.0
 
+    # The execution time which means probably NDT cannot matches scans properly. [ms]
+    critical_upper_bound_exe_time_ms: 100.0
+
     # Number of threads used for parallel computing
     num_threads: 4
 
@@ -98,6 +101,3 @@
 
     # 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. [ms]
-    critical_upper_bound_exe_time_ms: 100
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
@@ -225,7 +225,7 @@ class NDTScanMatcher : public rclcpp::Node
   double z_margin_for_ground_removal_;
 
   // The execution time which means probably NDT cannot matches scans properly
-  int64_t critical_upper_bound_exe_time_ms_;
+  double 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
@@ -118,7 +118,7 @@ NDTScanMatcher::NDTScanMatcher()
   lidar_topic_timeout_sec_ = this->declare_parameter<double>("lidar_topic_timeout_sec");
 
   critical_upper_bound_exe_time_ms_ =
-    this->declare_parameter<int64_t>("critical_upper_bound_exe_time_ms");
+    this->declare_parameter<double>("critical_upper_bound_exe_time_ms");
 
   initial_pose_timeout_sec_ = this->declare_parameter<double>("initial_pose_timeout_sec");
 
@@ -316,8 +316,7 @@ void NDTScanMatcher::publish_diagnostic()
   }
   if (
     state_ptr_->count("execution_time") &&
-    std::stod((*state_ptr_)["execution_time"]) >=
-      static_cast<double>(critical_upper_bound_exe_time_ms_)) {
+    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])";