From 1406692c975dc998de6a5e4590ed50cfc16aab14 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Thu, 19 Jan 2023 14:04:02 +0900 Subject: [PATCH 1/4] Add a function to generate a delay time warning message --- localization/ekf_localizer/CMakeLists.txt | 4 ++- .../include/ekf_localizer/warning_message.hpp | 24 +++++++++++++++++ .../ekf_localizer/src/ekf_localizer.cpp | 23 ++++++---------- .../ekf_localizer/src/warning_message.cpp | 22 +++++++++++++++ .../test/test_warning_message.cpp | 27 +++++++++++++++++++ 5 files changed, 84 insertions(+), 16 deletions(-) create mode 100644 localization/ekf_localizer/include/ekf_localizer/warning_message.hpp create mode 100644 localization/ekf_localizer/src/warning_message.cpp create mode 100644 localization/ekf_localizer/test/test_warning_message.cpp diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 93223ac99b4b9..6e42034e9eac3 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -19,6 +19,7 @@ ament_auto_add_library(ekf_localizer_lib SHARED src/mahalanobis.cpp src/measurement.cpp src/state_transition.cpp + src/warning_message.cpp ) target_link_libraries(ekf_localizer_lib Eigen3::Eigen) @@ -46,7 +47,8 @@ if(BUILD_TESTING) test/test_mahalanobis.cpp test/test_measurement.cpp test/test_numeric.cpp - test/test_state_transition.cpp) + test/test_state_transition.cpp + test/test_warning_message.cpp) foreach(filepath ${TEST_FILES}) add_testcase(${filepath}) diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp new file mode 100644 index 0000000000000..a34100b83c81b --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -0,0 +1,24 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__WARNING_MESSAGE_HPP_ +#define EKF_LOCALIZER__WARNING_MESSAGE_HPP_ + +#include + +#include + +std::string delayTimeWarningMessage(const double delay_time); + +#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 e12e6dc8d9bb9..03be52cb14648 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -22,6 +22,7 @@ #include "ekf_localizer/state_index.hpp" #include "ekf_localizer/state_transition.hpp" #include "ekf_localizer/warning.hpp" +#include "ekf_localizer/warning_message.hpp" #include #include @@ -403,15 +404,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 */ @@ -432,7 +424,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(delayTimeWarningMessage(delay_time), 1000); + } + delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); @@ -514,12 +509,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(delayTimeWarningMessage(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 new file mode 100644 index 0000000000000..9703072035cbc --- /dev/null +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -0,0 +1,22 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/warning_message.hpp" + +#include + +std::string delayTimeWarningMessage(const double delay_time) +{ + return fmt::format("Delay time {} cannot be less than zero.", delay_time); +} diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp new file mode 100644 index 0000000000000..6097d82051cbc --- /dev/null +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -0,0 +1,27 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/warning_message.hpp" + +#include + +TEST(DelayTimeWarningMessage, SmokeTest) +{ + EXPECT_STREQ( + delayTimeWarningMessage(-1.0).c_str(), + "Delay time -1 cannot be less than zero."); + EXPECT_STREQ( + delayTimeWarningMessage(-0.4).c_str(), + "Delay time -0.4 cannot be less than zero."); +} From 615a3878922c870e44ac2a52be861b1a74e27721 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 19 Jan 2023 05:07:39 +0000 Subject: [PATCH 2/4] ci(pre-commit): autofix --- localization/ekf_localizer/test/test_warning_message.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index 6097d82051cbc..c9e01df5d82cf 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -18,10 +18,6 @@ TEST(DelayTimeWarningMessage, SmokeTest) { - EXPECT_STREQ( - delayTimeWarningMessage(-1.0).c_str(), - "Delay time -1 cannot be less than zero."); - EXPECT_STREQ( - delayTimeWarningMessage(-0.4).c_str(), - "Delay time -0.4 cannot be less than zero."); + EXPECT_STREQ(delayTimeWarningMessage(-1.0).c_str(), "Delay time -1 cannot be less than zero."); + EXPECT_STREQ(delayTimeWarningMessage(-0.4).c_str(), "Delay time -0.4 cannot be less than zero."); } From d83f293dae87f1bed33d7651320be8ed1f5bc34b Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Tue, 24 Jan 2023 17:35:08 +0900 Subject: [PATCH 3/4] Add a warning message function for each twist and pose --- .../include/ekf_localizer/warning_message.hpp | 3 ++- .../ekf_localizer/src/ekf_localizer.cpp | 4 ++-- .../ekf_localizer/src/warning_message.cpp | 11 ++++++++-- .../test/test_warning_message.cpp | 20 ++++++++++++++++--- 4 files changed, 30 insertions(+), 8 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index a34100b83c81b..e292e1d2f39c8 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -19,6 +19,7 @@ #include -std::string delayTimeWarningMessage(const double delay_time); +std::string poseDelayTimeWarningMessage(const double delay_time); +std::string twistDelayTimeWarningMessage(const double delay_time); #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 03be52cb14648..9a39c468e3f25 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -425,7 +425,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; if (delay_time < 0.0) { - warning_.warnThrottle(delayTimeWarningMessage(delay_time), 1000); + warning_.warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); @@ -509,7 +509,7 @@ 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(delayTimeWarningMessage(delay_time), 1000); + warning_.warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 9703072035cbc..7e64604f76277 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -16,7 +16,14 @@ #include -std::string delayTimeWarningMessage(const double delay_time) +std::string poseDelayTimeWarningMessage(const double delay_time) { - return fmt::format("Delay time {} cannot be less than zero.", 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); } diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index c9e01df5d82cf..a33f0e5838b66 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -16,8 +16,22 @@ #include -TEST(DelayTimeWarningMessage, SmokeTest) +TEST(PoseDelayTimeWarningMessage, SmokeTest) { - EXPECT_STREQ(delayTimeWarningMessage(-1.0).c_str(), "Delay time -1 cannot be less than zero."); - EXPECT_STREQ(delayTimeWarningMessage(-0.4).c_str(), "Delay time -0.4 cannot be less than zero."); + 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"); } From 2f98ca667b713cceb1460b75ab0003ec2cf20b02 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 24 Jan 2023 10:08:45 +0000 Subject: [PATCH 4/4] ci(pre-commit): autofix --- .../ekf_localizer/include/ekf_localizer/warning_message.hpp | 2 +- localization/ekf_localizer/src/warning_message.cpp | 5 +++-- localization/ekf_localizer/test/test_warning_message.cpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index c418f7b3b3807..dbe44df702af0 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -21,4 +21,4 @@ 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_ \ No newline at end of file +#endif // EKF_LOCALIZER__WARNING_MESSAGE_HPP_ diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 99c1ba8580a67..1088d5c061375 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -14,9 +14,10 @@ #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}"; @@ -33,4 +34,4 @@ std::string mahalanobisWarningMessage(const double distance, const double max_di { const std::string s = "The Mahalanobis distance {:.4f} is over the limit {:.4f}."; return fmt::format(s, distance, max_distance); -} \ No newline at end of file +} diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index 4a7825db36553..fc86df0d6cd80 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -41,4 +41,4 @@ TEST(MahalanobisWarningMessage, SmokeTest) EXPECT_STREQ( mahalanobisWarningMessage(1.0, 0.5).c_str(), "The Mahalanobis distance 1.0000 is over the limit 0.5000."); -} \ No newline at end of file +}