Merge pull request #859 from tier4/feat/ekf_localizer/ignore_zero_band

feat(ekf_localizer): ignore zero band

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,21 @@ 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           |
+
+### Misc
+
+| Name                              | Type   | Description                                                                                        | Default value  |
+| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------- | :------------- |
+| threshold_observable_velocity_mps | double | Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor | 0.0 (disabled) |
+
 ## How to tune EKF parameters
 
 ### 0. Preliminaries
@@ -194,6 +213,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,12 @@
     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
+
+    # for velocity measurement limitation (Set 0.0 if you want to ignore)
+    threshold_observable_velocity_mps: 0.0 # [m/s]

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,17 @@ class HyperParameters
     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)),
+    threshold_observable_velocity_mps(
+      node->declare_parameter("threshold_observable_velocity_mps", 0.5))
   {
   }
 
@@ -59,6 +69,11 @@ class HyperParameters
   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;
+  const double threshold_observable_velocity_mps;
 };
 
 #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>