refactor(ekf localizer): apply static analysis (#7324)

* rename to snake_case

Signed-off-by: Yamato Ando <[email protected]>

* add nodiscard

Signed-off-by: Yamato Ando <[email protected]>

* initilize variables

Signed-off-by: Yamato Ando <[email protected]>

* fix math.h to cmath

Signed-off-by: Yamato Ando <[email protected]>

* fix data type

Signed-off-by: Yamato Ando <[email protected]>

* remove redundant return statement

Signed-off-by: Yamato Ando <[email protected]>

* add &, remove t_curr

Signed-off-by: Yamato Ando <[email protected]>

* fix string empty

Signed-off-by: Yamato Ando <[email protected]>

* fix data type

Signed-off-by: Yamato Ando <[email protected]>

* remove unused variable

Signed-off-by: Yamato Ando <[email protected]>

* readability-else-after-return

Signed-off-by: Yamato Ando <[email protected]>

* add nolint

Signed-off-by: Yamato Ando <[email protected]>

* rename varibles

Signed-off-by: Yamato Ando <[email protected]>

* fix cast

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Yamato Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: SakodaShintaro <[email protected]>

localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp

@@ -22,11 +22,11 @@ template <typename Object>
 class AgedObjectQueue
 {
 public:
-  explicit AgedObjectQueue(const int max_age) : max_age_(max_age) {}
+  explicit AgedObjectQueue(const size_t max_age) : max_age_(max_age) {}
 
-  bool empty() const { return this->size() == 0; }
+  [[nodiscard]] bool empty() const { return this->size() == 0; }
 
-  size_t size() const { return objects_.size(); }
+  [[nodiscard]] size_t size() const { return objects_.size(); }
 
   Object back() const { return objects_.back(); }
 
@@ -39,7 +39,7 @@ class AgedObjectQueue
   Object pop_increment_age()
   {
     const Object object = objects_.front();
-    const int age = ages_.front() + 1;
+    const size_t age = ages_.front() + 1;
     objects_.pop();
     ages_.pop();
 
@@ -54,13 +54,13 @@ class AgedObjectQueue
   void clear()
   {
     objects_ = std::queue<Object>();
-    ages_ = std::queue<int>();
+    ages_ = std::queue<size_t>();
   }
 
 private:
-  const int max_age_;
+  const size_t max_age_;
   std::queue<Object> objects_;
-  std::queue<int> ages_;
+  std::queue<size_t> ages_;
 };
 
 #endif  // EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_

localization/ekf_localizer/include/ekf_localizer/covariance.hpp

@@ -17,7 +17,7 @@
 
 #include "ekf_localizer/matrix_types.hpp"
 
-std::array<double, 36> ekfCovarianceToPoseMessageCovariance(const Matrix6d & P);
-std::array<double, 36> ekfCovarianceToTwistMessageCovariance(const Matrix6d & P);
+std::array<double, 36> ekf_covariance_to_pose_message_covariance(const Matrix6d & P);
+std::array<double, 36> ekf_covariance_to_twist_message_covariance(const Matrix6d & P);
 
 #endif  // EKF_LOCALIZER__COVARIANCE_HPP_

localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp

@@ -20,21 +20,21 @@
 #include <string>
 #include <vector>
 
-diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated);
+diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated);
 
-diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated(
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated(
   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(
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size(
   const std::string & measurement_type, const size_t queue_size);
-diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate(
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate(
   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(
+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 mergeDiagnosticStatus(
+diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status(
   const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array);
 
 #endif  // EKF_LOCALIZER__DIAGNOSTICS_HPP_

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -58,17 +58,15 @@ class Simple1DFilter
     x_ = 0;
     dev_ = 1e9;
     proc_dev_x_c_ = 0.0;
-    return;
   };
-  void init(const double init_obs, const double obs_dev, const rclcpp::Time time)
+  void init(const double init_obs, const double obs_dev, const rclcpp::Time & time)
   {
     x_ = init_obs;
     dev_ = obs_dev;
     latest_time_ = time;
     initialized_ = true;
-    return;
   };
-  void update(const double obs, const double obs_dev, const rclcpp::Time time)
+  void update(const double obs, const double obs_dev, const rclcpp::Time & time)
   {
     if (!initialized_) {
       init(obs, obs_dev, time);
@@ -86,10 +84,9 @@ class Simple1DFilter
     dev_ = (1 - kalman_gain) * dev_;
 
     latest_time_ = time;
-    return;
   };
   void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; }
-  double get_x() const { return x_; }
+  [[nodiscard]] double get_x() const { return x_; }
 
 private:
   bool initialized_;
@@ -158,10 +155,9 @@ class EKFLocalizer : public rclcpp::Node
   double ekf_dt_;
 
   /* process noise variance for discrete model */
-  double proc_cov_yaw_d_;       //!< @brief  discrete yaw process noise
-  double proc_cov_yaw_bias_d_;  //!< @brief  discrete yaw bias process noise
-  double proc_cov_vx_d_;        //!< @brief  discrete process noise in d_vx=0
-  double proc_cov_wz_d_;        //!< @brief  discrete process noise in d_wz=0
+  double proc_cov_yaw_d_;  //!< @brief  discrete yaw process noise
+  double proc_cov_vx_d_;   //!< @brief  discrete process noise in d_vx=0
+  double proc_cov_wz_d_;   //!< @brief  discrete process noise in d_wz=0
 
   bool is_activated_;
 
@@ -174,68 +170,69 @@ class EKFLocalizer : public rclcpp::Node
   /**
    * @brief computes update & prediction of EKF for each ekf_dt_[s] time
    */
-  void timerCallback();
+  void timer_callback();
 
   /**
    * @brief publish tf for tf_rate [Hz]
    */
-  void timerTFCallback();
+  void timer_tf_callback();
 
   /**
-   * @brief set poseWithCovariance measurement
+   * @brief set pose with covariance measurement
    */
-  void callbackPoseWithCovariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+  void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 
   /**
-   * @brief set twistWithCovariance measurement
+   * @brief set twist with covariance measurement
    */
-  void callbackTwistWithCovariance(geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+  void callback_twist_with_covariance(
+    geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
 
   /**
    * @brief set initial_pose to current EKF pose
    */
-  void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+  void callback_initial_pose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 
   /**
    * @brief update predict frequency
    */
-  void updatePredictFrequency(const rclcpp::Time & current_time);
+  void update_predict_frequency(const rclcpp::Time & current_time);
 
   /**
    * @brief get transform from frame_id
    */
-  bool getTransformFromTF(
+  bool get_transform_from_tf(
     std::string parent_frame, std::string child_frame,
     geometry_msgs::msg::TransformStamped & transform);
 
   /**
    * @brief publish current EKF estimation result
    */
-  void publishEstimateResult(
+  void publish_estimate_result(
     const geometry_msgs::msg::PoseStamped & current_ekf_pose,
     const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose,
     const geometry_msgs::msg::TwistStamped & current_ekf_twist);
 
   /**
    * @brief publish diagnostics message
    */
-  void publishDiagnostics(const rclcpp::Time & current_time);
+  void publish_diagnostics(const rclcpp::Time & current_time);
 
   /**
-   * @brief update simple1DFilter
+   * @brief update simple 1d filter
    */
-  void updateSimple1DFilters(
+  void update_simple_1d_filters(
     const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step);
 
   /**
-   * @brief initialize simple1DFilter
+   * @brief initialize simple 1d filter
    */
-  void initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
+  void init_simple_1d_filters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
 
   /**
    * @brief trigger node
    */
-  void serviceTriggerNode(
+  void service_trigger_node(
     const std_srvs::srv::SetBool::Request::SharedPtr req,
     std_srvs::srv::SetBool::Response::SharedPtr res);
 

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp

@@ -34,24 +34,13 @@
 
 struct EKFDiagnosticInfo
 {
-  EKFDiagnosticInfo()
-  : no_update_count(0),
-    queue_size(0),
-    is_passed_delay_gate(true),
-    delay_time(0),
-    delay_time_threshold(0),
-    is_passed_mahalanobis_gate(true),
-    mahalanobis_distance(0)
-  {
-  }
-
-  size_t no_update_count;
-  size_t queue_size;
-  bool is_passed_delay_gate;
-  double delay_time;
-  double delay_time_threshold;
-  bool is_passed_mahalanobis_gate;
-  double mahalanobis_distance;
+  size_t no_update_count{0};
+  size_t queue_size{0};
+  bool is_passed_delay_gate{true};
+  double delay_time{0.0};
+  double delay_time_threshold{0.0};
+  bool is_passed_mahalanobis_gate{true};
+  double mahalanobis_distance{0.0};
 };
 
 class EKFModule
@@ -63,31 +52,32 @@ class EKFModule
   using Twist = geometry_msgs::msg::TwistStamped;
 
 public:
-  EKFModule(std::shared_ptr<Warning> warning, const HyperParameters params);
+  EKFModule(std::shared_ptr<Warning> warning, const HyperParameters & params);
 
   void initialize(
     const PoseWithCovariance & initial_pose,
     const geometry_msgs::msg::TransformStamped & transform);
 
-  geometry_msgs::msg::PoseStamped getCurrentPose(
+  [[nodiscard]] geometry_msgs::msg::PoseStamped get_current_pose(
     const rclcpp::Time & current_time, const double z, const double roll, const double pitch,
     bool get_biased_yaw) const;
-  geometry_msgs::msg::TwistStamped getCurrentTwist(const rclcpp::Time & current_time) const;
-  double getYawBias() const;
-  std::array<double, 36> getCurrentPoseCovariance() const;
-  std::array<double, 36> getCurrentTwistCovariance() const;
+  [[nodiscard]] geometry_msgs::msg::TwistStamped get_current_twist(
+    const rclcpp::Time & current_time) const;
+  [[nodiscard]] double get_yaw_bias() const;
+  [[nodiscard]] std::array<double, 36> get_current_pose_covariance() const;
+  [[nodiscard]] std::array<double, 36> get_current_twist_covariance() const;
 
-  size_t find_closest_delay_time_index(double target_value) const;
+  [[nodiscard]] size_t find_closest_delay_time_index(double target_value) const;
   void accumulate_delay_time(const double dt);
 
-  void predictWithDelay(const double dt);
-  bool measurementUpdatePose(
+  void predict_with_delay(const double dt);
+  bool measurement_update_pose(
     const PoseWithCovariance & pose, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & pose_diag_info);
-  bool measurementUpdateTwist(
+  bool measurement_update_twist(
     const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & twist_diag_info);
-  geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay(
+  geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay(
     const PoseWithCovariance & pose, const double delay_time);
 
 private:

localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp

@@ -68,14 +68,14 @@ class HyperParameters
   const double tf_rate_;
   const bool publish_tf_;
   const bool enable_yaw_bias_estimation;
-  const int extend_state_step;
+  const size_t extend_state_step;
   const std::string pose_frame_id;
   const double pose_additional_delay;
   const double pose_gate_dist;
-  const int pose_smoothing_steps;
+  const size_t pose_smoothing_steps;
   const double twist_additional_delay;
   const double twist_gate_dist;
-  const int twist_smoothing_steps;
+  const size_t twist_smoothing_steps;
   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

localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp

@@ -18,7 +18,7 @@
 #include <Eigen/Core>
 #include <Eigen/Dense>
 
-double squaredMahalanobis(
+double squared_mahalanobis(
   const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C);
 
 double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C);

localization/ekf_localizer/include/ekf_localizer/measurement.hpp

@@ -17,11 +17,11 @@
 
 #include <Eigen/Core>
 
-Eigen::Matrix<double, 3, 6> poseMeasurementMatrix();
-Eigen::Matrix<double, 2, 6> twistMeasurementMatrix();
-Eigen::Matrix3d poseMeasurementCovariance(
+Eigen::Matrix<double, 3, 6> pose_measurement_matrix();
+Eigen::Matrix<double, 2, 6> twist_measurement_matrix();
+Eigen::Matrix3d pose_measurement_covariance(
   const std::array<double, 36ul> & covariance, const size_t smoothing_step);
-Eigen::Matrix2d twistMeasurementCovariance(
+Eigen::Matrix2d twist_measurement_covariance(
   const std::array<double, 36ul> & covariance, const size_t smoothing_step);
 
 #endif  // EKF_LOCALIZER__MEASUREMENT_HPP_

localization/ekf_localizer/include/ekf_localizer/numeric.hpp

@@ -19,12 +19,12 @@
 
 #include <cmath>
 
-inline bool hasInf(const Eigen::MatrixXd & v)
+inline bool has_inf(const Eigen::MatrixXd & v)
 {
   return v.array().isInf().any();
 }
 
-inline bool hasNan(const Eigen::MatrixXd & v)
+inline bool has_nan(const Eigen::MatrixXd & v)
 {
   return v.array().isNaN().any();
 }

localization/ekf_localizer/include/ekf_localizer/state_transition.hpp

@@ -17,10 +17,10 @@
 
 #include "ekf_localizer/matrix_types.hpp"
 
-double normalizeYaw(const double & yaw);
-Vector6d predictNextState(const Vector6d & X_curr, const double dt);
-Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt);
-Matrix6d processNoiseCovariance(
+double normalize_yaw(const double & yaw);
+Vector6d predict_next_state(const Vector6d & X_curr, const double dt);
+Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt);
+Matrix6d process_noise_covariance(
   const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d);
 
 #endif  // EKF_LOCALIZER__STATE_TRANSITION_HPP_

localization/ekf_localizer/include/ekf_localizer/string.hpp

@@ -17,7 +17,7 @@
 
 #include <string>
 
-inline std::string eraseLeadingSlash(const std::string & s)
+inline std::string erase_leading_slash(const std::string & s)
 {
   std::string a = s;
   if (a.front() == '/') {

localization/ekf_localizer/include/ekf_localizer/warning.hpp

@@ -29,7 +29,7 @@ class Warning
     RCLCPP_WARN(node_->get_logger(), "%s", message.c_str());
   }
 
-  void warnThrottle(const std::string & message, const int duration_milliseconds) const
+  void warn_throttle(const std::string & message, const int duration_milliseconds) const
   {
     RCLCPP_WARN_THROTTLE(
       node_->get_logger(), *(node_->get_clock()),

localization/ekf_localizer/include/ekf_localizer/warning_message.hpp

@@ -17,11 +17,12 @@
 
 #include <string>
 
-std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold);
-std::string twistDelayStepWarningMessage(
+std::string pose_delay_step_warning_message(
   const double delay_time, const double delay_time_threshold);
-std::string poseDelayTimeWarningMessage(const double delay_time);
-std::string twistDelayTimeWarningMessage(const double delay_time);
-std::string mahalanobisWarningMessage(const double distance, const double max_distance);
+std::string twist_delay_step_warning_message(
+  const double delay_time, const double delay_time_threshold);
+std::string pose_delay_time_warning_message(const double delay_time);
+std::string twist_delay_time_warning_message(const double delay_time);
+std::string mahalanobis_warning_message(const double distance, const double max_distance);
 
 #endif  // EKF_LOCALIZER__WARNING_MESSAGE_HPP_