style(pre-commit): autofix

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
 <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 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
 

localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp

@@ -15,15 +15,27 @@
 #ifndef EKF_LOCALIZER__DIAGNOSTICS_HPP_
 #define EKF_LOCALIZER__DIAGNOSTICS_HPP_
 
-#include <string>
-
 #include <diagnostic_updater/diagnostic_updater.hpp>
 
-void checkProcessActivated(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool* const is_activated_ptr);
+#include <string>
+
+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_

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 <diagnostic_updater/diagnostic_updater.hpp>
 #include <kalman_filter/kalman_filter.hpp>
 #include <kalman_filter/time_delay_kalman_filter.hpp>
 #include <rclcpp/rclcpp.hpp>
 #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>
@@ -41,9 +43,6 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
-#include <diagnostic_updater/diagnostic_updater.hpp>
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
-
 #include <chrono>
 #include <iostream>
 #include <memory>
@@ -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<geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr> pose_queue_;
   AgedObjectQueue<geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr> twist_queue_;
 

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))
   {
   }
 

localization/ekf_localizer/package.xml

@@ -18,6 +18,8 @@
 
   <build_depend>eigen</build_depend>
 
+  <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_updater</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>kalman_filter</depend>
@@ -30,8 +32,6 @@
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_debug_msgs</depend>
-  <depend>diagnostic_msgs</depend>
-  <depend>diagnostic_updater</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

localization/ekf_localizer/src/diagnostics.cpp

@@ -14,12 +14,12 @@
 
 #include "ekf_localizer/diagnostics.hpp"
 
-#include <string>
-
 #include <diagnostic_updater/diagnostic_updater.hpp>
 
+#include <string>
+
 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;