From 67ae6f9a64c1650b5750d5c33ca8dc4261509740 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 7 Sep 2023 09:03:21 +0000 Subject: [PATCH] style(pre-commit): autofix --- localization/ekf_localizer/README.md | 29 ++++----- .../include/ekf_localizer/diagnostics.hpp | 26 ++++++-- .../include/ekf_localizer/ekf_localizer.hpp | 14 ++-- .../ekf_localizer/hyper_parameters.hpp | 12 ++-- localization/ekf_localizer/package.xml | 4 +- .../ekf_localizer/src/diagnostics.cpp | 65 +++++++++++-------- .../ekf_localizer/src/ekf_localizer.cpp | 53 +++++++++++---- .../ekf_localizer/test/test_diagnostics.cpp | 64 +++++++++++------- 8 files changed, 168 insertions(+), 99 deletions(-) diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 97a640c57f009..00b24fca10936 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -152,17 +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 | -| :------------------------------------ | :----- | :------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| diagnostics_update_rate | double | Frequency for diagnostics broadcasting [Hz] | 10.0 | -| 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 | - +| Name | Type | Description | Default value | +| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| diagnostics_update_rate | double | Frequency for diagnostics broadcasting [Hz] | 10.0 | +| 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 @@ -216,15 +214,16 @@ Note that, although the dimension gets larger since the analytical expansion can

- ### 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 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`. + +- 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 diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp index a343dfd31d4d1..4bd332a923b56 100644 --- a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp @@ -15,15 +15,27 @@ #ifndef EKF_LOCALIZER__DIAGNOSTICS_HPP_ #define EKF_LOCALIZER__DIAGNOSTICS_HPP_ -#include - #include -void checkProcessActivated(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool* const is_activated_ptr); +#include + +void checkProcessActivated( + diagnostic_updater::DiagnosticStatusWrapper & stat, const bool * const is_activated_ptr); -void checkMeasurementUpdated(diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, const size_t* const no_update_count_ptr, const size_t* const no_update_count_threshold_warn_ptr, const size_t* const no_update_count_threshold_error_ptr); -void checkMeasurementQueueSize(diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, const size_t* const queue_size_ptr); -void checkMeasurementDelayGate(diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, const bool* const is_passed_delay_gate_ptr, const double* const delay_time_ptr, const double* const delay_time_threshold_ptr); -void checkMeasurementMahalanobisGate(diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, const bool* const is_passed_mahalabobis_gate_ptr, const double* const mahalabobis_distance_ptr, const double* const mahalabobis_distance_threshold_ptr); +void checkMeasurementUpdated( + diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, + const size_t * const no_update_count_ptr, const size_t * const no_update_count_threshold_warn_ptr, + const size_t * const no_update_count_threshold_error_ptr); +void checkMeasurementQueueSize( + diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, + const size_t * const queue_size_ptr); +void checkMeasurementDelayGate( + diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, + const bool * const is_passed_delay_gate_ptr, const double * const delay_time_ptr, + const double * const delay_time_threshold_ptr); +void checkMeasurementMahalanobisGate( + diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, + const bool * const is_passed_mahalabobis_gate_ptr, const double * const mahalabobis_distance_ptr, + const double * const mahalabobis_distance_threshold_ptr); #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 e08be83ae7690..109347ccf46cb 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -19,12 +19,14 @@ #include "ekf_localizer/hyper_parameters.hpp" #include "ekf_localizer/warning.hpp" +#include #include #include #include #include #include +#include #include #include #include @@ -41,9 +43,6 @@ #include #include -#include -#include - #include #include #include @@ -186,21 +185,20 @@ class EKFLocalizer : public rclcpp::Node size_t pose_no_update_count_; size_t pose_queue_size_; - bool pose_is_passed_delay_gate_; + bool pose_is_passed_delay_gate_; double pose_delay_time_; double pose_delay_time_threshold_; - bool pose_is_passed_mahalabobis_gate_; + bool pose_is_passed_mahalabobis_gate_; double pose_mahalabobis_distance_; size_t twist_no_update_count_; size_t twist_queue_size_; - bool twist_is_passed_delay_gate_; + bool twist_is_passed_delay_gate_; double twist_delay_time_; double twist_delay_time_threshold_; - bool twist_is_passed_mahalabobis_gate_; + bool twist_is_passed_mahalabobis_gate_; double twist_mahalabobis_distance_; - AgedObjectQueue pose_queue_; AgedObjectQueue twist_queue_; diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 6193bc77e6563..019a06d1c59b0 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -41,10 +41,14 @@ class HyperParameters 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)), diagnostics_update_rate(node->declare_parameter("diagnostics_update_rate", 10)), - 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)) + 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)) { } diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index b44380707df40..84ac8e45939ed 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -18,6 +18,8 @@ eigen + diagnostic_msgs + diagnostic_updater fmt geometry_msgs kalman_filter @@ -30,8 +32,6 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - diagnostic_msgs - diagnostic_updater ament_cmake_ros ament_lint_auto diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp index c5507ec115cb6..a167c7ce72991 100644 --- a/localization/ekf_localizer/src/diagnostics.cpp +++ b/localization/ekf_localizer/src/diagnostics.cpp @@ -14,12 +14,12 @@ #include "ekf_localizer/diagnostics.hpp" -#include - #include +#include + void checkProcessActivated( - diagnostic_updater::DiagnosticStatusWrapper & stat, const bool* const is_activated_ptr) + diagnostic_updater::DiagnosticStatusWrapper & stat, const bool * const is_activated_ptr) { if (is_activated_ptr == nullptr) { return; @@ -38,23 +38,27 @@ void checkProcessActivated( } void checkMeasurementUpdated( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, const size_t* const no_update_count_ptr, const size_t* const no_update_count_threshold_warn_ptr, const size_t* const no_update_count_threshold_error_ptr) + diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, + const size_t * const no_update_count_ptr, const size_t * const no_update_count_threshold_warn_ptr, + const size_t * const no_update_count_threshold_error_ptr) { - if (no_update_count_ptr == nullptr || - no_update_count_threshold_warn_ptr == nullptr || - no_update_count_threshold_error_ptr == nullptr) { + if ( + no_update_count_ptr == nullptr || no_update_count_threshold_warn_ptr == nullptr || + no_update_count_threshold_error_ptr == nullptr) { return; } - stat.add( measurement_type + "_no_update_count", *no_update_count_ptr); - stat.add( measurement_type + "_no_update_count_threshold_warn", *no_update_count_threshold_warn_ptr); - stat.add( measurement_type + "_no_update_count_threshold_error", *no_update_count_threshold_error_ptr); + stat.add(measurement_type + "_no_update_count", *no_update_count_ptr); + stat.add( + measurement_type + "_no_update_count_threshold_warn", *no_update_count_threshold_warn_ptr); + stat.add( + measurement_type + "_no_update_count_threshold_error", *no_update_count_threshold_error_ptr); int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string diag_message; if (*no_update_count_ptr >= *no_update_count_threshold_warn_ptr) { diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_message = "[WARN]" + measurement_type + "_queue is empty"; + diag_message = "[WARN]" + measurement_type + "_queue is empty"; } if (*no_update_count_ptr >= *no_update_count_threshold_error_ptr) { diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -64,15 +68,15 @@ void checkMeasurementUpdated( stat.summary(diag_level, diag_message); } - void checkMeasurementQueueSize( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, const size_t* const queue_size_ptr) + diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, + const size_t * const queue_size_ptr) { if (queue_size_ptr == nullptr) { return; } - stat.add( measurement_type + "_queue_size", *queue_size_ptr); + stat.add(measurement_type + "_queue_size", *queue_size_ptr); int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string diag_message; @@ -81,17 +85,19 @@ void checkMeasurementQueueSize( } void checkMeasurementDelayGate( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, const bool* const is_passed_delay_gate_ptr, const double* const delay_time_ptr, const double* const delay_time_threshold_ptr) + diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, + const bool * const is_passed_delay_gate_ptr, const double * const delay_time_ptr, + const double * const delay_time_threshold_ptr) { - if (is_passed_delay_gate_ptr == nullptr || - delay_time_ptr == nullptr || - delay_time_threshold_ptr == nullptr) { + if ( + is_passed_delay_gate_ptr == nullptr || delay_time_ptr == nullptr || + delay_time_threshold_ptr == nullptr) { return; } - stat.add( measurement_type + "_is_passed_delay_gate", *is_passed_delay_gate_ptr); - stat.add( measurement_type + "_delay_time", *delay_time_ptr); - stat.add( measurement_type + "_delay_time_threshold", *delay_time_threshold_ptr); + stat.add(measurement_type + "_is_passed_delay_gate", *is_passed_delay_gate_ptr); + stat.add(measurement_type + "_delay_time", *delay_time_ptr); + stat.add(measurement_type + "_delay_time_threshold", *delay_time_threshold_ptr); int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string diag_message; @@ -104,17 +110,20 @@ void checkMeasurementDelayGate( } void checkMeasurementMahalanobisGate( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, const bool* const is_passed_mahalabobis_gate_ptr, const double* const mahalabobis_distance_ptr, const double* const mahalabobis_distance_threshold_ptr) + diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & measurement_type, + const bool * const is_passed_mahalabobis_gate_ptr, const double * const mahalabobis_distance_ptr, + const double * const mahalabobis_distance_threshold_ptr) { - if ( is_passed_mahalabobis_gate_ptr == nullptr || - mahalabobis_distance_ptr == nullptr || - mahalabobis_distance_threshold_ptr == nullptr) { + if ( + is_passed_mahalabobis_gate_ptr == nullptr || mahalabobis_distance_ptr == nullptr || + mahalabobis_distance_threshold_ptr == nullptr) { return; } - stat.add( measurement_type + "_is_passed_mahalabobis_gate", *is_passed_mahalabobis_gate_ptr); - stat.add( measurement_type + "_mahalabobis_distance", *mahalabobis_distance_ptr); - stat.add( measurement_type + "_mahalabobis_distance_threshold", *mahalabobis_distance_threshold_ptr); + stat.add(measurement_type + "_is_passed_mahalabobis_gate", *is_passed_mahalabobis_gate_ptr); + stat.add(measurement_type + "_mahalabobis_distance", *mahalabobis_distance_ptr); + stat.add( + measurement_type + "_mahalabobis_distance_threshold", *mahalabobis_distance_threshold_ptr); int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string diag_message; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 06ec05c4b8cdb..bc56e0cf61795 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -103,17 +103,47 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti tf_br_ = std::make_shared( std::shared_ptr(this, [](auto) {})); - diag_process_activated_.reset(new diagnostic_updater::FunctionDiagnosticTask("process activated", std::bind(&checkProcessActivated, std::placeholders::_1, &is_activated_))); - diag_pose_updated_.reset(new diagnostic_updater::FunctionDiagnosticTask("pose updated", std::bind(&checkMeasurementUpdated, std::placeholders::_1, "pose", &pose_no_update_count_, &(params_.pose_no_update_count_threshold_warn), &(params_.pose_no_update_count_threshold_error)))); - diag_pose_queue_size_.reset(new diagnostic_updater::FunctionDiagnosticTask("pose queue size", std::bind(&checkMeasurementQueueSize, std::placeholders::_1, "pose", &pose_queue_size_))); - diag_pose_delay_gate_.reset(new diagnostic_updater::FunctionDiagnosticTask("pose delay gate", std::bind(&checkMeasurementDelayGate, std::placeholders::_1, "pose", &pose_is_passed_delay_gate_, &pose_delay_time_, &pose_delay_time_threshold_))); - diag_pose_mahalanobis_gate_.reset(new diagnostic_updater::FunctionDiagnosticTask("pose mahalanobis gate", std::bind(&checkMeasurementMahalanobisGate, std::placeholders::_1, "pose", &pose_is_passed_mahalabobis_gate_, &pose_mahalabobis_distance_, &(params_.pose_gate_dist)))); - diag_twist_updated_.reset(new diagnostic_updater::FunctionDiagnosticTask("twist updated", std::bind(&checkMeasurementUpdated, std::placeholders::_1, "twist", &twist_no_update_count_, &(params_.twist_no_update_count_threshold_warn), &(params_.twist_no_update_count_threshold_error)))); - diag_twist_queue_size_.reset(new diagnostic_updater::FunctionDiagnosticTask("twist queue size", std::bind(&checkMeasurementQueueSize, std::placeholders::_1, "twist", &twist_queue_size_))); - diag_twist_delay_gate_.reset(new diagnostic_updater::FunctionDiagnosticTask("twist delay gate", std::bind(&checkMeasurementDelayGate, std::placeholders::_1, "twist", &twist_is_passed_delay_gate_, &twist_delay_time_, &twist_delay_time_threshold_))); - diag_twist_mahalanobis_gate_.reset(new diagnostic_updater::FunctionDiagnosticTask("twist mahalanobis gate", std::bind(&checkMeasurementMahalanobisGate, std::placeholders::_1, "twist", &twist_is_passed_mahalabobis_gate_, &twist_mahalabobis_distance_, &(params_.twist_gate_dist)))); - - diag_composite_task_.reset(new diagnostic_updater::CompositeDiagnosticTask("ekf_localizer_status")); + diag_process_activated_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "process activated", std::bind(&checkProcessActivated, std::placeholders::_1, &is_activated_))); + diag_pose_updated_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "pose updated", std::bind( + &checkMeasurementUpdated, std::placeholders::_1, "pose", + &pose_no_update_count_, &(params_.pose_no_update_count_threshold_warn), + &(params_.pose_no_update_count_threshold_error)))); + diag_pose_queue_size_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "pose queue size", + std::bind(&checkMeasurementQueueSize, std::placeholders::_1, "pose", &pose_queue_size_))); + diag_pose_delay_gate_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "pose delay gate", + std::bind( + &checkMeasurementDelayGate, std::placeholders::_1, "pose", &pose_is_passed_delay_gate_, + &pose_delay_time_, &pose_delay_time_threshold_))); + diag_pose_mahalanobis_gate_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "pose mahalanobis gate", + std::bind( + &checkMeasurementMahalanobisGate, std::placeholders::_1, "pose", + &pose_is_passed_mahalabobis_gate_, &pose_mahalabobis_distance_, &(params_.pose_gate_dist)))); + diag_twist_updated_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "twist updated", std::bind( + &checkMeasurementUpdated, std::placeholders::_1, "twist", + &twist_no_update_count_, &(params_.twist_no_update_count_threshold_warn), + &(params_.twist_no_update_count_threshold_error)))); + diag_twist_queue_size_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "twist queue size", + std::bind(&checkMeasurementQueueSize, std::placeholders::_1, "twist", &twist_queue_size_))); + diag_twist_delay_gate_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "twist delay gate", + std::bind( + &checkMeasurementDelayGate, std::placeholders::_1, "twist", &twist_is_passed_delay_gate_, + &twist_delay_time_, &twist_delay_time_threshold_))); + diag_twist_mahalanobis_gate_.reset(new diagnostic_updater::FunctionDiagnosticTask( + "twist mahalanobis gate", std::bind( + &checkMeasurementMahalanobisGate, std::placeholders::_1, "twist", + &twist_is_passed_mahalabobis_gate_, &twist_mahalabobis_distance_, + &(params_.twist_gate_dist)))); + + diag_composite_task_.reset( + new diagnostic_updater::CompositeDiagnosticTask("ekf_localizer_status")); diag_composite_task_->addTask(diag_process_activated_.get()); diag_composite_task_->addTask(diag_pose_updated_.get()); diag_composite_task_->addTask(diag_pose_queue_size_.get()); @@ -140,7 +170,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti pub_measured_pose_ = create_publisher("debug/measured_pose", 1); } - /* * updatePredictFrequency */ diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp index 3890f7ad90c25..c1990951b26fe 100644 --- a/localization/ekf_localizer/test/test_diagnostics.cpp +++ b/localization/ekf_localizer/test/test_diagnostics.cpp @@ -19,7 +19,7 @@ TEST(TestEkfDiagnostics, CheckProcessActivated) { diagnostic_updater::DiagnosticStatusWrapper stat; - + bool is_activated = true; checkProcessActivated(stat, &is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); @@ -32,41 +32,53 @@ TEST(TestEkfDiagnostics, CheckProcessActivated) TEST(TestEkfDiagnostics, checkMeasurementUpdated) { diagnostic_updater::DiagnosticStatusWrapper stat; - - const std::string measurement_type = "pose"; // not effect for stat.level + + 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; - checkMeasurementUpdated(stat, measurement_type, &no_update_count, &no_update_count_threshold_warn, &no_update_count_threshold_error); + checkMeasurementUpdated( + stat, 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; - checkMeasurementUpdated(stat, measurement_type, &no_update_count, &no_update_count_threshold_warn, &no_update_count_threshold_error); + checkMeasurementUpdated( + stat, 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; - checkMeasurementUpdated(stat, measurement_type, &no_update_count, &no_update_count_threshold_warn, &no_update_count_threshold_error); + checkMeasurementUpdated( + stat, 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; - checkMeasurementUpdated(stat, measurement_type, &no_update_count, &no_update_count_threshold_warn, &no_update_count_threshold_error); + checkMeasurementUpdated( + stat, 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; - checkMeasurementUpdated(stat, measurement_type, &no_update_count, &no_update_count_threshold_warn, &no_update_count_threshold_error); + checkMeasurementUpdated( + stat, 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; - checkMeasurementUpdated(stat, measurement_type, &no_update_count, &no_update_count_threshold_warn, &no_update_count_threshold_error); + checkMeasurementUpdated( + stat, 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_updater::DiagnosticStatusWrapper stat; - - const std::string measurement_type = "pose"; // not effect for stat.level + + const std::string measurement_type = "pose"; // not effect for stat.level size_t queue_size_ptr = 0; checkMeasurementQueueSize(stat, measurement_type, &queue_size_ptr); @@ -80,33 +92,39 @@ TEST(TestEkfDiagnostics, CheckMeasurementQueueSize) TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) { diagnostic_updater::DiagnosticStatusWrapper 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 + + 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; - checkMeasurementDelayGate(stat, measurement_type, &is_passed_delay_gate, &delay_time, &delay_time_threshold); + checkMeasurementDelayGate( + stat, 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; - checkMeasurementDelayGate(stat, measurement_type, &is_passed_delay_gate, &delay_time, &delay_time_threshold); + checkMeasurementDelayGate( + stat, measurement_type, &is_passed_delay_gate, &delay_time, &delay_time_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) { diagnostic_updater::DiagnosticStatusWrapper stat; - - const std::string measurement_type = "pose"; // not effect for stat.level - const double mahalabobis_distance = 0.1; // not effect for stat.level - const double mahalabobis_distance_threshold = 1.0; // not effect for stat.level + + const std::string measurement_type = "pose"; // not effect for stat.level + const double mahalabobis_distance = 0.1; // not effect for stat.level + const double mahalabobis_distance_threshold = 1.0; // not effect for stat.level bool is_passed_mahalabobis_gate = true; - checkMeasurementMahalanobisGate(stat, measurement_type, &is_passed_mahalabobis_gate, &mahalabobis_distance, &mahalabobis_distance_threshold); + checkMeasurementMahalanobisGate( + stat, measurement_type, &is_passed_mahalabobis_gate, &mahalabobis_distance, + &mahalabobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_passed_mahalabobis_gate = false; - checkMeasurementMahalanobisGate(stat, measurement_type, &is_passed_mahalabobis_gate, &mahalabobis_distance, &mahalabobis_distance_threshold); + checkMeasurementMahalanobisGate( + stat, measurement_type, &is_passed_mahalabobis_gate, &mahalabobis_distance, + &mahalabobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); }