Revision history [back]

This can be indicative of having old measurement data. If you get two identical time stamps in a row, it's because the filter published its state, and then received an old measurement, backed up in its history, inserted the measurement, and then fused the subsequent measurements, and re-published the last state. Try eliminating one input to see if that fixes the timing. If it does, then make sure your measurements are correctly stamped. You can also turn off the history buffer.

EDIT in response to comments.

First, it's probably a lot easier to post links to code, rather than copying and pasting it here. It also lets me know what code branch you're using. The code in question isn't from melodic-level, so I'll assume you're using kinetic-devel.

The longer answer requires us to walk through one cycle for the filter. This refers to the code branch you are using, rather than the latest stuff, which works on a timer (but would, I think, have the same behaviour).

At time t0, we call spinOnce(), which fires all the callbacks for the sensor messages we received. We throw them all into a measurement queue.
We walk through that queue, fusing each measurement in temporal order.
After fusing the measurements, we are now at time t1, because fusing the measurements obviously takes time. We publish the state at this time.
The elapsed time is e = t1 - t0. If the configured frequency of the filter is N, our cycle time is 1/N. But we ate up some of our cycle time fusing measurements, so we only need to sleep for 1/N - e seconds. I should note that the ros::Rate object does all this logic for you, but I wasn't aware of that when I wrote these, I think. But I digress...
Go back to step 1.

Hopefully you can see the problem here. In step 2, if there are three measurements in the first cycle and eight measurements in the second cycle, then there will be an uneven amount of time between thw two publications. We can't know how many measurements we'll have, so we can't know how long it will take to fuse them all.

There are (at least) two alternatives, though I'm open to hearing more:

Do all the fusing, then wait until the end of the cycle, and publish the last computed state. This is a bad idea, because you'd be publishing old information from time t1 at some later time, t2.
Sleep until the end of the cycle, and then predict the state using the filter's prediction stage to get the state at time t2, and publish that instead. This is also not great, for two reasons. First, the filter will have to go back in its history in the next cycle, because while you were sleeping (before the predict/publish), more measurements arrived in the queue. So now you have to rewind the state, replay those measurements, and output a new state. Second, it means that robots whose kinematics don't comply with the simple omnidirectional motion model that the filters use will be getting poor predicted states. If we publish immediately after fusing the true measurement data, we aren't introducing as much error in the state estimate.

Anyway, the short answer is that we can't predict how many measurements we'll have in a cycle.

When you use rostopic hz, it counts the number of messages over some small (and configurable) window. The small errors you see in publication time will average out.