refactor(ekf_localizer): add a warning class (autowarefoundation#1837)

* refactor(ekf_localizer): add a warning class to simplify displaying messages

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -15,6 +15,8 @@
 #ifndef EKF_LOCALIZER__EKF_LOCALIZER_HPP_
 #define EKF_LOCALIZER__EKF_LOCALIZER_HPP_
 
+#include "ekf_localizer/warning.hpp"
+
 #include <kalman_filter/kalman_filter.hpp>
 #include <kalman_filter/time_delay_kalman_filter.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -113,6 +115,8 @@ class EKFLocalizer : public rclcpp::Node
   EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options);
 
 private:
+  const Warning warning_;
+
   //!< @brief ekf estimated pose publisher
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_;
   //!< @brief estimated ekf pose with covariance publisher

localization/ekf_localizer/include/ekf_localizer/warning.hpp

@@ -0,0 +1,43 @@
+// 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_HPP_
+#define EKF_LOCALIZER__WARNING_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+
+class Warning
+{
+public:
+  explicit Warning(rclcpp::Node * node) : node_(node) {}
+
+  void warn(const std::string & message) const
+  {
+    RCLCPP_WARN(node_->get_logger(), message.c_str());
+  }
+
+  void warnThrottle(const std::string & message, const int duration_milliseconds) const
+  {
+    RCLCPP_WARN_THROTTLE(
+      node_->get_logger(), *(node_->get_clock()),
+      std::chrono::milliseconds(duration_milliseconds).count(), message.c_str());
+  }
+
+private:
+  rclcpp::Node * node_;
+};
+
+#endif  // EKF_LOCALIZER__WARNING_HPP_

localization/ekf_localizer/package.xml

@@ -16,6 +16,7 @@
   <build_depend>autoware_cmake</build_depend>
   <build_depend>eigen</build_depend>
 
+  <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>kalman_filter</depend>
   <depend>nav_msgs</depend>

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -18,10 +18,13 @@
 #include "ekf_localizer/measurement.hpp"
 #include "ekf_localizer/state_index.hpp"
 #include "ekf_localizer/state_transition.hpp"
+#include "ekf_localizer/warning.hpp"
 
 #include <rclcpp/logging.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 
+#include <fmt/core.h>
+
 #include <algorithm>
 #include <functional>
 #include <memory>
@@ -39,7 +42,7 @@
 using std::placeholders::_1;
 
 EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options)
-: rclcpp::Node(node_name, node_options), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */)
+: rclcpp::Node(node_name, node_options), warning_(this), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */)
 {
   show_debug_info_ = declare_parameter("show_debug_info", false);
   ekf_rate_ = declare_parameter("predict_frequency", 50.0);
@@ -123,7 +126,7 @@ void EKFLocalizer::updatePredictFrequency()
 {
   if (last_predict_time_) {
     if (get_clock()->now() < *last_predict_time_) {
-      RCLCPP_WARN(get_logger(), "Detected jump back in time");
+      warning_.warn("Detected jump back in time");
     } else {
       ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds();
       DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_);
@@ -291,7 +294,7 @@ bool EKFLocalizer::getTransformFromTF(
       transform = tf_buffer.lookupTransform(parent_frame, child_frame, tf2::TimePointZero);
       return true;
     } catch (tf2::TransformException & ex) {
-      RCLCPP_WARN(get_logger(), "%s", ex.what());
+      warning_.warn(ex.what());
       rclcpp::sleep_for(std::chrono::milliseconds(100));
     }
   }
@@ -438,10 +441,11 @@ void EKFLocalizer::predictKinematicsModel()
 void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose)
 {
   if (pose.header.frame_id != pose_frame_id_) {
-    RCLCPP_WARN_THROTTLE(
-      get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(),
-      "pose frame_id is %s, but pose_frame is set as %s. They must be same.",
-      pose.header.frame_id.c_str(), pose_frame_id_.c_str());
+    warning_.warnThrottle(
+      fmt::format(
+        "pose frame_id is %s, but pose_frame is set as %s. They must be same.",
+        pose.header.frame_id.c_str(), pose_frame_id_.c_str()),
+      2000);
   }
   Eigen::MatrixXd X_curr(dim_x_, 1);  // current state
   ekf_.getLatestX(X_curr);
@@ -454,17 +458,18 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
   double delay_time = (t_curr - pose.header.stamp).seconds() + pose_additional_delay_;
   if (delay_time < 0.0) {
     delay_time = 0.0;
-    RCLCPP_WARN_THROTTLE(
-      get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
-      "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time);
+    warning_.warnThrottle(
+      fmt::format("Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time),
+      1000);
   }
   int delay_step = std::roundf(delay_time / ekf_dt_);
   if (delay_step > extend_state_step_ - 1) {
-    RCLCPP_WARN_THROTTLE(
-      get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
-      "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
-      "extend_state_step * ekf_dt : %f [s]",
-      delay_time, extend_state_step_ * ekf_dt_);
+    warning_.warnThrottle(
+      fmt::format(
+        "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
+        "extend_state_step * ekf_dt : %f [s]",
+        delay_time, extend_state_step_ * ekf_dt_),
+      1000);
     return;
   }
   DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time);
@@ -480,8 +485,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
   y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw;
 
   if (isnan(y.array()).any() || isinf(y.array()).any()) {
-    RCLCPP_WARN(
-      get_logger(),
+    warning_.warn(
       "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
     return;
   }
@@ -494,10 +498,10 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
   ekf_.getLatestP(P_curr);
   P_y = P_curr.block(0, 0, dim_y, dim_y);
   if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) {
-    RCLCPP_WARN_THROTTLE(
-      get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(),
+    warning_.warnThrottle(
       "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore "
-      "measurement data.");
+      "measurement data.",
+      2000);
     return;
   }
 
@@ -539,18 +543,20 @@ void EKFLocalizer::measurementUpdateTwist(
   /* Calculate delay step */
   double delay_time = (t_curr - twist.header.stamp).seconds() + twist_additional_delay_;
   if (delay_time < 0.0) {
-    RCLCPP_WARN_THROTTLE(
-      get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
-      "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time);
+    warning_.warnThrottle(
+      fmt::format(
+        "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time),
+      1000);
     delay_time = 0.0;
   }
   int delay_step = std::roundf(delay_time / ekf_dt_);
   if (delay_step > extend_state_step_ - 1) {
-    RCLCPP_WARN_THROTTLE(
-      get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
-      "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
-      "extend_state_step * ekf_dt : %f [s]",
-      delay_time, extend_state_step_ * ekf_dt_);
+    warning_.warnThrottle(
+      fmt::format(
+        "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
+        "extend_state_step * ekf_dt : %f [s]",
+        delay_time, extend_state_step_ * ekf_dt_),
+      1000);
     return;
   }
   DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time);
@@ -560,8 +566,7 @@ void EKFLocalizer::measurementUpdateTwist(
   y << twist.twist.twist.linear.x, twist.twist.twist.angular.z;
 
   if (isnan(y.array()).any() || isinf(y.array()).any()) {
-    RCLCPP_WARN(
-      get_logger(),
+    warning_.warn(
       "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message.");
     return;
   }
@@ -574,10 +579,10 @@ void EKFLocalizer::measurementUpdateTwist(
   ekf_.getLatestP(P_curr);
   P_y = P_curr.block(4, 4, dim_y, dim_y);
   if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) {
-    RCLCPP_WARN_THROTTLE(
-      get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(),
+    warning_.warnThrottle(
       "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore "
-      "measurement data.");
+      "measurement data.",
+      2000);
     return;
   }