diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index 019fe0825013c..dbe44df702af0 100644 --- 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 +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 index 580262f08a3eb..5243015a64bae 100644 --- 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 index 5dbeaa847509a..1088d5c061375 100644 --- 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 -#include + +#include + +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 index d499a80f2acd5..fc86df0d6cd80 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -16,6 +16,26 @@ #include +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(