RTLSapp_Ubuntu/remote_client.py at master · vyhuy2004/RTLSapp_Ubuntu · 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
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
import signal
#from functools import partial
import time
import sys
import cv2

import argparse
import numpy as np
from multiprocessing import Manager, Process
import threading
import socket

from TrackerModule.tracker import LocationTracker
from CameraModule.camera import CameraController
from pid import PID
from gui import GUI
from VideoGet import VideoGet

#cap = cv2.VideoCapture(0)

def core_signal_handler(sig, frame):
    sys.exit()

def pid_signal_handler(sig, frame):
    sys.exit()

def core_process(output_path, pan, tilt, pan_error, tilt_error, pid_active):
        s = socket.socket()
        s.connect(('192.168.70.4', 8802))
        print(s.recv(1024))
        signal.signal(signal.SIGINT, core_signal_handler)
        video_getter = VideoGet(remote=True).start()
        tracker = LocationTracker(remote=True, address=('', 8801))
        camera = CameraController(camera_position=[0.03,2.82,1.33], remote=True, socket=s)

        camera.init(refpoint_world=[0.85, 0.0, 1.35],
                    use_pid = True, pid_active=pid_active, pan_error=pan_error, tilt_error=tilt_error)

        servo_thread = threading.Thread(target=camera.CameraRotation_Thread, args=(pan, tilt, video_getter))
        servo_thread.start()
        gui = GUI(output_path, camera, tracker, video_getter, remote=True, socket=s)
        gui.root.mainloop()
        servo_thread.join()
        #s.close()

def pid_process(output, p, i, d, error, pid_active):
    signal.signal(signal.SIGINT, pid_signal_handler)

    pid_control = PID(p.value, i.value, d.value)
    pid_control.initialize()

    while True:
        if (pid_active.value == True):
            output.value = pid_control.update(error.value)

if __name__== "__main__":
    ap = argparse.ArgumentParser()
    ap.add_argument("-o", "--output", default="./",
        help="path to output directory to store snapshots (default: current folder")
    args = vars(ap.parse_args())

    # start the app
    with Manager() as manager:
        pan_error = manager.Value("i", 0)
        tilt_error = manager.Value("i", 0)
        pan = manager.Value("i", 0)
        tilt = manager.Value("i", 0)

        panP = manager.Value("f", 0.188) #188
        panI = manager.Value("f", 0.0)
        panD = manager.Value("f", 0.0)

        tiltP = manager.Value("f", 0.1558) # 1558
        tiltI = manager.Value("f", 0.0)
        tiltD = manager.Value("f", 0.0)

        pid_active = manager.Value("b", False)

        processCore = Process(target=core_process,
                              args=(args["output"], pan, tilt, pan_error, tilt_error, pid_active))
        processPanPID = Process(target=pid_process,
                              args=(pan, panP, panI, panD, pan_error, pid_active))
        processTiltPID = Process(target=pid_process,
                              args=(tilt, tiltP, tiltI, tiltD, tilt_error, pid_active))

        processCore.start()
        processPanPID.start()
        processTiltPID.start()

        processCore.join()
        processPanPID.join()
        processTiltPID.join()