diff --git a/src/depth_local_costmap_noetic_test_node.cpp b/src/depth_local_costmap_noetic_test_node.cpp index 5974912..0b2a405 100644 --- a/src/depth_local_costmap_noetic_test_node.cpp +++ b/src/depth_local_costmap_noetic_test_node.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -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 = - is_static ? tf3::Time::now() - : tf3::Time(ros_tf.header.stamp.sec, ros_tf.header.stamp.nsec); + tf3_msg.header.stamp = is_static ? tf3::Time::now() : src_stamp; 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_buffer_; tf3::BufferCore& tf3_buffer_; std::atomic 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 last_dynamic_stamp_; std::thread worker_; };