From 97c301602f4ce32d608fa9167aba14efb39d2caf Mon Sep 17 00:00:00 2001
From: TaikiYamada4 <taiki.yamada@tier4.jp>
Date: Wed, 15 Nov 2023 11:24:20 +0900
Subject: [PATCH] Refactored names of the variables and function to suit their
 current functionality. Especially, updatePredictionTime is no longer updating
 prediction time which is fixed here, and changed its name to measureLapTime.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
---
 .../include/ekf_localizer/ekf_localizer.hpp   |  8 +++----
 .../ekf_localizer/src/ekf_localizer.cpp       | 22 +++++++++----------
 2 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
index e972b316626ab..0a7a89a910202 100644
--- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
+++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -163,8 +163,8 @@ class EKFLocalizer : public rclcpp::Node
   AgedObjectQueue<geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr> pose_queue_;
   AgedObjectQueue<geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr> twist_queue_;
 
-  //!< @brief accumulated time for timer callback
-  rclcpp::Duration accumulated_time_;
+  //!< @brief accumulated time for prediction in timer callback
+  rclcpp::Duration accumulated_lap_time_;
 
   /**
    * @brief computes update & prediction of EKF for each ekf_dt_[s] time
@@ -192,9 +192,9 @@ class EKFLocalizer : public rclcpp::Node
   void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 
   /**
-   * @brief update predict frequency
+   * @brief measure lap time of timer callback
    */
-  void updatePredictFrequency();
+  void measureLapTime();
 
   /**
    * @brief get transform from frame_id
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
index 9aee78566c866..35ba82c5b6a72 100644
--- a/localization/ekf_localizer/src/ekf_localizer.cpp
+++ b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -46,7 +46,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
   params_(this),
   pose_queue_(params_.pose_smoothing_steps),
   twist_queue_(params_.twist_smoothing_steps),
-  accumulated_time_(0, 0)
+  accumulated_lap_time_(0, 0)
 {
   is_activated_ = false;
 
@@ -95,18 +95,18 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
 }
 
 /*
- * updatePredictFrequency
+ * measureLapTime
  */
-void EKFLocalizer::updatePredictFrequency()
+void EKFLocalizer::measureLapTime()
 {
   rclcpp::Time current_time = get_clock()->now();
   if (last_predict_time_.seconds() != 0.0) {
     if (current_time < last_predict_time_) {
       warning_->warn("Detected jump back in time");
     } else {
-      accumulated_time_ = accumulated_time_ + (current_time - last_predict_time_);
+      accumulated_lap_time_ = accumulated_lap_time_ + (current_time - last_predict_time_);
       double current_ekf_dt_ = (current_time - last_predict_time_).seconds();
-      DEBUG_INFO(get_logger(), "[EKF] update ekf_dt_ to %f seconds", current_ekf_dt_);
+      DEBUG_INFO(get_logger(), "[EKF] timer callback lap time = %f [sec]", current_ekf_dt_);
     }
   } 
   last_predict_time_ = current_time;
@@ -126,20 +126,20 @@ void EKFLocalizer::timerCallback()
 
   DEBUG_INFO(get_logger(), "========================= timer called =========================");
 
-  /* update predict frequency with measured timer rate */
-  updatePredictFrequency();
+  /* measure lap time of this timer callback and check time jumps */
+  measureLapTime();
 
   /* predict model in EKF */
   stop_watch_.tic();
   DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------");
-  DEBUG_INFO(get_logger(), "[EKF] accumulated_time = %lf [s] (before prediction)", accumulated_time_.seconds());
-  size_t prediction_times = static_cast<int>(accumulated_time_.seconds() / params_.ekf_dt);
+  DEBUG_INFO(get_logger(), "[EKF] accumulated_time = %lf [s] (before prediction)", accumulated_lap_time_.seconds());
+  size_t prediction_times = static_cast<int>(accumulated_lap_time_.seconds() / params_.ekf_dt);
   for (size_t i = 0; i < prediction_times; ++i) {
     ekf_module_->predictWithDelay(params_.ekf_dt);
   }
-  accumulated_time_ = accumulated_time_ - rclcpp::Duration::from_seconds(params_.ekf_dt * prediction_times);
+  accumulated_lap_time_ = accumulated_lap_time_ - rclcpp::Duration::from_seconds(params_.ekf_dt * prediction_times);
   DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms] (predicted %d times)", stop_watch_.toc(), static_cast<int>(prediction_times));
-  DEBUG_INFO(get_logger(), "[EKF] accumulated_time = %lf [s] (after prediction)", accumulated_time_.seconds());
+  DEBUG_INFO(get_logger(), "[EKF] accumulated_time = %lf [s] (after prediction)", accumulated_lap_time_.seconds());
   DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n");
 
   /* pose measurement update */