Persistent Transform Warning in robot_localization
I am fusing odometry, imu, and ultrasonic beacon data using two robot_localization Kalman filters - one creating a fused odometry measurement and publishing the /odom frame, and a second fusing a world pose gathered from Marvelmind Ultrasonic Beacons.
We have used the setup for a number of years with no real issues, but it seems that recently something has changed which is now triggering a stream of warnings from our map kalman filter. It is not obvious when this change was introduced, or if it is detrimental (switching from kinetic to melodic, building ros for python 3, switching hardware platforms)
While our localization runs, we get a stream of messages like this:
Node: /ekf_map
Time: 11:46:05.217432323 (2020-01-24)
Severity: Warn
Published Topics: /diagnostics, /odom_map, /rosout, /tf
Transform from odom to map was unavailable for the time requested. Using latest instead.
Also including warning such as:
[ WARN] [1579896201.631471320]: Transform from beacon_frame to map was unavailable for the time requested. Using latest instead.
[ WARN] [1579896203.627275704]: Transform from imu to base_footprint was unavailable for the time requested. Using latest instead.
I have looked through all the other posts that address this, which are largely centered around nav_sat_transform nodes, where time lag is the root of the issue. Our beacons are emitting measurements at around 4hz, so it makes sense to me that it could be a related problem. However, I have tried all the methods suggested for people having those issues (adjusting transform_time_offset, predict to current time, sensor_timeout values, etc) with no luck.
All of my relevant frames are publishing, and I have verified they are current at the time that the warnings are being generated. I am ultimately at a loss for what the issue may be.
If you are able to provide any additional insight, it would be much appreciated. Find attached the frames, configurations for each kalman filter, and a 60s ros bag of sensor data, fused odometry, and tf output.
I don't have enough karma to upload files directly it seems, so find the files in this shared folder: https://drive.google.com/open?id=1RsL...
A few notes on the tf tree - we have static transforms defining:
map -> beacon_frame
base_footprint -> base_link
base_link -> imu
base_link -> hedge_link
Odometry EKF (fusing imu and wheel encoder data):
frequency: 24
sensor_timeout: 0.05
two_d_mode: true
transform_time_offset: 0
transform_timeout: 0.0
publish_tf: true
publish_acceleration: false
odom_frame: odom
base_link_frame: base_footprint
world_frame: odom
odom0: odom_wheels
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 2
imu0: imu/data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
imu0_relative: true
imu0_queue_size: 20
imu0_remove_gravitational_acceleration: true
use_control: false
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0 ...