refactor(ekf_localizer): add a function to generate a delay time warn…

…ing message (autowarefoundation#2694) * Add a function to generate a delay time warning message * Add a warning message function for each twist and pose Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
asana17 · Feb 8, 2023 · 6773c69 · 6773c69
1 parent 829a26a
commit 6773c69
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 16 deletions.
diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp
@@ -17,6 +17,8 @@
 
 #include <string>
 
+std::string poseDelayTimeWarningMessage(const double delay_time);
+std::string twistDelayTimeWarningMessage(const double delay_time);
 std::string mahalanobisWarningMessage(const double distance, const double max_distance);
 
 #endif  // EKF_LOCALIZER__WARNING_MESSAGE_HPP_
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -377,15 +377,6 @@ void EKFLocalizer::initEKF()
   ekf_.init(X, P, params_.extend_state_step);
 }
 
-void warnIfPoseDelayTimeLessThanZero(const Warning & warning, const double delay_time)
-{
-  if (delay_time < 0.0) {
-    const auto s =
-      fmt::format("Pose time stamp is inappropriate, set delay to 0[s]. delay = {}", delay_time);
-    warning.warnThrottle(s, 1000);
-  }
-}
-
 /*
  * measurementUpdatePose
  */
@@ -406,7 +397,10 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
 
   /* Calculate delay step */
   double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay;
-  warnIfPoseDelayTimeLessThanZero(warning_, delay_time);
+  if (delay_time < 0.0) {
+    warning_.warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000);
+  }
+
   delay_time = std::max(delay_time, 0.0);
 
   int delay_step = std::roundf(delay_time / ekf_dt_);
@@ -488,12 +482,10 @@ void EKFLocalizer::measurementUpdateTwist(
   /* Calculate delay step */
   double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay;
   if (delay_time < 0.0) {
-    warning_.warnThrottle(
-      fmt::format(
-        "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time),
-      1000);
-    delay_time = 0.0;
+    warning_.warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000);
   }
+  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) {
     warning_.warnThrottle(

diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp
@@ -15,7 +15,20 @@
 #include "ekf_localizer/warning_message.hpp"
 
 #include <fmt/core.h>
-#include <gtest/gtest.h>
+
+#include <string>
+
+std::string poseDelayTimeWarningMessage(const double delay_time)
+{
+  const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}";
+  return fmt::format(s, delay_time);
+}
+
+std::string twistDelayTimeWarningMessage(const double delay_time)
+{
+  const std::string s = "Twist time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}";
+  return fmt::format(s, delay_time);
+}
 
 std::string mahalanobisWarningMessage(const double distance, const double max_distance)
 {

diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp
@@ -16,6 +16,26 @@
 
 #include <gtest/gtest.h>
 
+TEST(PoseDelayTimeWarningMessage, SmokeTest)
+{
+  EXPECT_STREQ(
+    poseDelayTimeWarningMessage(-1.0).c_str(),
+    "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000");
+  EXPECT_STREQ(
+    poseDelayTimeWarningMessage(-0.4).c_str(),
+    "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400");
+}
+
+TEST(TwistDelayTimeWarningMessage, SmokeTest)
+{
+  EXPECT_STREQ(
+    twistDelayTimeWarningMessage(-1.0).c_str(),
+    "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000");
+  EXPECT_STREQ(
+    twistDelayTimeWarningMessage(-0.4).c_str(),
+    "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400");
+}
+
 TEST(MahalanobisWarningMessage, SmokeTest)
 {
   EXPECT_STREQ(