diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
index 13721c03ca22e..78052cb8198a3 100644
--- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
+++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
@@ -51,8 +51,7 @@ class MapUpdateModule
     rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
     std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
     std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
-    rclcpp::CallbackGroup::SharedPtr main_callback_group,
-    std::shared_ptr<std::map<std::string, std::string>> state_ptr);
+    rclcpp::CallbackGroup::SharedPtr main_callback_group);
 
 private:
   friend class NDTScanMatcher;
@@ -82,7 +81,6 @@ class MapUpdateModule
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
-  std::shared_ptr<std::map<std::string, std::string>> state_ptr_;
 
   std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
   std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
index 9eb62230dad9e..f0d90fdfa0e08 100644
--- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
+++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
@@ -132,7 +132,7 @@ class NDTScanMatcher : public rclcpp::Node
     const rclcpp::Time & sensor_ros_time);
   void add_regularization_pose(const rclcpp::Time & sensor_ros_time);
 
-  void timer_diagnostic();
+  void publish_diagnostic();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
@@ -203,8 +203,6 @@ class NDTScanMatcher : public rclcpp::Node
   std::mutex ndt_ptr_mtx_;
   std::mutex initial_pose_array_mtx_;
 
-  std::thread diagnostic_thread_;
-
   // variables for regularization
   const bool regularization_enabled_;
   std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>
diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
index 7512ae2e77a21..f0a583417fb76 100644
--- a/localization/ndt_scan_matcher/src/map_update_module.cpp
+++ b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -26,15 +26,13 @@ MapUpdateModule::MapUpdateModule(
   rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
   std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
-  rclcpp::CallbackGroup::SharedPtr main_callback_group,
-  std::shared_ptr<std::map<std::string, std::string>> state_ptr)
+  rclcpp::CallbackGroup::SharedPtr main_callback_group)
 : ndt_ptr_(std::move(ndt_ptr)),
   ndt_ptr_mutex_(ndt_ptr_mutex),
   map_frame_(std::move(map_frame)),
   logger_(node->get_logger()),
   clock_(node->get_clock()),
   tf2_listener_module_(std::move(tf2_listener_module)),
-  state_ptr_(std::move(state_ptr)),
   dynamic_map_loading_update_distance_(
     node->declare_parameter<double>("dynamic_map_loading_update_distance")),
   dynamic_map_loading_map_radius_(
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
index cb66998147220..a2e89540a172f 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -280,16 +280,12 @@ NDTScanMatcher::NDTScanMatcher()
       &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
     rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group);
 
-  diagnostic_thread_ = std::thread(&NDTScanMatcher::timer_diagnostic, this);
-  diagnostic_thread_.detach();
-
   tf2_listener_module_ = std::make_shared<Tf2ListenerModule>(this);
 
   use_dynamic_map_loading_ = this->declare_parameter<bool>("use_dynamic_map_loading");
   if (use_dynamic_map_loading_) {
     map_update_module_ = std::make_unique<MapUpdateModule>(
-      this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group,
-      state_ptr_);
+      this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group);
   } else {
     map_module_ = std::make_unique<MapModule>(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group);
   }
@@ -297,76 +293,71 @@ NDTScanMatcher::NDTScanMatcher()
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
 }
 
-void NDTScanMatcher::timer_diagnostic()
+void NDTScanMatcher::publish_diagnostic()
 {
-  rclcpp::Rate rate(100);
-  while (rclcpp::ok()) {
-    diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
-    diag_status_msg.name = "ndt_scan_matcher";
-    diag_status_msg.hardware_id = "";
-
-    for (const auto & key_value : (*state_ptr_)) {
-      diagnostic_msgs::msg::KeyValue key_value_msg;
-      key_value_msg.key = key_value.first;
-      key_value_msg.value = key_value.second;
-      diag_status_msg.values.push_back(key_value_msg);
-    }
-
+  diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
+  diag_status_msg.name = "ndt_scan_matcher";
+  diag_status_msg.hardware_id = "";
+
+  for (const auto & key_value : (*state_ptr_)) {
+    diagnostic_msgs::msg::KeyValue key_value_msg;
+    key_value_msg.key = key_value.first;
+    key_value_msg.value = key_value.second;
+    diag_status_msg.values.push_back(key_value_msg);
+  }
+
+  diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  diag_status_msg.message = "";
+  if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message += "Initializing State. ";
+  }
+  if (
+    state_ptr_->count("lidar_topic_delay_time_sec") &&
+    std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. ";
+  }
+  if (
+    state_ptr_->count("skipping_publish_num") &&
+    std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 &&
+    std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message += "skipping_publish_num > 1. ";
+  }
+  if (
+    state_ptr_->count("skipping_publish_num") &&
+    std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    diag_status_msg.message += "skipping_publish_num exceed limit. ";
+  }
+  if (
+    state_ptr_->count("nearest_voxel_transformation_likelihood") &&
+    std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) <
+      converged_param_nearest_voxel_transformation_likelihood_) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message += "NDT score is unreliably low. ";
+  }
+  if (
+    state_ptr_->count("execution_time") &&
+    std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message +=
+      "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])";
+  }
+  // Ignore local optimal solution
+  if (
+    state_ptr_->count("is_local_optimal_solution_oscillation") &&
+    std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) {
     diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-    diag_status_msg.message = "";
-    if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message += "Initializing State. ";
-    }
-    if (
-      state_ptr_->count("lidar_topic_delay_time_sec") &&
-      std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. ";
-    }
-    if (
-      state_ptr_->count("skipping_publish_num") &&
-      std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 &&
-      std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message += "skipping_publish_num > 1. ";
-    }
-    if (
-      state_ptr_->count("skipping_publish_num") &&
-      std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
-      diag_status_msg.message += "skipping_publish_num exceed limit. ";
-    }
-    if (
-      state_ptr_->count("nearest_voxel_transformation_likelihood") &&
-      std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) <
-        converged_param_nearest_voxel_transformation_likelihood_) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message += "NDT score is unreliably low. ";
-    }
-    if (
-      state_ptr_->count("execution_time") &&
-      std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message +=
-        "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])";
-    }
-    // Ignore local optimal solution
-    if (
-      state_ptr_->count("is_local_optimal_solution_oscillation") &&
-      std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-      diag_status_msg.message = "local optimal solution oscillation occurred";
-    }
-
-    diagnostic_msgs::msg::DiagnosticArray diag_msg;
-    diag_msg.header.stamp = this->now();
-    diag_msg.status.push_back(diag_status_msg);
+    diag_status_msg.message = "local optimal solution oscillation occurred";
+  }
 
-    diagnostics_pub_->publish(diag_msg);
+  diagnostic_msgs::msg::DiagnosticArray diag_msg;
+  diag_msg.header.stamp = this->now();
+  diag_msg.status.push_back(diag_status_msg);
 
-    rate.sleep();
-  }
+  diagnostics_pub_->publish(diag_msg);
 }
 
 void NDTScanMatcher::callback_initial_pose(
@@ -476,13 +467,11 @@ void NDTScanMatcher::callback_sensor_points(
   }
 
   // perform ndt scan matching
-  (*state_ptr_)["state"] = "Aligning";
   const Eigen::Matrix4f initial_pose_matrix =
     pose_to_matrix4f(interpolator.get_current_pose().pose.pose);
   auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
   ndt_ptr_->align(*output_cloud, initial_pose_matrix);
   const pclomp::NdtResult ndt_result = ndt_ptr_->getResult();
-  (*state_ptr_)["state"] = "Sleeping";
 
   const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose);
   std::vector<geometry_msgs::msg::Pose> transformation_msg_array;
@@ -573,6 +562,7 @@ void NDTScanMatcher::callback_sensor_points(
       make_float32_stamped(sensor_ros_time, no_ground_nearest_voxel_transformation_likelihood));
   }
 
+  (*state_ptr_)["state"] = "Aligned";
   (*state_ptr_)["transform_probability"] = std::to_string(ndt_result.transform_probability);
   (*state_ptr_)["nearest_voxel_transformation_likelihood"] =
     std::to_string(ndt_result.nearest_voxel_transformation_likelihood);
@@ -584,6 +574,8 @@ void NDTScanMatcher::callback_sensor_points(
     (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0";
   }
   (*state_ptr_)["execution_time"] = std::to_string(exe_time);
+
+  publish_diagnostic();
 }
 
 void NDTScanMatcher::transform_sensor_measurement(
@@ -859,8 +851,6 @@ void NDTScanMatcher::service_trigger_node(
   if (is_activated_) {
     std::lock_guard<std::mutex> initial_pose_array_lock(initial_pose_array_mtx_);
     initial_pose_msg_ptr_array_.clear();
-  } else {
-    (*state_ptr_)["state"] = "Initializing";
   }
   res->success = true;
 }
@@ -897,9 +887,7 @@ void NDTScanMatcher::service_ndt_align(
     return;
   }
 
-  (*state_ptr_)["state"] = "Aligning";
   res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame);
-  (*state_ptr_)["state"] = "Sleeping";
   res->success = true;
   res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance;
 }