:optimal

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