Reset the filter on ROS time jumping back

[peci1]

I see the filters do not react to ROS time jumping back. Would it be possible to add a (configurable) threshold on the size of the jump, and when the jump is larger than the threshold, the filter would automatically re-initialize? This would be especially helpful when debugging on a bag file that is played in a loop.

I've checked the interface provided by the Madgwick filter, and there's no public method that would allow complete reset of the filter from downstream code. I could call setOrientation() to at least reset the orientation, but initialized_ would remain true. Maybe just adding a reset() method would suffice, and then whoever cares about the time jumps could implement the automatic reset downstream.

Should I create a PR adding the reset() methods, or is there another preferred solution to this issue?

[mintar]

I'm on vacation right now so I can't check, but this sounds like a very reasonable proposal. I would even enable it by default.

[peci1]

Thanks for the prompt response even from your vacation :) I'll prep the PR in the coming days. I'll leave a few ideas here, but it's okay if you look at them after your vacation is over, they're not blockers for the implementation.

I think finding the right threshold is not that easy. E.g. see #160 - what if the PC started with some future time and jumped back after syncing via NTP? We regularly observe time jumps in the order of seconds after booting our robots until all the PCs settle. We try to not run any code in the first few minutes after boot, but sometimes you need to start quickly. On the other hand, some bag files can be pretty short, so if you set the threshold too high, looping the short bags would not trigger the reset. Maybe the default could depend on use_sim_time? I'd suggest using a lower value in sim time (which should rarely jump back except for re-playing the same bag), and a higher value in real time (minutes? hours? days???).

Also, there's question whether to automatically reset only on jumps back, or also on jumps forward. Imagine you play two bags, one recorded at one day, and the other recorded the next day. There would be a +1 day jump. But as #160 mentions, sometimes the users do not want to reset when this happens. My view on this is that if the threshold is configurable, it should apply both ways. If the user knows his/her system has large time jumps, he would have to adjust the parameter accordingly (the parameter can be mentioned in the log message saying that the filter was reset).

[mintar]

Just some ideas: How does TF do it? You always get those "time jumped backwards" warnings from TF, so they must be doing something similar. Another idea: instead of using use_sim_time directly, you could check if ros::time::now() jumped backwards (even without threshold). That should only happen on rosbags.

Regarding the large time jumps on real robots - I think that will cause problems anyway, because of the large dt in the equation, so perhaps resetting the filter (or ignoring the measurement) would be a good idea anyway. I think there already is some parameter to that effect? I don't quite remember.

[peci1]

I've created #165 with a possible solution to this issue.