refactor(ekf_localizer): add a function to check the delay step (auto…

…warefoundation#2657) * Add a function to check delay step * Apply the delay step checker to twist * Test the delay step cheker
badai-nguyen · Apr 4, 2023 · d5b160a · d5b160a
1 parent 28fab8f
commit d5b160a
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 12 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,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
@@ -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
@@ -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
@@ -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(