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 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?