:optimal

src/depth_local_costmap_noetic_test_node.cpp

@@ -44,6 +44,7 @@
 #include <sstream>
 #include <string>
 #include <thread>
+#include <unordered_map>
 #include <vector>
 
 #include <boost/thread/locks.hpp>
@@ -215,10 +216,21 @@ private:
               ros_tf.header.stamp.isZero() ||
               (ros::Time::now() - ros_tf.header.stamp).toSec() > kStaticStampAgeSec;
 
+          const tf3::Time src_stamp(ros_tf.header.stamp.sec, ros_tf.header.stamp.nsec);
+          if (!is_static)
+          {
+            // robot_state_publisher / odom often publish slower than this 50 Hz
+            // poll; re-pushing an unchanged latest stamp makes tf3 log
+            // TF_REPEATED_DATA every cycle and wastes work. Only push when the
+            // source stamp actually advanced.
+            const auto it = last_dynamic_stamp_.find(edge.child);
+            if (it != last_dynamic_stamp_.end() && it->second == src_stamp)
+              continue;
+            last_dynamic_stamp_[edge.child] = src_stamp;
+          }
+
           tf3::TransformStampedMsg tf3_msg;
-          tf3_msg.header.stamp =
+          tf3_msg.header.stamp = is_static ? tf3::Time::now() : src_stamp;
-              is_static ? tf3::Time::now()
-                        : tf3::Time(ros_tf.header.stamp.sec, ros_tf.header.stamp.nsec);
           tf3_msg.header.frame_id = ros_tf.header.frame_id;
           tf3_msg.child_frame_id = ros_tf.child_frame_id;
           tf3_msg.transform.translation.x = ros_tf.transform.translation.x;
@@ -246,6 +258,9 @@ private:
   std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
   tf3::BufferCore& tf3_buffer_;
   std::atomic<bool> stop_{false};
+  // Last source stamp pushed per child frame; used to skip redundant re-inserts.
+  // Declared before worker_ so it is constructed before the thread starts.
+  std::unordered_map<std::string, tf3::Time> last_dynamic_stamp_;
   std::thread worker_;
 };