reset on backwards jumps in time (e.g. looping rosbag)

[lucasw]

I'm using fuse_simple_tutorial.launch to test with (and it is using fixed_lag_smoother):

roslaunch fuse_tutorials fuse_simple_tutorial.launch

Currently when rosbag -l is used the jump back in time causes the state_estimation node to refuse all the old incoming messages:

...ers/src/fixed_lag_smoother.cpp:379]: The current lag expiration time is 203.372000000. The queued transaction with timestamp 162.504000000 from sensor imu_sensor has a minimum involved timestamp of 162.504000000, which is 40.868000000 seconds too old. Ignoring this transaction.

I initially tried detecting the jump back in time within FixedLagSmoother::optimizationLoop() then called resetServiceCallback(), but that locks up that loop (haven't tracked it down yet, could try building with debug symbols next). lucasw@f368a28#diff-969711c8c46e1a4ced29c0acd11c4939bcfa965ef47be0d91d4a9449b0c5e079R171-R181

This log message continues on though:

[W] [1637859555.355, , 193.055 /state_estimator .../src/odometry_2d_publisher.cpp:339]: No valid state data yet. Delaying tf broadcast.
...

When I call reset from the service call before the bag loop the same lockup happens (so that seems like a separate issue, and it isn't my fault for calling reset from a thread I shouldn't be?):

rosservice call /state_estimator/reset "{}"

The solution I currently have is to detect the time jumps in an outer loop, and just remake the whole fuse_optimizers::FixedLagSmoother object. which is maybe slower meaning more missed messages- but if it is good enough I could make a PR of it:

lucasw@5d43eb2