diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index b91282015732e..3b417b12bc6e9 100644 --- a/localization/ekf_localizer/README.md +++ b/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 diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 19bfd2d498b68..4a7696ec9e65e 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/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) diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp index 4c92b2947642b..8815a1758de01 100644 --- a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp +++ b/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 & stat_array); diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 65003f5fe9864..ada9024faceee 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/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 diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 8d76102e5d64d..7c3c03c83b778 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -57,6 +57,13 @@ class HyperParameters node->declare_parameter("diagnostics.twist_no_update_count_threshold_warn")), twist_no_update_count_threshold_error( node->declare_parameter("diagnostics.twist_no_update_count_threshold_error")), + ellipse_scale(node->declare_parameter("diagnostics.ellipse_scale")), + error_ellipse_size(node->declare_parameter("diagnostics.error_ellipse_size")), + warn_ellipse_size(node->declare_parameter("diagnostics.warn_ellipse_size")), + error_ellipse_size_lateral_direction( + node->declare_parameter("diagnostics.error_ellipse_size_lateral_direction")), + warn_ellipse_size_lateral_direction( + node->declare_parameter("diagnostics.warn_ellipse_size_lateral_direction")), threshold_observable_velocity_mps( node->declare_parameter("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; }; diff --git a/localization/ekf_localizer/media/ekf_diagnostics.png b/localization/ekf_localizer/media/ekf_diagnostics.png index 16de42ac2d85a..234b2f1e19b6d 100644 Binary files a/localization/ekf_localizer/media/ekf_diagnostics.png and b/localization/ekf_localizer/media/ekf_diagnostics.png differ diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 07efca6339b98..9c89867025632 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -28,6 +28,7 @@ fmt geometry_msgs kalman_filter + localization_util nav_msgs rclcpp rclcpp_components diff --git a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json index 2e2dca411971e..eda9e7d06f6f4 100644 --- a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json @@ -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": [ diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp index ecbeaf8b2445a..902efeafacdb3 100644 --- a/localization/ekf_localizer/src/diagnostics.cpp +++ b/localization/ekf_localizer/src/diagnostics.cpp @@ -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( diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 11fa2b6713a52..e541939026d95 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.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 #include @@ -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 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;