refactor(ekf_localizer): add a struct to load hyper parameters

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -15,6 +15,7 @@
 #ifndef EKF_LOCALIZER__EKF_LOCALIZER_HPP_
 #define EKF_LOCALIZER__EKF_LOCALIZER_HPP_
 
+#include "ekf_localizer/hyper_parameters.hpp"
 #include "ekf_localizer/warning.hpp"
 
 #include <kalman_filter/kalman_filter.hpp>
@@ -162,39 +163,15 @@ class EKFLocalizer : public rclcpp::Node
   Simple1DFilter roll_filter_;
   Simple1DFilter pitch_filter_;
 
+  const HyperParameters params_;
+
+  double ekf_rate_;
+  double ekf_dt_;
+
   /* parameters */
-  bool show_debug_info_;
-  double ekf_rate_;                  //!< @brief  EKF predict rate
-  double ekf_dt_;                    //!< @brief  = 1 / ekf_rate_
-  double tf_rate_;                   //!< @brief  tf publish rate
-  bool enable_yaw_bias_estimation_;  //!< @brief for LiDAR mount error.
-                                     //!< if true,publish /estimate_yaw_bias
-  std::string pose_frame_id_;
-
-  int dim_x_;              //!< @brief  dimension of EKF state
-  int extend_state_step_;  //!< @brief  for time delay compensation
-  int dim_x_ex_;  //!< @brief  dimension of extended EKF state (dim_x_ * extended_state_step)
 
-  /* Pose */
-  double pose_additional_delay_;          //!< @brief  compensated pose delay time =
-                                          //!< (pose.header.stamp - now) + additional_delay [s]
-  double pose_measure_uncertainty_time_;  //!< @brief  added for measurement covariance
-  double pose_rate_;  //!< @brief  pose rate [s], used for covariance calculation
-  //!< @brief  the mahalanobis distance threshold to ignore pose measurement
-  double pose_gate_dist_;
-
-  /* twist */
-  double twist_additional_delay_;  //!< @brief  compensated delay = (twist.header.stamp - now)
-                                   //!< + additional_delay [s]
-  double twist_rate_;              //!< @brief  rate [s], used for covariance calculation
-  //!< @brief  measurement is ignored if the mahalanobis distance is larger than this value.
-  double twist_gate_dist_;
-
-  /* process noise standard deviation */
-  double proc_stddev_yaw_c_;       //!< @brief  yaw process noise
-  double proc_stddev_yaw_bias_c_;  //!< @brief  yaw bias process noise
-  double proc_stddev_vx_c_;        //!< @brief  vx process noise
-  double proc_stddev_wz_c_;        //!< @brief  wz process noise
+  int dim_x_;     //!< @brief  dimension of EKF state
+  int dim_x_ex_;  //!< @brief  dimension of extended EKF state (dim_x_ * extended_state_step)
 
   /* process noise variance for discrete model */
   double proc_cov_yaw_d_;       //!< @brief  discrete yaw process noise
@@ -214,9 +191,6 @@ class EKFLocalizer : public rclcpp::Node
   std::array<double, 36ul> current_pose_covariance_;
   std::array<double, 36ul> current_twist_covariance_;
 
-  int pose_smoothing_steps_;
-  int twist_smoothing_steps_;
-
   /**
    * @brief computes update & prediction of EKF for each ekf_dt_[s] time
    */

localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp

@@ -0,0 +1,64 @@
+// Copyright 2022 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__HYPER_PARAMETERS_HPP_
+#define EKF_LOCALIZER__HYPER_PARAMETERS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <algorithm>
+#include <string>
+
+class HyperParameters
+{
+public:
+  explicit HyperParameters(rclcpp::Node * node)
+  : show_debug_info(node->declare_parameter("show_debug_info", false)),
+    ekf_rate(node->declare_parameter("predict_frequency", 50.0)),
+    ekf_dt(1.0 / std::max(ekf_rate, 0.1)),
+    tf_rate_(node->declare_parameter("tf_rate", 10.0)),
+    enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)),
+    extend_state_step(node->declare_parameter("extend_state_step", 50)),
+    pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))),
+    pose_additional_delay(node->declare_parameter("pose_additional_delay", 0.0)),
+    pose_gate_dist(node->declare_parameter("pose_gate_dist", 10000.0)),
+    pose_smoothing_steps(node->declare_parameter("pose_smoothing_steps", 5)),
+    twist_additional_delay(node->declare_parameter("twist_additional_delay", 0.0)),
+    twist_gate_dist(node->declare_parameter("twist_gate_dist", 10000.0)),
+    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))
+  {
+  }
+
+  const bool show_debug_info;
+  const double ekf_rate;
+  const double ekf_dt;
+  const double tf_rate_;
+  const bool enable_yaw_bias_estimation;
+  const int 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 double twist_additional_delay;
+  const double twist_gate_dist;
+  const int 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
+};
+
+#endif  // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_