def on_vio(self, vio, time):
if self.state == STATE_WAIT:
pos_diff, rot_diff = pose.pose_diff(vio, self.last_vio)
time_diff = (time - self.last_time).total_seconds()
if pos_diff > MOVE_THRESH or rot_diff > ROT_THRESH or time_diff > TIME_THRESH:
self.state = STATE_FINDTAG
self.hud.update(color=COLOR_FINDTAG)
def vio_filter(vio, client_id):
global vio_state
vio_pose_last = vio_state.get(client_id)
if vio_pose_last is None:
vio_state[client_id] = vio
return False
pos_diff, rot_diff = pose.pose_diff(vio, vio_pose_last)
vio_state[client_id] = vio
if pos_diff > MOVE_THRESH or rot_diff > ROT_THRESH:
return False
return True