"Sanity Checker" - handle jumping & disappearing of tracked objects

[jsren]

As has been discussed previously, it's clear that we need some manner of system which can sanitize/normalise the raw data coming frame-by-frame from the vision system. While this could be done by the planner folk, it seems perhaps more convenient for those with a better understanding of the vision (such as how reporting top plates works) to write such a system.

[jsren]

@DimpleG: What's the status on the Kalman filter? Any idea on applicability here?

[LinasKo]

Currently we don't have a way to check what result a shorter queue would provide. I suggest augmenting the getters in robot_instance with optional median size parameters for every individual call for more up-to-date results.

We could then build some kind sanity checker based on that. (e.g. assume that angles change radically if robot is mislocated. If they don't use smaller median)

• [x] optional median size

I've added an automatic median selector which, whenever an angle is requested, checks if the last provided point (x, y, angle) is within set distance and rotation thresholds and returns the first found (or max==queue_size) median value. It is turned on by default, but not yet tested.

• [x] Median selector
• [x] Test it!

Tested the median selector. It seems to give live data more often than we had before. Although everything's still jumpy.