class App(object):
# init creates all objects, draws initUI, and starts all loops (including serial thread)
# closeWin first prompts the user if they really want to close, then ends serial thread and tkinter
# restartAll restarts all objects that store map data, allowing history to be wiped without hard reset
# setDisplayMode link to data.setDisplayMode function (prevents restart from breaking reference)
# setRelDestination link to data.setRelDestination function (prevents restart from breaking reference)
# saveImage tells data object to capture the current map and save as a png
# getScanData pulls LIDAR data directly from the serial port and does preliminary processing
# updateData calls getScanData, and updates the slam object and data matrix with this data, and loops to itself
# updateMap draws a new map, using whatever data is available, and loops to itself
def __init__(self, master):
self.master = master # root tk window
self.master.protocol("WM_DELETE_WINDOW", self.closeWin) # control what happens when a window is closed externally (e.g. by the 'x')
self.master.wm_title("Aerospace Robotics LIDAR Viewer") # name window
self.master.geometry('+100+50') # position window 100,100 pixels from top-left corner
# physical objects
self.robot = DaguRover5()
self.laser = RPLIDAR(DIST_MIN, DIST_MAX)
# initialize serial object, prompting user for input if required
self.statusQueue = Queue() # status of serial thread # FIFO queue by default
self.RXQueue = Queue() # data from serial to root # FIFO queue by default
self.TXQueue = Queue() # data from root to serial # FIFO queue by default
self.serThread = SerialThread(self.laser, self.statusQueue, self.RXQueue, self.TXQueue) # initialize thread object
# initialize root variables
self.statusStr = StringVar() # status of serThread
self.restarting = False # are we in the process of soft restarting?
self.paused = False # should the loops be doing nothing right now?
# helper objects
self.data = DataMatrix(**KWARGS) # handle map data
self.slam = Slam(self.robot, self.laser, **KWARGS) # do slam processing
if FAST_MAPPING:
# create the OpenCV window
self.regionFrame = SlamShow(CV_IMG_SIZE, CV_IMG_RES_PIX_PER_MM, 'SLAM Rover: Hit ESC to quit')
# create Tkinter control frames
self.statusFrame = EntryFrame(self.master, self.robot, self.closeWin, self.restartAll, self.saveImage, \
self.serThread.getACK, self.serThread.resetACK, self.TXQueue, self.statusStr, twoLines=True)
self.insetFrame = InsetFrame(self.master, self.data.getInsetMatrix(),
sendCommand=self.statusFrame.sendCommand, setRelDestination=self.setRelDestination, **KWARGS)
# pack frame
self.statusFrame.pack(side='bottom', fill='x')
self.insetFrame.pack(side='left', fill='both', expand=True)
else:
# create all the pretty stuff in the Tkinter window
self.statusFrame = EntryFrame(self.master, self.robot, self.closeWin, self.restartAll, self.saveImage, \
self.serThread.getACK, self.serThread.resetACK, self.TXQueue, self.statusStr)
self.regionFrame = RegionFrame(self.master, self.data.getMapMatrix(), setDisplayMode=self.setDisplayMode, **KWARGS)
self.insetFrame = InsetFrame(self.master, self.data.getInsetMatrix(),
sendCommand=self.statusFrame.sendCommand, setRelDestination=self.setRelDestination, **KWARGS)
# pack frames
self.statusFrame.pack(side='bottom', fill='x')
self.regionFrame.pack(side='left', fill='both', expand=True)
self.insetFrame.pack(side='right', fill='both', expand=True)
# Start loops
self.serThread.start() # begin fetching data from serial port and processing it
self.updateData() # pull data from queue, put into data matrix
self.updateMap() # draw new data matrix
self.statusFrame.autosendCommand() # check for user input and automatically send it
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 restartAll(self, funcStep=0): # two-part initialization to be re-done at soft reset
if funcStep == 0:
self.restarting = True # stop currently running loops
self.master.after(2000, lambda: self.restartAll(funcStep=1)) # let processor wrap things up elsewhere # should be smarter
return
elif funcStep == 1:
with self.RXQueue.mutex: self.RXQueue.queue.clear() # empty incoming data queue
self.data = DataMatrix(**KWARGS)
self.slam = Slam(self.robot, self.laser, **KWARGS)
self.restarting = False
self.updateData() # pull data from queue, put into data matrix
self.updateMap() # draw new data matrix
def setDisplayMode(self, *args, **kwargs):
return self.data.setDisplayMode(*args, **kwargs)
def setRelDestination(self, *args, **kwargs):
return self.data.setRelDestination(*args, **kwargs)
def saveImage(self): # 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 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 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 updateMap(self, loop=True):
if not self.paused and not self.dataInit: # wait until first data update to update map
# draw map using slam data # 16ms
self.data.drawBreezyMap(self.slam.getBreezyMap())
self.data.drawInset() # new relative map # 7ms
self.insetFrame.updateMap(self.data.get_robot_rel(), self.data.getRelDestination(), self.data.getInsetMatrix()) # 25ms
if FAST_MAPPING:
self.regionFrame.displayMap(self.data.getMapArray((CV_IMG_SIZE,CV_IMG_SIZE))) # 36ms
self.regionFrame.displayRobot(self.data.get_robot_abs())
if self.regionFrame.refresh() == 27: self.closeWin() # ESC key pressed
else:
self.regionFrame.updateMap(self.data.get_robot_rel(), self.data.getDestination(), self.data.getMapMatrix())
if loop and not self.restarting: self.master.after(MAP_RATE, self.updateMap)
class App(object):
# init creates all objects, draws initUI, and starts all loops (including serial thread)
# closeWin first prompts the user if they really want to close, then ends serial thread and tkinter
# restartAll restarts all objects that store map data, allowing history to be wiped without hard reset
# setDisplayMode link to data.setDisplayMode function (prevents restart from breaking reference)
# saveImage tells data object to capture the current map and save as a png
# getScanData pulls LIDAR data directly from the serial port and does preliminary processing
# updateData calls getScanData, and updates the slam object and data matrix with this data, and loops to itself
# updateMap draws a new map, using whatever data is available, and loops to itself
def __init__(self, master):
self.master = master # root tk window
self.master.protocol("WM_DELETE_WINDOW", self.closeWin) # control what happens when a window is closed externally (e.g. by the 'x')
self.master.wm_title("Aerospace Robotics LIDAR Viewer") # name window
self.master.geometry('+100+50') # position window 100,100 pixels from top-left corner
# physical objects
self.robot = DaguRover5()
self.laser = RPLIDAR(DIST_MIN, DIST_MAX)
# get data from log file
self.logFileIndex = 0
self.logFileContents = [[int(el) for el in rawline.split(' ')] for rawline in logFile.read().strip().split('\n')]
# initialize root variables
self.statusStr = StringVar() # status of serThread
self.restarting = False # are we in the process of soft restarting?
self.paused = False # should the loops be doing nothing right now?
# helper objects
self.data = DataMatrix(**KWARGS) # handle map data
self.slam = Slam(self.robot, self.laser, **KWARGS) # do slam processing
if FAST_MAPPING:
# create the OpenCV window
self.regionFrame = SlamShow(CV_IMG_SIZE, CV_IMG_RES_PIX_PER_MM, 'SLAM Rover: Hit ESC to quit')
# create Tkinter control frames
self.statusFrame = StatusFrame(self.master, self.closeWin, self.restartAll, self.saveImage, self.statusStr, twoLines=True)
self.insetFrame = InsetFrame(self.master, self.data.getInsetMatrix(), **KWARGS)
# pack frame
self.statusFrame.pack(side='bottom', fill='x')
self.insetFrame.pack(side='left', fill='both', expand=True)
else:
# create all the pretty stuff in the Tkinter window
self.statusFrame = StatusFrame(self.master, self.closeWin, self.restartAll, self.saveImage, self.statusStr)
self.regionFrame = RegionFrame(self.master, self.data.getMapMatrix(), setDisplayMode=self.setDisplayMode, **KWARGS)
self.insetFrame = InsetFrame(self.master, self.data.getInsetMatrix(), **KWARGS)
# pack frames
self.statusFrame.pack(side='bottom', fill='x')
self.regionFrame.pack(side='left', fill='both', expand=True)
self.insetFrame.pack(side='right', fill='both', expand=True)
# Start loops
self.updateData() # pull data from queue, put into data matrix
self.updateMap() # draw new data matrix
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 restartAll(self, funcStep=0): # two-part initialization to be re-done at soft reset
if funcStep == 0:
self.restarting = True # stop currently running loops
self.master.after(1000, lambda: self.restartAll(funcStep=1)) # let processor wrap things up elsewhere # should be smarter
return
elif funcStep == 1:
self.logFileIndex = 0 # read from beginning of log file
self.data = DataMatrix(**KWARGS)
self.slam = Slam(self.robot, self.laser, **KWARGS)
self.restarting = False
self.updateData() # pull data from queue, put into data matrix
self.updateMap() # draw new data matrix
def setDisplayMode(self, *args, **kwargs):
return self.data.setDisplayMode(*args, **kwargs)
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 getScanData(self):
scan = self.logFileContents[self.logFileIndex]
self.slam.currEncPos = scan[0:3]
self.points = zip(scan[3:], range(self.laser.SCAN_SIZE)) # distance, angle tuples
self.logFileIndex += 1
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 updateMap(self, loop=True):
if not self.paused and not self.dataInit: # wait until first data update to update map
# draw map using slam data # 16ms
self.data.drawBreezyMap(self.slam.getBreezyMap())
self.data.drawInset() # new relative map # 7ms
self.insetFrame.updateMap(self.data.get_robot_rel(), self.data.getRelDestination(), self.data.getInsetMatrix()) # 25ms
if FAST_MAPPING:
self.regionFrame.displayMap(self.data.getMapArray((CV_IMG_SIZE,CV_IMG_SIZE))) # 36ms
self.regionFrame.displayRobot(self.data.get_robot_abs())
if self.regionFrame.refresh() == 27: self.closeWin() # ESC key pressed
else:
self.regionFrame.updateMap(self.data.get_robot_rel(), self.data.getDestination(), self.data.getMapMatrix())
if loop and not self.restarting: self.master.after(MAP_RATE, self.updateMap)
class App(object):
# init creates all objects, draws initUI, and starts all loops (including serial thread)
# closeWin first prompts the user if they really want to close, then ends serial thread and tkinter
# restartAll restarts all objects that store map data, allowing history to be wiped without hard reset
# setDisplayMode link to data.setDisplayMode function (prevents restart from breaking reference)
# setRelDestination link to data.setRelDestination function (prevents restart from breaking reference)
# saveImage tells data object to capture the current map and save as a png
# getScanData pulls LIDAR data directly from the serial port and does preliminary processing
# updateData calls getScanData, and updates the slam object and data matrix with this data, and loops to itself
# updateMap draws a new map, using whatever data is available, and loops to itself
def __init__(self, master):
self.master = master # root tk window
self.master.protocol("WM_DELETE_WINDOW", self.closeWin) # control what happens when a window is closed externally (e.g. by the 'x')
self.master.wm_title("Aerospace Robotics LIDAR Viewer") # name window
self.master.geometry('+100+50') # position window 100,100 pixels from top-left corner
# physical objects
self.robot = DaguRover5()
self.laser = RPLIDAR(DIST_MIN, DIST_MAX)
# initialize serial object, prompting user for input if required
self.statusQueue = Queue() # status of serial thread # FIFO queue by default
self.RXQueue = Queue() # data from serial to root # FIFO queue by default
self.TXQueue = Queue() # data from root to serial # FIFO queue by default
self.serThread = SerialThread(self.laser, self.statusQueue, self.RXQueue, self.TXQueue) # initialize thread object
# initialize root variables
self.statusStr = StringVar() # status of serThread
self.restarting = False # are we in the process of soft restarting?
self.paused = False # should the loops be doing nothing right now?
# helper objects
self.data = DataMatrix(**KWARGS) # handle map data
self.slam = Slam(self.robot, self.laser, **KWARGS) # do slam processing
if FAST_MAPPING:
# create the OpenCV window
self.regionFrame = SlamShow(CV_IMG_SIZE, CV_IMG_RES_PIX_PER_MM, 'SLAM Rover: Hit ESC to quit')
# create Tkinter control frames
self.statusFrame = EntryFrame(self.master, self.robot, self.closeWin, self.restartAll, self.saveImage, \
self.serThread.getACK, self.serThread.resetACK, self.TXQueue, self.statusStr,
twoLines=True, setDisplayMode=self.setDisplayMode, **KWARGS)
self.insetFrame = InsetFrame(self.master, self.data.getInsetMatrix(),
sendCommand=self.statusFrame.sendCommand, setRelDestination=self.setRelDestination, **KWARGS)
# pack frame
self.statusFrame.pack(side='bottom', fill='x')
self.insetFrame.pack(side='left', fill='both', expand=True)
else:
# create all the pretty stuff in the Tkinter window
self.statusFrame = EntryFrame(self.master, self.robot, self.closeWin, self.restartAll, self.saveImage, \
self.serThread.getACK, self.serThread.resetACK, self.TXQueue, self.statusStr,
setDisplayMode=self.setDisplayMode, **KWARGS)
self.regionFrame = RegionFrame(self.master, self.data.getMapMatrix(), **KWARGS)
self.insetFrame = InsetFrame(self.master, self.data.getInsetMatrix(),
sendCommand=self.statusFrame.sendCommand, setRelDestination=self.setRelDestination, **KWARGS)
# pack frames
self.statusFrame.pack(side='bottom', fill='x')
self.regionFrame.pack(side='left', fill='both', expand=True)
self.insetFrame.pack(side='right', fill='both', expand=True)
# Start loops
self.serThread.start() # begin fetching data from serial port and processing it
self.updateData() # pull data from queue, put into data matrix
self.updateMap() # draw new data matrix
self.statusFrame.autosendCommand() # check for user input and automatically send it
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 restartAll(self, funcStep=0): # two-part initialization to be re-done at soft reset
if funcStep == 0:
self.restarting = True # stop currently running loops
self.master.after(2000, lambda: self.restartAll(funcStep=1)) # let processor wrap things up elsewhere # should be smarter
return
elif funcStep == 1:
with self.RXQueue.mutex: self.RXQueue.queue.clear() # empty incoming data queue
self.data = DataMatrix(**KWARGS)
self.slam = Slam(self.robot, self.laser, **KWARGS)
self.restarting = False
self.updateData() # pull data from queue, put into data matrix
self.updateMap() # draw new data matrix
def setDisplayMode(self, *args, **kwargs):
return self.data.setDisplayMode(*args, **kwargs)
def setRelDestination(self, *args, **kwargs):
return self.data.setRelDestination(*args, **kwargs)
def saveImage(self): # 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 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 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 updateMap(self, loop=True):
if not self.paused and not self.dataInit: # wait until first data update to update map
# draw map using slam data # 16ms
self.data.drawBreezyMap(self.slam.getBreezyMap())
self.data.drawInset() # new relative map # 7ms
self.insetFrame.updateMap(self.data.get_robot_rel(), self.data.getRelDestination(), self.data.getInsetMatrix()) # 25ms
if FAST_MAPPING:
self.regionFrame.displayMap(self.data.getMapArray((CV_IMG_SIZE,CV_IMG_SIZE))) # 36ms
self.regionFrame.displayRobot(self.data.get_robot_abs())
if self.regionFrame.refresh() == 27: self.closeWin() # ESC key pressed
else:
self.regionFrame.updateMap(self.data.get_robot_rel(), self.data.getDestination(), self.data.getMapMatrix())
if loop and not self.restarting: self.master.after(MAP_RATE, self.updateMap)