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 __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}
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()
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()
