From cb2552b006c3b7163a173f3a1a996a2efadfe3ca Mon Sep 17 00:00:00 2001
From: Takeshi Ishita <ishitah.takeshi@gmail.com>
Date: Mon, 12 Sep 2022 11:10:07 +0900
Subject: [PATCH] fix(ekf_localizer): remove a setter function to improve
 refenential trasparency (#1822)

* fix(ekf_localizer): remove the setCurrentResult function to improve the referential transparency
---
 .../ekf_localizer/src/ekf_localizer.cpp       | 55 ++++++++++---------
 1 file changed, 28 insertions(+), 27 deletions(-)

diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
index d49ad5391fadf..4bca890af3bb2 100644
--- a/localization/ekf_localizer/src/ekf_localizer.cpp
+++ b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -197,46 +197,47 @@ void EKFLocalizer::timerCallback()
     DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n");
   }
 
-  /* set current pose, twist */
-  setCurrentResult();
+  const double x = ekf_.getXelement(IDX::X);
+  const double y = ekf_.getXelement(IDX::Y);
+  const double z = z_filter_.get_x();
 
-  /* publish ekf result */
-  publishEstimateResult();
-}
+  const double biased_yaw = ekf_.getXelement(IDX::YAW);
+  const double yaw_bias = ekf_.getXelement(IDX::YAWB);
 
-void EKFLocalizer::showCurrentX()
-{
-  if (show_debug_info_) {
-    Eigen::MatrixXd X(dim_x_, 1);
-    ekf_.getLatestX(X);
-    DEBUG_PRINT_MAT(X.transpose());
-  }
-}
+  const double roll = roll_filter_.get_x();
+  const double pitch = pitch_filter_.get_x();
+  const double yaw = biased_yaw + yaw_bias;
+  const double vx = ekf_.getXelement(IDX::VX);
+  const double wz = ekf_.getXelement(IDX::WZ);
 
-/*
- * setCurrentResult
- */
-void EKFLocalizer::setCurrentResult()
-{
   current_ekf_pose_.header.frame_id = pose_frame_id_;
   current_ekf_pose_.header.stamp = this->now();
-  current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X);
-  current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y);
-  current_ekf_pose_.pose.position.z = z_filter_.get_x();
-  double roll = roll_filter_.get_x();
-  double pitch = pitch_filter_.get_x();
-  double yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB);
+  current_ekf_pose_.pose.position.x = x;
+  current_ekf_pose_.pose.position.y = y;
+  current_ekf_pose_.pose.position.z = z;
   current_ekf_pose_.pose.orientation =
     tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw);
 
   current_biased_ekf_pose_ = current_ekf_pose_;
   current_biased_ekf_pose_.pose.orientation =
-    tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, ekf_.getXelement(IDX::YAW));
+    tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw);
 
   current_ekf_twist_.header.frame_id = "base_link";
   current_ekf_twist_.header.stamp = this->now();
-  current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX);
-  current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ);
+  current_ekf_twist_.twist.linear.x = vx;
+  current_ekf_twist_.twist.angular.z = wz;
+
+  /* publish ekf result */
+  publishEstimateResult();
+}
+
+void EKFLocalizer::showCurrentX()
+{
+  if (show_debug_info_) {
+    Eigen::MatrixXd X(dim_x_, 1);
+    ekf_.getLatestX(X);
+    DEBUG_PRINT_MAT(X.transpose());
+  }
 }
 
 /*