update

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);