From d5b160aeebf7feea84ce6c4aa748f202ac76bde4 Mon Sep 17 00:00:00 2001
From: Takeshi Ishita <ishitah.takeshi@gmail.com>
Date: Wed, 8 Feb 2023 10:38:58 +0900
Subject: [PATCH] refactor(ekf_localizer): add a function to check the delay
 step (#2657)

* Add a function to check delay step
* Apply the delay step checker to twist
* Test the delay step cheker
---
 .../include/ekf_localizer/warning_message.hpp  |  4 ++++
 .../ekf_localizer/src/ekf_localizer.cpp        | 16 ++++------------
 .../ekf_localizer/src/warning_message.cpp      | 18 ++++++++++++++++++
 .../test/test_warning_message.cpp              | 18 ++++++++++++++++++
 4 files changed, 44 insertions(+), 12 deletions(-)

diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp
index dbe44df702af0..f6c664eb26451 100644
--- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp
+++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp
@@ -17,6 +17,10 @@
 
 #include <string>
 
+std::string poseDelayStepWarningMessage(
+  const double delay_time, const int extend_state_step, const double ekf_dt);
+std::string twistDelayStepWarningMessage(
+  const double delay_time, const int extend_state_step, const double ekf_dt);
 std::string poseDelayTimeWarningMessage(const double delay_time);
 std::string twistDelayTimeWarningMessage(const double delay_time);
 std::string mahalanobisWarningMessage(const double distance, const double max_distance);
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
index 5243015a64bae..5a62e9c997836 100644
--- a/localization/ekf_localizer/src/ekf_localizer.cpp
+++ b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -404,13 +404,9 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
   delay_time = std::max(delay_time, 0.0);
 
   int delay_step = std::roundf(delay_time / ekf_dt_);
-  if (delay_step > params_.extend_state_step - 1) {
+  if (delay_step >= params_.extend_state_step) {
     warning_.warnThrottle(
-      fmt::format(
-        "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
-        "extend_state_step * ekf_dt : %f [s]",
-        delay_time, params_.extend_state_step * ekf_dt_),
-      1000);
+      poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000);
     return;
   }
   DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time);
@@ -487,13 +483,9 @@ void EKFLocalizer::measurementUpdateTwist(
   delay_time = std::max(delay_time, 0.0);
 
   int delay_step = std::roundf(delay_time / ekf_dt_);
-  if (delay_step > params_.extend_state_step - 1) {
+  if (delay_step >= params_.extend_state_step) {
     warning_.warnThrottle(
-      fmt::format(
-        "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
-        "extend_state_step * ekf_dt : %f [s]",
-        delay_time, params_.extend_state_step * ekf_dt_),
-      1000);
+      twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000);
     return;
   }
   DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time);
diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp
index 1088d5c061375..9a7bbf47a94eb 100644
--- a/localization/ekf_localizer/src/warning_message.cpp
+++ b/localization/ekf_localizer/src/warning_message.cpp
@@ -18,6 +18,24 @@
 
 #include <string>
 
+std::string poseDelayStepWarningMessage(
+  const double delay_time, const int extend_state_step, const double ekf_dt)
+{
+  const std::string s =
+    "Pose delay exceeds the compensation limit, ignored. "
+    "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]";
+  return fmt::format(s, delay_time, extend_state_step * ekf_dt);
+}
+
+std::string twistDelayStepWarningMessage(
+  const double delay_time, const int extend_state_step, const double ekf_dt)
+{
+  const std::string s =
+    "Twist delay exceeds the compensation limit, ignored. "
+    "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]";
+  return fmt::format(s, delay_time, extend_state_step * ekf_dt);
+}
+
 std::string poseDelayTimeWarningMessage(const double delay_time)
 {
   const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}";
diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp
index fc86df0d6cd80..b4a05afa844dc 100644
--- a/localization/ekf_localizer/test/test_warning_message.cpp
+++ b/localization/ekf_localizer/test/test_warning_message.cpp
@@ -14,8 +14,26 @@
 
 #include "ekf_localizer/warning_message.hpp"
 
+#include <rclcpp/rclcpp.hpp>
+
 #include <gtest/gtest.h>
 
+TEST(PoseDelayStepWarningMessage, SmokeTest)
+{
+  EXPECT_STREQ(
+    poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(),
+    "Pose delay exceeds the compensation limit, ignored. "
+    "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]");
+}
+
+TEST(TwistDelayStepWarningMessage, SmokeTest)
+{
+  EXPECT_STREQ(
+    twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(),
+    "Twist delay exceeds the compensation limit, ignored. "
+    "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]");
+}
+
 TEST(PoseDelayTimeWarningMessage, SmokeTest)
 {
   EXPECT_STREQ(