forked from pioneers/forseti
/
Flagger.py
executable file
·96 lines (76 loc) · 2.69 KB
/
Flagger.py
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
95
96
#!/usr/bin/env python2.7
import lcm
import threading
import time
from Forseti import Health
from Forseti import Tag
from Forseti import GoalBoxes
from Forseti import Flags
'''
Flagger subscribes to Goals/Goals and publishes to MaestroDriver/Flags to visualize it.
TODO(ajc): health
'''
class Flagger(object):
def __init__(self, lcm):
self.lcm = lcm
subscription = self.lcm.subscribe("Goals/Goals", self.msg_handler)
self._read_thread = threading.Thread(target=self._read_loop)
self._read_thread.daemon = True
self._write_thread = threading.Thread(target=self._write_loop)
self._write_thread.daemon = True
self._health_thread = threading.Thread(target=self._health_loop)
self._health_thread.daemon = True
self.startTime = time.time()
self.flagsLock = threading.Lock()
self.flags = Flags()
self.flags.goals = [[0,0,0,0,0], [0,0,0,0,0], [0,0,0,0,0], [0,0,0,0,0]]
self.boxvalsToFlagPos = {0:0, 1:1, 2:-1}
def read_run(self):
self._read_thread.run()
def write_start(self):
self._write_thread.start()
def health_start(self):
self._health_thread.start()
def _read_loop(self):
try:
while True:
self.lcm.handle()
except:
pass
def _health_loop(self):
while True:
msg = Health()
msg.uptime = time.time() - self.startTime
self.lcm.publish("Flagger/Health", msg.encode())
time.sleep(0.25)
def _write_loop(self):
while True:
self.flagsLock.acquire()
self.lcm.publish("MaestroDriver/Flags", self.flags.encode())
#self.lcm.publish("Goals/Goals", self.goalboxes.encode())
self.flagsLock.release()
time.sleep(0.1)
def msg_handler(self, channel, data):
if channel == "Goals/Goals":
msg = GoalBoxes.decode(data)
print ("received Goals!")
# print("reader=" + msg.reader)
# print("tagId=" + str(msg.tagId))
# add to goalboxes
self.flagsLock.acquire()
for goal in range(0, 4):
for box in range(0, 5):
val = msg.goals[goal][box]
print("received goal=" + str(goal) + ", box=" + str(box) + ", value=" + str(val))
self.flags.goals[goal][box] = self.boxvalsToFlagPos[val]
self.flagsLock.release()
def main():
print "Starting Flagger.py ..."
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=1")
print "LCM Initialized!"
gc = Flagger(lc)
gc.health_start()
gc.write_start()
gc.read_run()
if __name__ == '__main__':
main()