Fix discrepancy between tf and odometry/filtered when world_frame is set to map_frame

[sybrenkappert]

I found that, even when the transform_timeout parameter is set, there is a difference between /odometry/filtered and the map-base_link transform from a tf lookup with the same timestamp.

This occurs because the base_link-odom transform that is required to compute the broadcastable map-odom transform is evaluated at the wrong time. Instead of evaluating this at the same time as the computed map-base_link transform, it is always evaluated at tf2::TimePointZero.

This solution evaluates the base_link-odom transform at the correct time if a suitable value for the already existing transform_timeout new transform_timeout_odom_bl parameter is set. If the timeout is insufficient to get the transform at the requested time, the latest available transform is used (tf2::TimePointZero). This is covered by lookupTransFormsafe.

The solution improves performance in applications that rely on accurate map-odom transforms. Examples being precision navigation and comparison of estimates with ground truth data.

[ayrton04]

While I welcome the change, I think this behaviour is going to have to be a parameter, and it should default to false so we don't break anyone else's application. If we have to wait for the odom->base_link transform to be available, it could affect the latency and publication rate of the output (which is why we're using the latest available transform to begin with).

[sybrenkappert]

I have had some time to think. In the current form, the latency could indeed be affected in applications that have overridden the transform_timeout parameter. Rather than adding a parameter to enable/disable this behavior, I suggest we add a dedicated timeout parameter for the odom->base_link transform lookup. Reason being:

The transform_timeout parameter is specifically used during the conversion of measurements to the target frame. This purpose differs significantly from retrieving the odometry transform. I can imagine developers would want to set timeouts of these two cases to different values.

By adding a new parameter (e.g. transform_timeout_odom) and setting the default value to zero, the latency in existing applications should not be affected. The accuracy could improve even with the default value as it allows computation of a better transform if there is sufficient data in the transform buffer. Developers can override this new timeout parameter with a value that suits their application to benefit from improved broadcasted transform accuracy. Would this be a good approach?

[ayrton04]

Yes, that seems valid!

[ayrton04]

Maybe transform_timeout_odom_bl?

[ayrton04]

@sybrenkappert any chance you would want to update the PR with my feedback? If you've moved on and dropped this, it's totally fine, but in that case, would you mind closing the PR? Thanks.

[sybrenkappert]

@ayrton04 Thanks for the feedback! I'm planning on updating the PR, I wasn't able to find time to work on this until now. Hoping to publish an update soon.

[sybrenkappert]

@ayrton04 Just updated the PR.