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,15 @@ 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 |
+| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| 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 +207,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,9 @@
     proc_stddev_yaw_c: 0.005
     proc_stddev_vx_c: 10.0
     proc_stddev_wz_c: 5.0
+
+    # for diagnostics
+    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,40 @@
+// 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_msgs/msg/diagnostic_status.hpp>
+
+#include <string>
+#include <vector>
+
+diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated);
+
+diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated(
+  const std::string & measurement_type, const size_t no_update_count,
+  const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error);
+diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize(
+  const std::string & measurement_type, const size_t queue_size);
+diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate(
+  const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time,
+  const double delay_time_threshold);
+diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate(
+  const std::string & measurement_type, const bool is_passed_mahalanobis_gate,
+  const double mahalanobis_distance, const double mahalanobis_distance_threshold);
+
+diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus(
+  const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array);
+
+#endif  // EKF_LOCALIZER__DIAGNOSTICS_HPP_

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -25,6 +25,7 @@
 #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>
@@ -126,6 +127,8 @@ class EKFLocalizer : public rclcpp::Node
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_biased_pose_;
   //!< @brief ekf estimated yaw bias publisher
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_biased_pose_cov_;
+  //!< @brief diagnostics publisher
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr pub_diag_;
   //!< @brief initial pose subscriber
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;
   //!< @brief measurement pose with covariance subscriber
@@ -144,6 +147,7 @@ class EKFLocalizer : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_tf_;
   //!< @brief tf broadcaster
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_br_;
+
   //!< @brief  extended kalman filter instance.
   TimeDelayKalmanFilter ekf_;
   Simple1DFilter z_filter_;
@@ -167,6 +171,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_mahalanobis_gate_;
+  double pose_mahalanobis_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_mahalanobis_gate_;
+  double twist_mahalanobis_distance_;
+
   AgedObjectQueue<geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr> pose_queue_;
   AgedObjectQueue<geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr> twist_queue_;
 
@@ -221,13 +241,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
@@ -246,6 +266,11 @@ class EKFLocalizer : public rclcpp::Node
    */
   void publishEstimateResult();
 
+  /**
+   * @brief publish diagnostics message
+   */
+  void publishDiagnostics();
+
   /**
    * @brief for debug
    */

localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp

@@ -39,7 +39,15 @@
     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)),
+    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 +67,10 @@
   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 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,7 @@
 
   <build_depend>eigen</build_depend>
 
+  <depend>diagnostic_msgs</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>kalman_filter</depend>

localization/ekf_localizer/src/diagnostics.cpp

@@ -0,0 +1,169 @@
+// 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_msgs/msg/diagnostic_status.hpp>
+
+#include <string>
+#include <vector>
+
+diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "is_activated";
+  key_value.value = is_activated ? "True" : "False";
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (!is_activated) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]process is not activated";
+  }
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated(
+  const std::string & measurement_type, const size_t no_update_count,
+  const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = measurement_type + "_no_update_count";
+  key_value.value = std::to_string(no_update_count);
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_no_update_count_threshold_warn";
+  key_value.value = std::to_string(no_update_count_threshold_warn);
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_no_update_count_threshold_error";
+  key_value.value = std::to_string(no_update_count_threshold_error);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (no_update_count >= no_update_count_threshold_warn) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]" + measurement_type + " is not updated";
+  }
+  if (no_update_count >= no_update_count_threshold_error) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    stat.message = "[ERROR]" + measurement_type + " is not updated";
+  }
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize(
+  const std::string & measurement_type, const size_t queue_size)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = measurement_type + "_queue_size";
+  key_value.value = std::to_string(queue_size);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate(
+  const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time,
+  const double delay_time_threshold)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = measurement_type + "_is_passed_delay_gate";
+  key_value.value = is_passed_delay_gate ? "True" : "False";
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_delay_time";
+  key_value.value = std::to_string(delay_time);
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_delay_time_threshold";
+  key_value.value = std::to_string(delay_time_threshold);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (!is_passed_delay_gate) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]" + measurement_type + " topic is delay";
+  }
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate(
+  const std::string & measurement_type, const bool is_passed_mahalanobis_gate,
+  const double mahalanobis_distance, const double mahalanobis_distance_threshold)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = measurement_type + "_is_passed_mahalanobis_gate";
+  key_value.value = is_passed_mahalanobis_gate ? "True" : "False";
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_mahalanobis_distance";
+  key_value.value = std::to_string(mahalanobis_distance);
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_mahalanobis_distance_threshold";
+  key_value.value = std::to_string(mahalanobis_distance_threshold);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (!is_passed_mahalanobis_gate) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]mahalanobis distance of " + measurement_type + " topic is large";
+  }
+
+  return stat;
+}
+
+// The highest level within the stat_array will be reflected in the merged_stat.
+// When all stat_array entries are 'OK,' the message of merged_stat will be "OK"
+diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus(
+  const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array)
+{
+  diagnostic_msgs::msg::DiagnosticStatus merged_stat;
+
+  for (const auto & stat : stat_array) {
+    if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
+      if (!merged_stat.message.empty()) {
+        merged_stat.message += "; ";
+      }
+      merged_stat.message += stat.message;
+    }
+    if (stat.level > merged_stat.level) {
+      merged_stat.level = stat.level;
+    }
+    for (const auto & value : stat.values) {
+      merged_stat.values.push_back(value);
+    }
+  }
+
+  if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
+    merged_stat.message = "OK";
+  }
+
+  return merged_stat;
+}