diff --git a/Controllers/Packages/amr_control/src/tf_converter.cpp b/Controllers/Packages/amr_control/src/tf_converter.cpp
index 1dcdbc9..baacd81 100644
--- a/Controllers/Packages/amr_control/src/tf_converter.cpp
+++ b/Controllers/Packages/amr_control/src/tf_converter.cpp
@@ -27,20 +27,30 @@ void amr_control::TfConverter::tfWorker()
   std::string tree;
   std::string line;
 
+  int count_tf_receive_done = 0;
   ros::Rate rate(20);
   while (ros::ok() && !stop_tf_thread_)
   {
-      tree = tfBuffer.allFramesAsString();
+    ros::spinOnce();
+    tree = tfBuffer.allFramesAsString();
+    if (!tree.empty() && tree == last_tree)
+    {
+      count_tf_receive_done++;
+    }
+    else
+    {
+      count_tf_receive_done = 0;
+    }
 
-      if (!tree.empty() && tree == last_tree)
-      {
-          // ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
-          break;
-      }
+    if(count_tf_receive_done > 2)
+    {
+      ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
+      break;
+    }
 
-      last_tree = tree;
-      ros::spinOnce();
-      rate.sleep();
+    last_tree = tree;
+    
+    rate.sleep();
   }
 
   std::istringstream iss(tree);
diff --git a/pnkx_nav_core b/pnkx_nav_core
index b7e4c73..34cabd2 160000
--- a/pnkx_nav_core
+++ b/pnkx_nav_core
@@ -1 +1 @@
-Subproject commit b7e4c73c14f4b40cad35c38a3d3d7d147ad61890
+Subproject commit 34cabd2083edd58f3593284f70020cf01a9926b9