inno_robo_hack/app.py at master · egorchepiga/inno_robo_hack · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
import numpy as np
import cv2 as cv

from Robot import Robot
from automotive import *

HOST = '10.127.50.72'
PORT = 10099

Fred = Robot(HOST,PORT)


while True:
    sensors_data = Fred.get_sensor_data()
    left, right, disp = sensors_data['left'], sensors_data['right'], sensors_data['disp']
    mask_brown, mask_chess = sensors_data['mask_brown'], sensors_data['mask_chess']
    XYZ = sensors_data['XYZ']

    Z = XYZ[..., 2]
    Z = Z.astype(np.uint8)
    cv.imshow('disp', cv.applyColorMap(disp.astype(np.uint8), cv.COLORMAP_RAINBOW))


    cv.imshow('stacked', np.hstack((left, right)))

    key = cv.waitKey(10)
    try:
        if key == ord('q'):
            break
        elif key == ord('a'):
            Fred.rotate_head(yaw=5)
        elif key == ord('d'):
            Fred.rotate_head(yaw=-5)
        elif key == ord('s'):
            Fred.rotate_head(pitch=5)
        elif key == ord('w'):
            Fred.rotate_head(pitch=-5)
        elif key == ord('0'):
            watch_on_zero(Fred)
        elif key == ord('9'):
            watch_on_target(Fred)
        elif key == ord('8'):
            watch_on_box(Fred)
        elif key == ord('7'):
            test(Fred)
    except BrokenPipeError:
        print('broken pipe')
        Fred.start_socket_shit()