From 1a9dbf4532d8315bd443ad2732cb4fbd34c23673 Mon Sep 17 00:00:00 2001
From: Yamato Ando <yamato.ando@gmail.com>
Date: Thu, 25 Jan 2024 14:39:55 +0900
Subject: [PATCH] fix(ndt_scan_matcher): fix type of
 critical_upper_bound_exe_time_ms (#6161)

* fix type of critical_upper_bound_exe_time_ms

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>

* fix order

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>

---------

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>
---
 .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml     | 6 +++---
 .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp      | 2 +-
 localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 5 ++---
 3 files changed, 6 insertions(+), 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
index 9bc62d3f919c6..a5a4941bfec62 100644
--- 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
index 71895c59ec37d..e2503c20aef6e 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
@@ -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
index 942df03410165..0382d805b7276 100644
--- 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])";