From 84328140606cc679fb9fbdf322afc5e6a06b4f35 Mon Sep 17 00:00:00 2001 From: duongtd Date: Mon, 27 Jul 2026 10:10:35 +0700 Subject: [PATCH] :optimal --- src/depth_local_costmap_noetic_test_node.cpp | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/depth_local_costmap_noetic_test_node.cpp b/src/depth_local_costmap_noetic_test_node.cpp index 6731e26..5974912 100644 --- a/src/depth_local_costmap_noetic_test_node.cpp +++ b/src/depth_local_costmap_noetic_test_node.cpp @@ -201,8 +201,24 @@ private: const geometry_msgs::TransformStamped ros_tf = tf2_buffer_->lookupTransform(edge.parent, edge.child, ros::Time(0)); + // Preserve the real source timestamp so tf3 owns a true timeline and + // consumers can look up the pose at a sensor's capture time (needed to + // register depth clearing against the frame it was measured from). + // Static URDF mounts (base_link -> camera_*) come back from tf2 with a + // zero / very old stamp when queried at Time(0); feed those into the + // tf3 StaticCache (is_static) so they stay valid across all query + // times. If they were stored as dynamic with a single stale sample, + // any later stamped lookup would extrapolate off that lone point and + // throw, disabling clearing entirely. + constexpr double kStaticStampAgeSec = 1.0; // [s] + const bool is_static = + ros_tf.header.stamp.isZero() || + (ros::Time::now() - ros_tf.header.stamp).toSec() > kStaticStampAgeSec; + tf3::TransformStampedMsg tf3_msg; - tf3_msg.header.stamp = tf3::Time::now(); + tf3_msg.header.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; @@ -212,7 +228,7 @@ private: tf3_msg.transform.rotation.y = ros_tf.transform.rotation.y; tf3_msg.transform.rotation.z = ros_tf.transform.rotation.z; tf3_msg.transform.rotation.w = ros_tf.transform.rotation.w; - tf3_buffer_.setTransform(tf3_msg, "ros_tf_bridge"); + tf3_buffer_.setTransform(tf3_msg, "ros_tf_bridge", is_static); } catch (const tf2::TransformException& ex) {