def init_vision(heading=0.0):
t = time.monotonic()
v = Vision()
v.chassis = FakeChassis()
# Inject some fake odometry
v.odometry.appendleft(Odometry(0, 0, heading, t - 0.3))
v.odometry.appendleft(Odometry(1, 1, heading, t - 0.2))
v.odometry.appendleft(Odometry(2, 2, heading, t - 0.1))
v.odometry.appendleft(Odometry(3, 3, heading, t))
return v, t
def test_odom_deque_order():
v = Vision()
v.chassis = FakeChassis()
for i in range(5):
time.sleep(0.05)
v.execute()
assert len(v.odometry) > 0
newer_odom = v.odometry.popleft()
for odom in v.odometry:
assert newer_odom.t > odom.t
newer_odom = odom
