feat(ekf_localizer): add diagnostics

localization/ekf_localizer/CMakeLists.txt

@@ -16,6 +16,7 @@ ament_auto_find_build_dependencies()
 ament_auto_add_library(ekf_localizer_lib SHARED
   src/ekf_localizer.cpp
   src/covariance.cpp
+  src/diagnostics.cpp
   src/mahalanobis.cpp
   src/measurement.cpp
   src/state_transition.cpp

localization/ekf_localizer/README.md

@@ -88,6 +88,10 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi
 
   The estimated twist with covariance.
 
+- diagnostics (diagnostic_msgs/DiagnosticArray)
+
+  The diagnostic information.
+
 ### Published TF
 
 - base_link
@@ -148,6 +152,16 @@ The parameters are set in `launch/ekf_localizer.launch` .
 
 note: process noise for positions x & y are calculated automatically from nonlinear dynamics.
 
+### For diagnostics
+
+| Name                                  | Type   | Description                                                                                                                                | Default value |
+| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| diagnostics_update_rate               | double | Frequency for diagnostics broadcasting [Hz]                                                                                                | 10.0          |
+| pose_no_update_count_threshold_warn   | size_t | The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times.    | 50            |
+| pose_no_update_count_threshold_error  | size_t | The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times.  | 250           |
+| twist_no_update_count_threshold_warn  | size_t | The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times.   | 50            |
+| twist_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 250           |
+
 ## How to tune EKF parameters
 
 ### 0. Preliminaries
@@ -194,6 +208,23 @@ Note that, although the dimension gets larger since the analytical expansion can
 <img src="./media/ekf_autoware_res.png" width="600">
 </p>
 
+## Diagnostics
+
+<p align="center">
+<img src="./media/ekf_diagnostics.png" width="320">
+</p>
+
+### The conditions that result in a WARN state
+
+- The node is not in the activate state.
+- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`.
+- The timestamp of the Pose/Twist topic is beyond the delay compensation range.
+- The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation.
+
+### The conditions that result in an ERROR state
+
+- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_error`/`twist_no_update_count_threshold_error`.
+
 ## Known issues
 
 - In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation.

localization/ekf_localizer/config/ekf_localizer.param.yaml

@@ -21,3 +21,10 @@
     proc_stddev_yaw_c: 0.005
     proc_stddev_vx_c: 10.0
     proc_stddev_wz_c: 5.0
+
+    # for diagnostics
+    diagnostics_update_rate: 10
+    pose_no_update_count_threshold_warn: 50
+    pose_no_update_count_threshold_error: 250
+    twist_no_update_count_threshold_warn: 50
+    twist_no_update_count_threshold_error: 250

localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp

@@ -0,0 +1,41 @@
+// Copyright 2023 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__DIAGNOSTICS_HPP_
+#define EKF_LOCALIZER__DIAGNOSTICS_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+
+#include <string>
+
+void checkProcessActivated(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const bool * const is_activated_ptr);
+
+void checkMeasurementUpdated(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type,
+  const size_t * const no_update_count_ptr, const size_t * const no_update_count_threshold_warn_ptr,
+  const size_t * const no_update_count_threshold_error_ptr);
+void checkMeasurementQueueSize(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type,
+  const size_t * const queue_size_ptr);
+void checkMeasurementDelayGate(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type,
+  const bool * const is_passed_delay_gate_ptr, const double * const delay_time_ptr,
+  const double * const delay_time_threshold_ptr);
+void checkMeasurementMahalanobisGate(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type,
+  const bool * const is_passed_mahalabobis_gate_ptr, const double * const mahalabobis_distance_ptr,
+  const double * const mahalabobis_distance_threshold_ptr);
+
+#endif  // EKF_LOCALIZER__DIAGNOSTICS_HPP_

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -19,12 +19,14 @@
 #include "ekf_localizer/hyper_parameters.hpp"
 #include "ekf_localizer/warning.hpp"
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
 #include <kalman_filter/kalman_filter.hpp>
 #include <kalman_filter/time_delay_kalman_filter.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
@@ -144,6 +146,20 @@ class EKFLocalizer : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_tf_;
   //!< @brief tf broadcaster
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_br_;
+
+  //!< @brief diagnostic updater
+  std::shared_ptr<diagnostic_updater::Updater> diag_updater_;
+  std::shared_ptr<diagnostic_updater::CompositeDiagnosticTask> diag_composite_task_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_process_activated_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_pose_updated_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_pose_queue_size_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_pose_delay_gate_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_pose_mahalanobis_gate_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_twist_updated_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_twist_queue_size_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_twist_delay_gate_;
+  std::shared_ptr<diagnostic_updater::FunctionDiagnosticTask> diag_twist_mahalanobis_gate_;
+
   //!< @brief  extended kalman filter instance.
   TimeDelayKalmanFilter ekf_;
   Simple1DFilter z_filter_;
@@ -167,6 +183,22 @@ class EKFLocalizer : public rclcpp::Node
 
   bool is_activated_;
 
+  size_t pose_no_update_count_;
+  size_t pose_queue_size_;
+  bool pose_is_passed_delay_gate_;
+  double pose_delay_time_;
+  double pose_delay_time_threshold_;
+  bool pose_is_passed_mahalabobis_gate_;
+  double pose_mahalabobis_distance_;
+
+  size_t twist_no_update_count_;
+  size_t twist_queue_size_;
+  bool twist_is_passed_delay_gate_;
+  double twist_delay_time_;
+  double twist_delay_time_threshold_;
+  bool twist_is_passed_mahalabobis_gate_;
+  double twist_mahalabobis_distance_;
+
   AgedObjectQueue<geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr> pose_queue_;
   AgedObjectQueue<geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr> twist_queue_;
 
@@ -221,13 +253,13 @@ class EKFLocalizer : public rclcpp::Node
    * @brief compute EKF update with pose measurement
    * @param pose measurement value
    */
-  void measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
+  bool measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
 
   /**
    * @brief compute EKF update with pose measurement
    * @param twist measurement value
    */
-  void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist);
+  bool measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist);
 
   /**
    * @brief get transform from frame_id

localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp

@@ -39,7 +39,16 @@
     twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)),
     proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)),
     proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)),
-    proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005))
+    proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)),
+    diagnostics_update_rate(node->declare_parameter("diagnostics_update_rate", 10)),
+    pose_no_update_count_threshold_warn(
+      node->declare_parameter("pose_no_update_count_threshold_warn", 50)),
+    pose_no_update_count_threshold_error(
+      node->declare_parameter("pose_no_update_count_threshold_error", 250)),
+    twist_no_update_count_threshold_warn(
+      node->declare_parameter("twist_no_update_count_threshold_warn", 50)),
+    twist_no_update_count_threshold_error(
+      node->declare_parameter("twist_no_update_count_threshold_error", 250))
   {
   }
 
@@ -59,6 +68,11 @@
   const double proc_stddev_vx_c;   //!< @brief  vx process noise
   const double proc_stddev_wz_c;   //!< @brief  wz process noise
   const double proc_stddev_yaw_c;  //!< @brief  yaw process noise
+  const double diagnostics_update_rate;
+  const size_t pose_no_update_count_threshold_warn;
+  const size_t pose_no_update_count_threshold_error;
+  const size_t twist_no_update_count_threshold_warn;
+  const size_t twist_no_update_count_threshold_error;
 };
 
 #endif  // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_

localization/ekf_localizer/package.xml

@@ -18,6 +18,8 @@
 
   <build_depend>eigen</build_depend>
 
+  <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_updater</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>kalman_filter</depend>

localization/ekf_localizer/src/diagnostics.cpp

@@ -0,0 +1,136 @@
+// Copyright 2023 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/diagnostics.hpp"
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+
+#include <string>
+
+void checkProcessActivated(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const bool * const is_activated_ptr)
+{
+  if (is_activated_ptr == nullptr) {
+    return;
+  }
+
+  stat.add("is_activated", (*is_activated_ptr));
+
+  int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  std::string diag_message;
+  if (!(*is_activated_ptr)) {
+    diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_message = "[WARN]process is not activated";
+  }
+
+  stat.summary(diag_level, diag_message);
+}
+
+void checkMeasurementUpdated(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type,
+  const size_t * const no_update_count_ptr, const size_t * const no_update_count_threshold_warn_ptr,
+  const size_t * const no_update_count_threshold_error_ptr)
+{
+  if (
+    no_update_count_ptr == nullptr || no_update_count_threshold_warn_ptr == nullptr ||
+    no_update_count_threshold_error_ptr == nullptr) {
+    return;
+  }
+
+  stat.add(measurement_type + "_no_update_count", *no_update_count_ptr);
+  stat.add(
+    measurement_type + "_no_update_count_threshold_warn", *no_update_count_threshold_warn_ptr);
+  stat.add(
+    measurement_type + "_no_update_count_threshold_error", *no_update_count_threshold_error_ptr);
+
+  int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  std::string diag_message;
+  if (*no_update_count_ptr >= *no_update_count_threshold_warn_ptr) {
+    diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_message = "[WARN]" + measurement_type + "_queue is empty";
+  }
+  if (*no_update_count_ptr >= *no_update_count_threshold_error_ptr) {
+    diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    diag_message = "[ERROR]" + measurement_type + "_queue is empty";
+  }
+
+  stat.summary(diag_level, diag_message);
+}
+
+void checkMeasurementQueueSize(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type,
+  const size_t * const queue_size_ptr)
+{
+  if (queue_size_ptr == nullptr) {
+    return;
+  }
+
+  stat.add(measurement_type + "_queue_size", *queue_size_ptr);
+
+  int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  std::string diag_message;
+
+  stat.summary(diag_level, diag_message);
+}
+
+void checkMeasurementDelayGate(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type,
+  const bool * const is_passed_delay_gate_ptr, const double * const delay_time_ptr,
+  const double * const delay_time_threshold_ptr)
+{
+  if (
+    is_passed_delay_gate_ptr == nullptr || delay_time_ptr == nullptr ||
+    delay_time_threshold_ptr == nullptr) {
+    return;
+  }
+
+  stat.add(measurement_type + "_is_passed_delay_gate", *is_passed_delay_gate_ptr);
+  stat.add(measurement_type + "_delay_time", *delay_time_ptr);
+  stat.add(measurement_type + "_delay_time_threshold", *delay_time_threshold_ptr);
+
+  int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  std::string diag_message;
+  if (!(*is_passed_delay_gate_ptr)) {
+    diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_message = "[WARN]" + measurement_type + " topic is delay";
+  }
+
+  stat.summary(diag_level, diag_message);
+}
+
+void checkMeasurementMahalanobisGate(
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type,
+  const bool * const is_passed_mahalabobis_gate_ptr, const double * const mahalabobis_distance_ptr,
+  const double * const mahalabobis_distance_threshold_ptr)
+{
+  if (
+    is_passed_mahalabobis_gate_ptr == nullptr || mahalabobis_distance_ptr == nullptr ||
+    mahalabobis_distance_threshold_ptr == nullptr) {
+    return;
+  }
+
+  stat.add(measurement_type + "_is_passed_mahalabobis_gate", *is_passed_mahalabobis_gate_ptr);
+  stat.add(measurement_type + "_mahalabobis_distance", *mahalabobis_distance_ptr);
+  stat.add(
+    measurement_type + "_mahalabobis_distance_threshold", *mahalabobis_distance_threshold_ptr);
+
+  int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  std::string diag_message;
+  if (!(*is_passed_mahalabobis_gate_ptr)) {
+    diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_message = "[WARN]mahalabobis distance of " + measurement_type + " topic is large";
+  }
+
+  stat.summary(diag_level, diag_message);
+}