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)
Пример #2
0
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)
Пример #3
0
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)