diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 6e7e194f7cf72..8e29bfed14a28 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/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 diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 748b5ee5becc0..6492f20331a66 100644 --- a/localization/ekf_localizer/README.md +++ b/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

+## Diagnostics + +

+ +

+ +### 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. diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 4d3f5b9643462..667217d2591dc 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/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] diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp new file mode 100644 index 0000000000000..f4dc6436f6a40 --- /dev/null +++ b/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 + +#include +#include + +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 & stat_array); + +#endif // EKF_LOCALIZER__DIAGNOSTICS_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index a4ae47b670897..4fc2305cc7adc 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -126,6 +127,8 @@ class EKFLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_biased_pose_cov_; + //!< @brief diagnostics publisher + rclcpp::Publisher::SharedPtr pub_diag_; //!< @brief initial pose subscriber rclcpp::Subscription::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 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 pose_queue_; AgedObjectQueue 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 */ diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index c74fa9be79525..01ef658cf445d 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/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_ diff --git a/localization/ekf_localizer/media/ekf_diagnostics.png b/localization/ekf_localizer/media/ekf_diagnostics.png new file mode 100644 index 0000000000000..2580d6d973290 Binary files /dev/null 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 5bc9c5e42712d..005bddd3eb22b 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -18,6 +18,7 @@ eigen + diagnostic_msgs fmt geometry_msgs kalman_filter diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp new file mode 100644 index 0000000000000..9ff36561abaa9 --- /dev/null +++ b/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 + +#include +#include + +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 & 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; +} diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 0e46a26add852..b39112b1d8d62 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -15,6 +15,7 @@ #include "ekf_localizer/ekf_localizer.hpp" #include "ekf_localizer/covariance.hpp" +#include "ekf_localizer/diagnostics.hpp" #include "ekf_localizer/mahalanobis.hpp" #include "ekf_localizer/matrix_types.hpp" #include "ekf_localizer/measurement.hpp" @@ -87,6 +88,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); pub_biased_pose_cov_ = create_publisher( "ekf_biased_pose_with_covariance", 1); + pub_diag_ = this->create_publisher("/diagnostics", 10); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -143,6 +145,7 @@ void EKFLocalizer::timerCallback() if (!is_activated_) { warning_.warnThrottle( "The node is not activated. Provide initial pose to pose_initializer", 2000); + publishDiagnostics(); return; } @@ -176,6 +179,16 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */ + + pose_queue_size_ = pose_queue_.size(); + pose_is_passed_delay_gate_ = true; + pose_delay_time_ = 0.0; + pose_delay_time_threshold_ = 0.0; + pose_is_passed_mahalanobis_gate_ = true; + pose_mahalanobis_distance_ = 0.0; + + bool pose_is_updated = false; + if (!pose_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); stop_watch_.tic(); @@ -184,13 +197,27 @@ void EKFLocalizer::timerCallback() const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - measurementUpdatePose(*pose); + bool is_updated = measurementUpdatePose(*pose); + if (is_updated) { + pose_is_updated = true; + } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } + pose_no_update_count_ = pose_is_updated ? 0 : (pose_no_update_count_ + 1); /* twist measurement update */ + + twist_queue_size_ = twist_queue_.size(); + twist_is_passed_delay_gate_ = true; + twist_delay_time_ = 0.0; + twist_delay_time_threshold_ = 0.0; + twist_is_passed_mahalanobis_gate_ = true; + twist_mahalanobis_distance_ = 0.0; + + bool twist_is_updated = false; + if (!twist_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------"); stop_watch_.tic(); @@ -199,11 +226,15 @@ void EKFLocalizer::timerCallback() const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - measurementUpdateTwist(*twist); + bool is_updated = measurementUpdateTwist(*twist); + if (is_updated) { + twist_is_updated = true; + } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } + twist_no_update_count_ = twist_is_updated ? 0 : (twist_no_update_count_ + 1); const double x = ekf_.getXelement(IDX::X); const double y = ekf_.getXelement(IDX::Y); @@ -235,6 +266,7 @@ void EKFLocalizer::timerCallback() /* publish ekf result */ publishEstimateResult(); + publishDiagnostics(); } void EKFLocalizer::showCurrentX() @@ -353,6 +385,11 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + // Ignore twist if velocity is too small. + // Note that this inequality must not include "equal". + if (std::abs(msg->twist.twist.linear.x) < params_.threshold_observable_velocity_mps) { + msg->twist.covariance[0 * 6 + 0] = 10000.0; + } twist_queue_.push(msg); } @@ -376,7 +413,7 @@ void EKFLocalizer::initEKF() /* * measurementUpdatePose */ -void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +bool EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { if (pose.header.frame_id != params_.pose_frame_id) { warning_.warnThrottle( @@ -400,10 +437,14 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); + + pose_delay_time_ = std::max(delay_time, pose_delay_time_); + pose_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; if (delay_step >= params_.extend_state_step) { + pose_is_passed_delay_gate_ = false; warning_.warnThrottle( poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); - return; + return false; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); @@ -420,7 +461,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar if (hasNan(y) || hasInf(y)) { warning_.warn( "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); - return; + return false; } /* Gate */ @@ -431,10 +472,12 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); const double distance = mahalanobis(y_ekf, y, P_y); + pose_mahalanobis_distance_ = std::max(distance, pose_mahalanobis_distance_); if (distance > params_.pose_gate_dist) { + pose_is_passed_mahalanobis_gate_ = false; warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); warning_.warnThrottle("Ignore the measurement data.", 2000); - return; + return false; } DEBUG_PRINT_MAT(y.transpose()); @@ -460,12 +503,14 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + + return true; } /* * measurementUpdateTwist */ -void EKFLocalizer::measurementUpdateTwist( +bool EKFLocalizer::measurementUpdateTwist( const geometry_msgs::msg::TwistWithCovarianceStamped & twist) { if (twist.header.frame_id != "base_link") { @@ -488,10 +533,14 @@ void EKFLocalizer::measurementUpdateTwist( delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); + + twist_delay_time_ = std::max(delay_time, twist_delay_time_); + twist_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; if (delay_step >= params_.extend_state_step) { + twist_is_passed_delay_gate_ = false; warning_.warnThrottle( twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); - return; + return false; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); @@ -502,7 +551,7 @@ void EKFLocalizer::measurementUpdateTwist( if (hasNan(y) || hasInf(y)) { warning_.warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); - return; + return false; } const Eigen::Vector2d y_ekf( @@ -512,10 +561,12 @@ void EKFLocalizer::measurementUpdateTwist( const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); const double distance = mahalanobis(y_ekf, y, P_y); + twist_mahalanobis_distance_ = std::max(distance, twist_mahalanobis_distance_); if (distance > params_.twist_gate_dist) { + twist_is_passed_mahalanobis_gate_ = false; warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); warning_.warnThrottle("Ignore the measurement data.", 2000); - return; + return false; } DEBUG_PRINT_MAT(y.transpose()); @@ -532,6 +583,8 @@ void EKFLocalizer::measurementUpdateTwist( const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + + return true; } /* @@ -607,6 +660,45 @@ void EKFLocalizer::publishEstimateResult() pub_debug_->publish(msg); } +void EKFLocalizer::publishDiagnostics() +{ + std::vector diag_status_array; + + diag_status_array.push_back(checkProcessActivated(is_activated_)); + + if (is_activated_) { + diag_status_array.push_back(checkMeasurementUpdated( + "pose", pose_no_update_count_, params_.pose_no_update_count_threshold_warn, + params_.pose_no_update_count_threshold_error)); + diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_queue_size_)); + diag_status_array.push_back(checkMeasurementDelayGate( + "pose", pose_is_passed_delay_gate_, pose_delay_time_, pose_delay_time_threshold_)); + diag_status_array.push_back(checkMeasurementMahalanobisGate( + "pose", pose_is_passed_mahalanobis_gate_, pose_mahalanobis_distance_, + params_.pose_gate_dist)); + + diag_status_array.push_back(checkMeasurementUpdated( + "twist", twist_no_update_count_, params_.twist_no_update_count_threshold_warn, + params_.twist_no_update_count_threshold_error)); + diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_queue_size_)); + diag_status_array.push_back(checkMeasurementDelayGate( + "twist", twist_is_passed_delay_gate_, twist_delay_time_, twist_delay_time_threshold_)); + diag_status_array.push_back(checkMeasurementMahalanobisGate( + "twist", twist_is_passed_mahalanobis_gate_, twist_mahalanobis_distance_, + params_.twist_gate_dist)); + } + + diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; + diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status.name = "localization: " + std::string(this->get_name()); + diag_merged_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_merged_status); + pub_diag_->publish(diag_msg); +} + void EKFLocalizer::updateSimple1DFilters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) { diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp new file mode 100644 index 0000000000000..f506dca1cb230 --- /dev/null +++ b/localization/ekf_localizer/test/test_diagnostics.cpp @@ -0,0 +1,192 @@ +// 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 + +TEST(TestEkfDiagnostics, CheckProcessActivated) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + bool is_activated = true; + stat = checkProcessActivated(is_activated); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_activated = false; + stat = checkProcessActivated(is_activated); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestEkfDiagnostics, checkMeasurementUpdated) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const size_t no_update_count_threshold_warn = 50; + const size_t no_update_count_threshold_error = 250; + + size_t no_update_count = 0; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 1; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 49; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 50; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + no_update_count = 249; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + no_update_count = 250; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestEkfDiagnostics, CheckMeasurementQueueSize) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + + size_t queue_size = 0; // not effect for stat.level + stat = checkMeasurementQueueSize(measurement_type, queue_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + queue_size = 1; // not effect for stat.level + stat = checkMeasurementQueueSize(measurement_type, queue_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const double delay_time = 0.1; // not effect for stat.level + const double delay_time_threshold = 1.0; // not effect for stat.level + + bool is_passed_delay_gate = true; + stat = checkMeasurementDelayGate( + measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_passed_delay_gate = false; + stat = checkMeasurementDelayGate( + measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const double mahalanobis_distance = 0.1; // not effect for stat.level + const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level + + bool is_passed_mahalanobis_gate = true; + stat = checkMeasurementMahalanobisGate( + measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, + mahalanobis_distance_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_passed_mahalanobis_gate = false; + stat = checkMeasurementMahalanobisGate( + measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, + mahalanobis_distance_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + std::vector stat_array(2); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + EXPECT_EQ(merged_stat.message, "OK"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(0).message = "ERROR0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); +} diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 2d1867b8cd0bc..97a0443195ae5 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -13,4 +13,5 @@ ament_target_dependencies(stop_filter) ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/localization/stop_filter/config/stop_filter.param.yaml b/localization/stop_filter/config/stop_filter.param.yaml new file mode 100644 index 0000000000000..ded090b75b5bd --- /dev/null +++ b/localization/stop_filter/config/stop_filter.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + vx_threshold: 0.1 # [m/s] + wz_threshold: 0.02 # [rad/s] diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 36a66a2c143c0..0ea92d26c9677 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -1,6 +1,5 @@ - - + @@ -10,7 +9,6 @@ - - + diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 111d460be737e..ac0960b8cb67b 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -27,8 +27,8 @@ using std::placeholders::_1; StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) : rclcpp::Node(node_name, node_options) { - vx_threshold_ = declare_parameter("vx_threshold", 0.01); - wz_threshold_ = declare_parameter("wz_threshold", 0.01); + vx_threshold_ = declare_parameter("vx_threshold"); + wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1));