feat(ekf_localizer): add covariance ellipse diagnostics (#7708)

* Added ellipse diagnostics to ekf

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Removed an unnecessary parenthesis

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed to ellipse_scale

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed to ellipse_scale

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Updated ekf_diagnostics.png

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Added condition

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed "and" to "or"

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

---------

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Author: SakodaShintaro

localization/ekf_localizer/README.md

@@ -186,10 +186,12 @@ Note that, although the dimension gets larger since the analytical expansion can
 - 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 covariance ellipse is bigger than threshold `warn_ellipse_size` for long axis or `warn_ellipse_size_lateral_direction` for lateral_direction.
 
 ### 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`.
+- The covariance ellipse is bigger than threshold `error_ellipse_size` for long axis or `error_ellipse_size_lateral_direction` for lateral_direction.
 
 ## Known issues
 

localization/ekf_localizer/config/ekf_localizer.param.yaml

@@ -39,6 +39,11 @@
       pose_no_update_count_threshold_error: 100
       twist_no_update_count_threshold_warn: 50
       twist_no_update_count_threshold_error: 100
+      ellipse_scale: 3.0
+      error_ellipse_size: 1.5
+      warn_ellipse_size: 1.2
+      error_ellipse_size_lateral_direction: 0.3
+      warn_ellipse_size_lateral_direction: 0.25
 
     misc:
       # for velocity measurement limitation (Set 0.0 if you want to ignore)

localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp

@@ -33,6 +33,9 @@ diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate(
 diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate(
   const std::string & measurement_type, const bool is_passed_mahalanobis_gate,
   const double mahalanobis_distance, const double mahalanobis_distance_threshold);
+diagnostic_msgs::msg::DiagnosticStatus check_covariance_ellipse(
+  const std::string & name, const double curr_size, const double warn_threshold,
+  const double error_threshold);
 
 diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status(
   const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array);

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -216,7 +216,8 @@ class EKFLocalizer : public rclcpp::Node
   /**
    * @brief publish diagnostics message
    */
-  void publish_diagnostics(const rclcpp::Time & current_time);
+  void publish_diagnostics(
+    const geometry_msgs::msg::PoseStamped & current_ekf_pose, const rclcpp::Time & current_time);
 
   /**
    * @brief update simple 1d filter

localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp

@@ -57,6 +57,13 @@ class HyperParameters
       node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_warn")),
     twist_no_update_count_threshold_error(
       node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_error")),
+    ellipse_scale(node->declare_parameter<double>("diagnostics.ellipse_scale")),
+    error_ellipse_size(node->declare_parameter<double>("diagnostics.error_ellipse_size")),
+    warn_ellipse_size(node->declare_parameter<double>("diagnostics.warn_ellipse_size")),
+    error_ellipse_size_lateral_direction(
+      node->declare_parameter<double>("diagnostics.error_ellipse_size_lateral_direction")),
+    warn_ellipse_size_lateral_direction(
+      node->declare_parameter<double>("diagnostics.warn_ellipse_size_lateral_direction")),
     threshold_observable_velocity_mps(
       node->declare_parameter<double>("misc.threshold_observable_velocity_mps"))
   {
@@ -86,6 +93,12 @@ class HyperParameters
   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;
+  double ellipse_scale;
+  double error_ellipse_size;
+  double warn_ellipse_size;
+  double error_ellipse_size_lateral_direction;
+  double warn_ellipse_size_lateral_direction;
+
   const double threshold_observable_velocity_mps;
 };
 

localization/ekf_localizer/media/ekf_diagnostics.png

@@ -28,6 +28,7 @@
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>kalman_filter</depend>
+  <depend>localization_util</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

localization/ekf_localizer/package.xml

@@ -24,6 +24,31 @@
           "type": "integer",
           "description": "The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times",
           "default": 100
+        },
+        "ellipse_scale": {
+          "type": "number",
+          "description": "The scale factor to apply the error ellipse size",
+          "default": 3.0
+        },
+        "error_ellipse_size": {
+          "type": "number",
+          "description": "The long axis size of the error ellipse to trigger a ERROR state",
+          "default": 1.5
+        },
+        "warn_ellipse_size": {
+          "type": "number",
+          "description": "The long axis size of the error ellipse to trigger a WARN state",
+          "default": 1.2
+        },
+        "error_ellipse_size_lateral_direction": {
+          "type": "number",
+          "description": "The lateral direction size of the error ellipse to trigger a ERROR state",
+          "default": 0.3
+        },
+        "warn_ellipse_size_lateral_direction": {
+          "type": "number",
+          "description": "The lateral direction size of the error ellipse to trigger a WARN state",
+          "default": 0.25
         }
       },
       "required": [

localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json

@@ -139,6 +139,39 @@ diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate(
   return stat;
 }
 
+diagnostic_msgs::msg::DiagnosticStatus check_covariance_ellipse(
+  const std::string & name, const double curr_size, const double warn_threshold,
+  const double error_threshold)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = name + "_size";
+  key_value.value = std::to_string(curr_size);
+  stat.values.push_back(key_value);
+
+  key_value.key = name + "_warn_threshold";
+  key_value.value = std::to_string(warn_threshold);
+  stat.values.push_back(key_value);
+
+  key_value.key = name + "_error_threshold";
+  key_value.value = std::to_string(error_threshold);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (curr_size >= warn_threshold) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]" + name + " is large";
+  }
+  if (curr_size >= error_threshold) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    stat.message = "[ERROR]" + name + " 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 merge_diagnostic_status(

localization/ekf_localizer/src/diagnostics.cpp

@@ -17,6 +17,7 @@
 #include "ekf_localizer/diagnostics.hpp"
 #include "ekf_localizer/string.hpp"
 #include "ekf_localizer/warning_message.hpp"
+#include "localization_util/covariance_ellipse.hpp"
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/unit_conversion.hpp>
@@ -148,7 +149,7 @@ void EKFLocalizer::timer_callback()
   if (!is_activated_) {
     warning_->warn_throttle(
       "The node is not activated. Provide initial pose to pose_initializer", 2000);
-    publish_diagnostics(current_time);
+    publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time);
     return;
   }
 
@@ -241,7 +242,7 @@ void EKFLocalizer::timer_callback()
 
   /* publish ekf result */
   publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist);
-  publish_diagnostics(current_time);
+  publish_diagnostics(current_ekf_pose, current_time);
 }
 
 /*
@@ -390,7 +391,8 @@ void EKFLocalizer::publish_estimate_result(
   pub_odom_->publish(odometry);
 }
 
-void EKFLocalizer::publish_diagnostics(const rclcpp::Time & current_time)
+void EKFLocalizer::publish_diagnostics(
+  const geometry_msgs::msg::PoseStamped & current_ekf_pose, const rclcpp::Time & current_time)
 {
   std::vector<diagnostic_msgs::msg::DiagnosticStatus> diag_status_array;
 
@@ -418,6 +420,18 @@ void EKFLocalizer::publish_diagnostics(const rclcpp::Time & current_time)
     diag_status_array.push_back(check_measurement_mahalanobis_gate(
       "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance,
       params_.twist_gate_dist));
+
+    geometry_msgs::msg::PoseWithCovariance pose_cov;
+    pose_cov.pose = current_ekf_pose.pose;
+    pose_cov.covariance = ekf_module_->get_current_pose_covariance();
+    const autoware::localization_util::Ellipse ellipse =
+      autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
+    diag_status_array.push_back(check_covariance_ellipse(
+      "cov_ellipse_long_axis", ellipse.long_radius, params_.warn_ellipse_size,
+      params_.error_ellipse_size));
+    diag_status_array.push_back(check_covariance_ellipse(
+      "cov_ellipse_lateral_direction", ellipse.size_lateral_direction,
+      params_.warn_ellipse_size_lateral_direction, params_.error_ellipse_size_lateral_direction));
   }
 
   diagnostic_msgs::msg::DiagnosticStatus diag_merged_status;