TF behaviour showing delay when using ros::Time::now() vs. ros::Time(0)
My situation is that I have /tf
topic publishing the following frame transforms:
- A to B
- B to C
- C to D
My goal is to create a simple C++ program which extracts the transform A to D at the current time (i.e. ros::Time::now()
). I call E the extracted target frame, and ideally E should coincide with D at each 10 Hz update of the while loop that I am using to listen in on the /tf
topic. Here is the C++ code:
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "tf_measurement_jumping_debug");
ros::NodeHandle nh;
tf::TransformListener tf_listener;
tf::TransformBroadcaster tf_broadcaster;
tf::StampedTransform transform;
ros::Rate loop_rate(10);
while (ros::ok())
{
loop_rate.sleep();
try
{
// Get the current transform from A to D
ros::Time tf_time = ros::Time::now();
tf_listener.waitForTransform("A", "D", tf_time, ros::Duration(3.0));
tf_listener.lookupTransform("A", "D", tf_time, transform);
// Publish back what I got
transform.child_frame_id_ = "E"; // Rename to not overwrite D
tf_broadcaster.sendTransform(transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR_STREAM(ex.what());
}
}
}
The result is not satisfactory as shown in the following screen recording:
There is some kind fo delay in the transform from frame A to E. The reason I think that this is a delay issue is because if I move frame B slowly enough, then E more or less coincides with D (note that the transform B to C is rigid).
In the above code, if instead of ros::Time tf_time = ros::Time::now();
I use ros::Time tf_time = ros::Time(0);
then the result is what I want:
My question is what is the cause of this? Ideally, I want a solution using ros::Time::now()
because
- What I am looking for is the current transform, not the latest transform;
- When transforms stop arriving (e.g. because transform C to D is no longer being published) then the
ros::Time(0)
"hack" will keep sending the last transform prior to this event and I do not want that.
Thank you very much for your help. I have looked at the /tf
tutorials and saw the timestamp debugging section - however unfortunately none of it has so far helped me to resolve this problem.
Update (tf_monitor output)
Here's the output of rosrun tf tf_monitor
:
RESULTS: for all Frames
Frames:
Frame: B published by unknown_publisher Average Delay: 0.000125737 Max Delay: 0.0130457
Frame: C published by unknown_publisher Average Delay: 0.00024707 Max Delay: 0.0130457
Frame: D published by unknown_publisher Average Delay: 0.24257 Max Delay: 0.282611
All Broadcasters:
Node: unknown_publisher 196.535 Hz, Average Delay: 0.00547737 Max Delay: 0.276091
And here is the output of rosrun tf tf_monitor A D
:
RESULTS: for A to D
Chain is: A -> B -> C -> D
Net delay avg = 0.864727: max = 3.39458
Frames:
Frame: B published by unknown_publisher Average Delay: -2.90574e-05 Max Delay: 0.0139616
Frame: C published by unknown_publisher Average Delay: 0.000132843 ...