ROS2 Jazzy TF2 delay causing docking pathing issues

Question

I am currently working on a mobile robot using ROS2 Jazzy with the Navigation2 docking server, along with Ros_ArUco_Opencv for ARTag pose recognition. I am seeing a delay between movement of the ARTag and the detected pose, but only in specific ways.

When the robot is stationary, and the tag is moving, the detected pose has almost no delay, and keeps up with the laser data, as shown here: Tag Moving Video

When the reference frame is base_link, and the robot is moving while the tag is stationary, there is also no delay.

But, when the reference frame is either odom or map, and the robot moves with a stationary tag, there is significant delay between the movement of the tag in the camera and the detected pose, shown here: Robot Moving Video

This causes issues with docking as when the robot approaches the dock tag, it constantly over-corrects, reacting to the delayed dock pose.

Full TF Tree shown here:

When running tf2_monitor, I am seeing that the average delay for the marker publish is ~0.8-1.3 seconds. Full output here, as the ~3000 second delay readings are also strange, this number increases as the nodes continue to run.

RESULTS: for all Frames

Frames:
Frame: base_footprint, published by <no authority available>, Average Delay: 3165.24, Max Delay: 3165.24
Frame: base_link, published by <no authority available>, Average Delay: 0.00892203, Max Delay: 0.0240092
Frame: base_link_offset, published by <no authority available>, Average Delay: 3165.24, Max Delay: 3165.24
Frame: camera, published by <no authority available>, Average Delay: 3165.24, Max Delay: 3165.24
Frame: drivewhl_l_link, published by <no authority available>, Average Delay: 0.00492064, Max Delay: 0.482618
Frame: drivewhl_r_link, published by <no authority available>, Average Delay: 0.00492276, Max Delay: 0.482621
Frame: laser, published by <no authority available>, Average Delay: 3165.24, Max Delay: 3165.24
Frame: laser2, published by <no authority available>, Average Delay: 3165.24, Max Delay: 3165.24
Frame: marker_1, published by <no authority available>, Average Delay: 0.803176, Max Delay: 0.834151
Frame: odom, published by <no authority available>, Average Delay: -0.183419, Max Delay: 0

My questions are as follows:

1. Is there any way to reduce the delay shown in the transform between the detected tags and the odom frame?
2. Are there other options for dock pose detection that would not have this delay? I have tried the apriltags ros package as well as another aruco tag detector.

Any help is greatly appreciated!

Answer 1

When the robot is stationary, and the tag is moving, the detected pose has almost no delay, and keeps up with the laser data, as shown here: Tag Moving Video

when the reference frame is either odom or map

What frame is this in in rviz? It looks like it gets there, but has some convergence delay as created by your odometry or localization system (if in odom, then its odometry; if its in map then you'd have to switch to odom and see if its still an issue; if both then its probably your odometry). It seems like the detection is doing the right thing, but your transforms of your robot are having issues.

What does this look like in the base_link and/or the camera frame? If not, then its your state estimation for sure. If there's still delay, then could be camera data / detector transform related instead of state estimation.

I assume the reason there's "no delay" when the robot is stationary is because the issues are w.r.t. your state estimation and since your robot isn't moving, the state estimation isn't updating and therefore you don't see any issues with the lag since the change in data is near-zero.

Is there any way to reduce the delay shown in the transform between the detected tags and the odom frame?

You're using your own tag detector, so you'd have to look into that yourself, that doesn't relate to anything that Nav2 or Docking provides. That's a node that you found or wrote :-) As you point out in your TF findings, that's what's delayed which the rest of the system is trying to use (or perhaps odometry?). Are you sure its the detector node that's delayed, or all camera data that's going into the detector node?

Are there other options for dock pose detection that would not have this delay? I have tried the apriltags ros package as well as another aruco tag detector.

I wouldn't immediately blame the detector method, decoding QR codes is wicked fast. It might be the data source, data transport, or other issue in stamping from the node's execution itself related to camera misstamping. It could be your detector node though, who's to say without further investigation.

PS: both video links go to the same video, so I'm not sure what the tag moving only video was supposed to show. I think I get the gist though.

Answer 2

I was able to "fix" this issue by editing my dock pose publisher node to circumvent the still present delay in some of my TF tree transforms.

Previously, all this node did was subscribe to the detentions made by the ArUco node, and publish that pose onto the /detected_dock_pose topic, letting the docking server handle the transform from the camera frame to the odom frame.

Now, the edited node caches the static camera -> base_link transform, as well as the most recently published base_link -> odom transform. When a new detection is made, I use these two cached transforms to publish the pose directly in the odom frame.

This is opposed to the old implementation, which transforms the pose from the camera frame to the odom frame only after a detection is made, which makes the dock pose publishing more responsive, at the cost of some accuracy.