Currently FusionCore starts the filter as soon as it receives the first IMU message and begins accumulating state. If GPS and wheel odometry come online 2 or 3 seconds later (which is normal at startup), the filter has already drifted purely on IMU integration before the other sensors can anchor it.
The fix is a configurable initialization guard: the filter waits until all sensors that are expected to be present have published at least one message before starting. So if you have IMU + wheel odom + GPS configured, it holds in a pre-initialized state until all three have checked in, then initializes cleanly with a full set of measurements.
fusioncore:
ros__parameters:
init.wait_for_all_sensors: true # default false for backwards compat
init.sensor_wait_timeout: 10.0 # give up and start anyway after this many secondsThis has been open in robot_localization since 2015 (issue #166, 10+ years) and was never implemented. The workaround people use is adding a sleep in their launch file which is obviously terrible.
The timeout is important so the filter doesn't hang forever if a sensor fails to come up. After the timeout it should log a warning showing which sensors were missing and start anyway.
Currently FusionCore starts the filter as soon as it receives the first IMU message and begins accumulating state. If GPS and wheel odometry come online 2 or 3 seconds later (which is normal at startup), the filter has already drifted purely on IMU integration before the other sensors can anchor it.
The fix is a configurable initialization guard: the filter waits until all sensors that are expected to be present have published at least one message before starting. So if you have IMU + wheel odom + GPS configured, it holds in a pre-initialized state until all three have checked in, then initializes cleanly with a full set of measurements.
This has been open in robot_localization since 2015 (issue #166, 10+ years) and was never implemented. The workaround people use is adding a
sleepin their launch file which is obviously terrible.The timeout is important so the filter doesn't hang forever if a sensor fails to come up. After the timeout it should log a warning showing which sensors were missing and start anyway.