From 6fa5c3f57b00ee9f907cd11819b3887f5d899587 Mon Sep 17 00:00:00 2001
From: Takeshi Ishita <ishitah.takeshi@gmail.com>
Date: Thu, 25 Aug 2022 14:52:02 +0900
Subject: [PATCH] fix(ekf_localizer): update the state and publish the
 estimated pose only after initialization

---
 .../include/ekf_localizer/ekf_localizer.hpp          |  2 ++
 localization/ekf_localizer/src/ekf_localizer.cpp     | 12 ++++++++++++
 2 files changed, 14 insertions(+)

diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
index 2b3c6049676f9..c9a7a0dc6014c 100644
--- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
+++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -196,6 +196,8 @@ class EKFLocalizer : public rclcpp::Node
   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_initialized_;
+
   enum IDX {
     X = 0,
     Y = 1,
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
index d6b98c368ae09..41ae99015f871 100644
--- a/localization/ekf_localizer/src/ekf_localizer.cpp
+++ b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -65,6 +65,8 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
   proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0);
   proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0);
 
+  is_initialized_ = false;
+
   /* initialize ros system */
   auto period_control_ns =
     std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(ekf_dt_));
@@ -137,6 +139,10 @@ void EKFLocalizer::updatePredictFrequency()
  */
 void EKFLocalizer::timerCallback()
 {
+  if (!is_initialized_) {
+    return;
+  }
+
   DEBUG_INFO(get_logger(), "========================= timer called =========================");
 
   /* update predict frequency with measured timer rate */
@@ -234,6 +240,10 @@ void EKFLocalizer::setCurrentResult()
  */
 void EKFLocalizer::timerTFCallback()
 {
+  if (!is_initialized_) {
+    return;
+  }
+
   if (current_ekf_pose_.header.frame_id == "") {
     return;
   }
@@ -325,6 +335,8 @@ void EKFLocalizer::callbackInitialPose(
   updateSimple1DFilters(*initialpose);
 
   while (!current_pose_info_queue_.empty()) current_pose_info_queue_.pop();
+
+  is_initialized_ = true;
 }
 
 /*