DuongTD/depth_local_costmap_noetic_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

:optimal

Browse Source
This commit is contained in:
duongtd
2026-07-27 10:10:35 +07:00
parent dcfc5809cc
commit 8432814060
1 changed files with 18 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/depth_local_costmap_noetic_test_node.cpp
View File

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