RobotZed/main.py at master · TeaPackCZ/RobotZed · 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
from Logger import Logger
import signal
import sys
from time import sleep
import zmq

class master:
    def __init__(self):
        self.logger = Logger("mainLog")

        self.InPorts = ["10105","10201"]
        self.OutPortGPS = "10110"
        self.OutPortMBED = "10210"

        self.enabled = True
        signal.signal(signal.SIGINT, self.sigINT_Handler)
        zMQC = zmq.Context()

        self.subscriber = zMQC.socket(zmq.SUB)

        for port in self.InPorts:
            self.subscriber.connect('tcp://127.0.0.1:'+port)
            self.logger.save_line("Binded to port: " + port)
        self.subscriber.setsockopt(zmq.SUBSCRIBE, b"")

        self.publisherGPS = zMQC.socket(zmq.PUB)
        self.publisherGPS.bind('tcp://127.0.0.1:'+self.OutPortGPS)
        self.logger.save_line("PublisherGPS connected to port: " + self.OutPortGPS)

        self.publisherMBED = zMQC.socket(zmq.PUB)
        self.publisherMBED.bind('tcp://127.0.0.1:'+self.OutPortMBED)
        self.logger.save_line("PublisherMBED connected to port: " + self.OutPortMBED)

        sleep(0.5)

    def sigINT_Handler(self, signal, frame):
        print("\nMaster detected SigINT signal")
        self.logger.save_line("Signal SigINT detected")
        self.enabled = False
        for port in self.InPorts:
            self.subscriber.disconnect('tcp://127.0.0.1:'+port)
            self.logger.save_line("SUB disconnected from local port: " + port)

        self.publisherGPS.disconnect('tcp://127.0.0.1:'+self.OutPortGPS)
        self.logger.save_line("PUB_GPS disconnected from local port: " + self.OutPortGPS)

        self.publisherMBED.disconnect('tcp://127.0.0.1:'+self.OutPortMBED)
        self.logger.save_line("PUB_MBED disconnected from local port: " + self.OutPortMBED)

        sys.exit(0)

    def SetWaypoint(self,data):
        self.publisherGPS.send_string(data)

    def checkAll(self):
        msg = ""
        oldmsg = " "
        waitingMSG = self.subscriber.poll(100,zmq.POLLIN)
        print("Waiting msgs: " +str(waitingMSG))
        while(waitingMSG > 0):
            msg = self.subscriber.recv_string()
            self.parseMessage(msg)
            waitingMSG = self.subscriber.poll(100,zmq.POLLIN)

    def parseMessage(self,data):
        return 0

    def initRobot(self):
        sleep(1)
        self.publisherGPS.send_string("ID:GPS1;RESTART:COLDSTART")
        self.publisherGPS.send_string("ID:GPS2;RESTART:COLDSTART")

    def Game(self):
        waypoints = []

        #Load waypoints
        waypoints.append("ID:MAIN;LON:14.1471803;LAT:49.3101150;ERR:3")
        waypoints.append("ID:MAIN;LON:14.1511122;LAT:49.3083453;ERR:3")

        self.initRobot()
        #WaitForStartingWindow (if any)
        for waypoint in waypoints:
            self.SetWaypoint(waypoint)
            atWaypoint = False
            while(not atWaypoint):
                self.checkAll()
                sleep(0.1)
        #       updateAll
        #   MarkWaypoint/Goal
        #Play4Winner
        #Switch to manual

M = master()
M.Game()