[0., 0., 1., 0., 0.]])
filter = Filtering(Rvel, Rcam, thym, Hvel, Hcam, Ts)
thym.go = 1
# Begin testbench
pos_cam = False
update_cam = False
while thym.go:
tps1 = time.monotonic()
if thym.state == 'ASTOLFI':
thym.ASTOLFI(th, Ts, filter, pos_cam, update_cam)
elif thym.state == 'TURN':
thym.TURN(th, Ts)
elif thym.state == 'LOCAL':
thym.LOCAL(th, Ts, filter, pos_cam)
elif thym.state == 'INIT':
thym.INIT(global_path, Init_pos)
tps2 = time.monotonic()
Ts = tps2 - tps1
print(thym.Pos)
# check if we arrive at the goal
if thym.p < 3 and thym.node == len(thym.global_path) - 2:
th.set_var("motor.left.target", 0)
th.set_var("motor.right.target", 0)
