def updateData(self, init=True):
self.dataInit = init
if not self.paused and self.logFileIndex < len(self.logFileContents):
self.statusStr.set('Scan number {0:4d} from {1:s}'.format(
self.logFileIndex, logFileName))
# pull data from log file
self.getScanData()
# update robot position
if init:
self.slam.prevEncPos = self.slam.currEncPos # set both values the first time through
self.data.getRobotPos(
self.slam.updateSlam(self.points),
init=init) # send data to slam to do stuff # 15ms
self.data.drawPointMap(self.points) # draw map using scan points
if init: init = False # initial data gathered successfully
elif self.logFileIndex >= len(self.logFileContents):
self.statusStr.set(
paddedStr("End of data.", len(self.statusStr.get())))
if not self.restarting:
self.master.after(DATA_RATE, lambda: self.updateData(init=init))
else:
self.statusStr.set(
paddedStr("Restarting...", len(
self.statusStr.get()))) # if loop ends, we're restarting
def updateData(self, init=True):
self.dataInit = init
if not self.paused:
try:
status = self.statusQueue.get_nowait(
) # do this before anything else
except QueueEmpty:
pass
else:
self.statusStr.set(paddedStr(status,
len(self.statusStr.get())))
if self.RXQueue.qsize(
) > (2 if init else 1) * NUM_SAMP: # ready to pull new scan data
# pull data from serial thread via RXQueue, ignoring the first scan, which is incomplete
self.getScanData(repeat=init) # 2ms
# update robot position
if init:
self.slam.prevEncPos = self.slam.currEncPos # set both values the first time through
self.data.getRobotPos(
self.slam.updateSlam(self.points),
init=init) # send data to slam to do stuff # 15ms
self.data.drawPointMap(
self.points) # draw map using scan points
if init: init = False # initial data gathered successfully
if not self.restarting:
self.master.after(DATA_RATE, lambda: self.updateData(init=init))
else:
self.statusStr.set(
paddedStr("Restarting...", len(
self.statusStr.get()))) # if loop ends, we're restarting
def closeWin(self):
self.paused = True
self.statusStr.set(paddedStr("Paused.", len(self.statusStr.get())))
if askokcancel("Quit?", "Are you sure you want to quit?"):
self.statusStr.set(paddedStr("Stopping",len(self.statusStr.get()))) # keep length of label constant
print("Closing program")
self.master.quit() # kills interpreter (necessary for some reason)
else: self.paused = False
def closeWin(self):
self.paused = True
self.statusStr.set(paddedStr("Paused.", len(self.statusStr.get())))
if askokcancel("Quit?", "Are you sure you want to quit?"):
self.statusStr.set(paddedStr("Stopping.", len(self.statusStr.get())))
print("Shutting down LIDAR")
self.serThread.stop() # tell serial thread to stop running
print("Closing program")
self.master.quit() # kills interpreter (necessary for some reason)
else: self.paused = False
def closeWin(self):
self.paused = True
self.statusStr.set(paddedStr("Paused.", len(self.statusStr.get())))
if askokcancel("Quit?", "Are you sure you want to quit?"):
self.statusStr.set(paddedStr("Stopping.", len(self.statusStr.get())))
print("Shutting down LIDAR")
self.serThread.stop() # tell serial thread to stop running
print("Closing program")
self.master.quit() # kills interpreter (necessary for some reason)
else: self.paused = False
def saveImage(self, step=0): # function prototype until data is initialized
self.paused = True
self.statusStr.set(paddedStr("Saving image. Close to resume.", len(self.statusStr.get()))) # keep length of label constant
self.master.update() # force statusStr update
self.updateMap(loop=False) # make sure we save the newest map
self.data.saveImage()
self.paused = False
def updateData(self, init=True):
self.dataInit = init
if not self.paused and self.logFileIndex < len(self.logFileContents):
self.statusStr.set('Scan number {0:4d} from {1:s}'.format(self.logFileIndex, logFileName))
# pull data from log file
self.getScanData()
# update robot position
if init: self.slam.prevEncPos = self.slam.currEncPos # set both values the first time through
self.data.getRobotPos(self.slam.updateSlam(self.points), init=init) # send data to slam to do stuff # 15ms
self.data.drawPointMap(self.points) # draw map using scan points
if init: init = False # initial data gathered successfully
elif self.logFileIndex >= len(self.logFileContents):
self.statusStr.set(paddedStr("End of data.", len(self.statusStr.get())))
if not self.restarting: self.master.after(DATA_RATE, lambda: self.updateData(init=init))
else: self.statusStr.set(paddedStr("Restarting...", len(self.statusStr.get()))) # if loop ends, we're restarting
def updateData(self, init=True):
self.dataInit = init
if not self.paused:
try: status = self.statusQueue.get_nowait() # do this before anything else
except QueueEmpty: pass
else: self.statusStr.set(paddedStr(status, len(self.statusStr.get())))
if self.RXQueue.qsize() > (2 if init else 1)*NUM_SAMP: # ready to pull new scan data
# pull data from serial thread via RXQueue, ignoring the first scan, which is incomplete
self.getScanData(repeat=init) # 2ms
# update robot position
if init: self.slam.prevEncPos = self.slam.currEncPos # set both values the first time through
self.data.getRobotPos(self.slam.updateSlam(self.points), init=init) # send data to slam to do stuff # 15ms
self.data.drawPointMap(self.points) # draw map using scan points
if init: init = False # initial data gathered successfully
if not self.restarting: self.master.after(DATA_RATE, lambda: self.updateData(init=init))
else: self.statusStr.set(paddedStr("Restarting...", len(self.statusStr.get()))) # if loop ends, we're restarting
def getScanData(self, repeat=False):
self.points = [] # wipe old data before writing new data
while True:
# Make sure there's actually data to get
try: queueItem = self.RXQueue.get_nowait()
except QueueEmpty: # something's wrong
self.statusStr.set(paddedStr("RXQueue empty. Send 'l' iff LIDAR stopped.", len(self.statusStr.get())))
return # stop because no data to read
else:
if isinstance(queueItem[0], float): # scan data
self.points.append(queueItem)
elif isinstance(queueItem[1], int): # encoder data
self.slam.currEncPos = queueItem
break # encoder data signals new scan
else:
print("RXQueue broken (something weird happened...)")
if repeat: self.getScanData()
def getScanData(self, repeat=False):
self.points = [] # wipe old data before writing new data
while True:
# Make sure there's actually data to get
try: queueItem = self.RXQueue.get_nowait()
except QueueEmpty: # something's wrong
self.statusStr.set(paddedStr("RXQueue empty. Send 'l' iff LIDAR stopped.", len(self.statusStr.get())))
return # stop because no data to read
else:
if isinstance(queueItem[0], float): # scan data
self.points.append(queueItem)
elif isinstance(queueItem[1], int): # encoder data
self.slam.currEncPos = queueItem
break # encoder data signals new scan
else:
print("RXQueue broken (something weird happened...)")
if repeat: self.getScanData()
