refactor(ekf_localizer): isolate ekf into independent module (#5424)

* refactor(ekf_localizer): use struct for diag info Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * move diag_info to ekf_localizer.hpp Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove diag.hpp Signed-off-by: kminoda <koji.minoda@tier4.jp> * refactor(ekf_localizer): remote current_ekf_pose and so on Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix(ekf_localizer): remove unnecessary publishers Signed-off-by: kminoda <koji.minoda@tier4.jp> * still build failes due to one current_ekf_twist_ Signed-off-by: kminoda <koji.minoda@tier4.jp> * now works Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * refactor(ekf_localizer): isolate ekf into module Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * remove commentou Signed-off-by: kminoda <koji.minoda@tier4.jp> * minor fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove unnecessary headers Signed-off-by: kminoda <koji.minoda@tier4.jp> * update print logs Signed-off-by: kminoda <koji.minoda@tier4.jp> * include memory Signed-off-by: kminoda <koji.minoda@tier4.jp> * ekf_ to kalman_filter_ Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * remove duplicated struct declaration Signed-off-by: kminoda <koji.minoda@tier4.jp> * reflected comments Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Nov 6, 2023 · b73330b · b73330b
1 parent c7aa19e
commit b73330b
Show file tree

Hide file tree

Showing 5 changed files with 454 additions and 390 deletions.
diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt
@@ -21,6 +21,7 @@ ament_auto_add_library(ekf_localizer_lib SHARED
   src/measurement.cpp
   src/state_transition.cpp
   src/warning_message.cpp
+  src/ekf_module.cpp
 )
 
 target_link_libraries(ekf_localizer_lib Eigen3::Eigen)

diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -16,11 +16,10 @@
 #define EKF_LOCALIZER__EKF_LOCALIZER_HPP_
 
 #include "ekf_localizer/aged_object_queue.hpp"
+#include "ekf_localizer/ekf_module.hpp"
 #include "ekf_localizer/hyper_parameters.hpp"
 #include "ekf_localizer/warning.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/ros/logger_level_configure.hpp>
@@ -50,28 +49,6 @@
 #include <string>
 #include <vector>
 
-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;
-};
-
 class Simple1DFilter
 {
 public:
@@ -128,7 +105,7 @@ class EKFLocalizer : public rclcpp::Node
   EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options);
 
 private:
-  const Warning warning_;
+  const std::shared_ptr<Warning> warning_;
 
   //!< @brief ekf estimated pose publisher
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_;
@@ -171,7 +148,7 @@ class EKFLocalizer : public rclcpp::Node
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
   //!< @brief  extended kalman filter instance.
-  TimeDelayKalmanFilter ekf_;
+  std::unique_ptr<EKFModule> ekf_module_;
   Simple1DFilter z_filter_;
   Simple1DFilter roll_filter_;
   Simple1DFilter pitch_filter_;
@@ -181,10 +158,6 @@ class EKFLocalizer : public rclcpp::Node
   double ekf_rate_;
   double ekf_dt_;
 
-  /* parameters */
-
-  int dim_x_;  //!< @brief  dimension of EKF state
-
   /* 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
@@ -224,55 +197,18 @@ class EKFLocalizer : public rclcpp::Node
    */
   void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 
-  /**
-   * @brief initialization of EKF
-   */
-  void initEKF();
-
   /**
    * @brief update predict frequency
    */
   void updatePredictFrequency();
 
-  /**
-   * @brief compute EKF prediction
-   */
-  void predictKinematicsModel();
-
-  /**
-   * @brief compute EKF update with pose measurement
-   * @param pose measurement value
-   */
-  bool measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
-
-  /**
-   * @brief compute EKF update with pose measurement
-   * @param twist measurement value
-   */
-  bool measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist);
-
   /**
    * @brief get transform from frame_id
    */
   bool getTransformFromTF(
     std::string parent_frame, std::string child_frame,
     geometry_msgs::msg::TransformStamped & transform);
 
-  /**
-   * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_
-   */
-  void setCurrentResult();
-
-  /**
-   * @brief get current ekf pose
-   */
-  geometry_msgs::msg::PoseStamped getCurrentEKFPose(bool get_biased_yaw) const;
-
-  /**
-   * @brief get current ekf twist
-   */
-  geometry_msgs::msg::TwistStamped getCurrentEKFTwist() const;
-
   /**
    * @brief publish current EKF estimation result
    */
@@ -286,11 +222,6 @@ class EKFLocalizer : public rclcpp::Node
    */
   void publishDiagnostics();
 
-  /**
-   * @brief for debug
-   */
-  void showCurrentX();
-
   /**
    * @brief update simple1DFilter
    */

diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp
@@ -0,0 +1,97 @@
+// Copyright 2018-2019 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__EKF_MODULE_HPP_
+#define EKF_LOCALIZER__EKF_MODULE_HPP_
+
+#include "ekf_localizer/hyper_parameters.hpp"
+#include "ekf_localizer/state_index.hpp"
+#include "ekf_localizer/warning.hpp"
+
+#include <kalman_filter/kalman_filter.hpp>
+#include <kalman_filter/time_delay_kalman_filter.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+
+#include <memory>
+
+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;
+};
+
+class EKFModule
+{
+private:
+  using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using TwistWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped;
+  using Pose = geometry_msgs::msg::PoseStamped;
+  using Twist = geometry_msgs::msg::TwistStamped;
+
+public:
+  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(
+    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;
+
+  void predictWithDelay(const double dt);
+  bool measurementUpdatePose(
+    const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr,
+    EKFDiagnosticInfo & pose_diag_info);
+  bool measurementUpdateTwist(
+    const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr,
+    EKFDiagnosticInfo & twist_diag_info);
+  geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay(
+    const PoseWithCovariance & pose, const double delay_time);
+
+private:
+  TimeDelayKalmanFilter kalman_filter_;
+
+  std::shared_ptr<Warning> warning_;
+  const int dim_x_;
+  const HyperParameters params_;
+};
+
+#endif  // EKF_LOCALIZER__EKF_MODULE_HPP_