Changeset 3ae2cba


Ignore:
Timestamp:
12/28/10 18:13:16 (3 years ago)
Author:
xkolman2 <xkolman2@…>
Branches:
master
Children:
6bf9bee
Parents:
9c7798c
git-author:
xkolman2 <xkolman2@…> (12/28/10 18:13:16)
git-committer:
xkolman2 <xkolman2@…> (12/28/10 18:13:16)
Message:

add a simple heursitics to decide on which turn to start navigation after getting a new route
-> this might fix the issues with missing the first turn when doing a route lookup while moving

git-svn-id: https://nlp.fi.muni.cz/svn/gps_navigace/trunk@254 0858a4d0-ffff-46e5-938e-62b5ecb34222

Location:
modules
Files:
8 edited

Legend:

Unmodified
Added
Removed
  • modules/mod_onlineServices.py

    ra19cb41 r3ae2cba  
    146146       -> verbatim start and destination will be used in route descritpion, no geocoding 
    147147       outputHandler will be provided with the results + the specified key string""" 
    148     self.routingThread = self.Worker(self, "onlineRoute", (start, destination), outputHandler, key) 
     148    routeRequestSentTimestamp = time.time() # used for measuring how long the route lookup took 
     149    self.routingThread = self.Worker(self, "onlineRoute", (start, destination, routeRequestSentTimestamp), outputHandler, key) 
    149150    self.routingThread.daemon = True 
    150151    self.routingThread.start() 
     
    154155    - Lat Lon pairsversion -> for geocoding the start/destination points (NOT first/last route points) 
    155156       outputHandler will be provided with the results + the specified key string""" 
    156     self.routingThread = self.Worker(self, "onlineRouteLL", (start, destination), outputHandler, key) 
     157    routeRequestSentTimestamp = time.time() # used for measuring how long the route lookup took 
     158    self.routingThread = self.Worker(self, "onlineRouteLL", (start, destination, routeRequestSentTimestamp), outputHandler, key) 
    157159    self.routingThread.daemon = True 
    158160    self.routingThread.start() 
     
    293295 
    294296      elif self.type == "onlineRoute" or self.type == "onlineRouteLL": 
    295         if self.args and len(self.args) == 2: 
    296           (start, destination) = self.args 
     297        if self.args and len(self.args) == 3: 
     298          (start, destination, routeRequestSentTimestamp) = self.args 
    297299          print "worker: routing from",start," to ",destination 
    298300          self.setStatusMessage("online routing in progress...") 
     
    321323          # send the results to the output handler 
    322324          if self.returnResult: # check if our result is expected and should be returned to the oputpt handler 
    323             self.outputHandler(self.key, (directions, startAddress, destinationAddress, startLL, destinationLL)) 
     325            self.outputHandler(self.key, (directions, startAddress, destinationAddress, startLL, destinationLL, routeRequestSentTimestamp)) 
    324326             
    325327      # cleanup 
  • modules/mod_options.py

    r25e85fe r3ae2cba  
    295295       "en en") # TODO: use locale for default language ? 
    296296 
    297     self.addOption("Autostart navigation", "autostartNavigationDefaultOn", 
    298       [('disabled',"Disabled"), 
    299        ('first',"From first point"), 
    300        ('closest',"From closest point")], 
     297    self.addOption("Autostart navigation", "autostartNavigationDefaultOnAutoselectTurn", 
     298      [('disabled',"OFF"), 
     299       ('enabled',"ON")], 
    301300       "Navigation", 
    302        'closest') 
     301       'enabled') 
    303302 
    304303    self.addOption("Point reached distance", "pointReachedDistance", 
  • modules/mod_route.py

    r11f5f60 r3ae2cba  
    4646    ranaModule.__init__(self, m, d) 
    4747    self.route = [] # maybe remove this ? 
     48    self.routeRequestSentTimestamp = None 
    4849    self.pxpyRoute = [] 
    4950    self.directions = [] 
     
    7172    if (message == "clear"): 
    7273      self.route = [] 
     74      self.routeRequestSentTimestamp = None 
    7375      self.pxpyRoute = [] 
    7476      self.directions = [] 
     
    179181      print "Routing %f,%f to %f,%f" % (fromLat, fromLon, toLat, toLon) 
    180182 
    181       # TODO: wait message (would it be needed when using internet routing ?) 
    182183      self.doRoute(fromLat, fromLon, toLat, toLon) 
    183184      self.set('needRedraw', True) # show the new route 
     
    321322    """handle a routing result""" 
    322323    if key == "onlineRoute": 
    323       if len(resultsTupple) == 5: 
    324         (directions, startAddress, destinationAddress, start, destination) = resultsTupple 
     324      if len(resultsTupple) == 6: 
     325        (directions, startAddress, destinationAddress, start, destination, routeRequestSentTimestamp) = resultsTupple 
    325326        # remove any possible prev. route description, so new a new one for this route is created 
    326327        self.text = None 
     
    328329          polyline = directions['Directions']['Polyline']['points'] # the route is encoded as a polyline 
    329330          route = self.decode_line(polyline) # we decode the polyline to a list of points 
    330           self.processAndSaveResults(route, directions, startAddress, destinationAddress, start, destination) 
     331          self.processAndSaveResults(route, directions, startAddress, destinationAddress, start, destination, routeRequestSentTimestamp) 
    331332    elif key == "onlineRouteAdress2Adress": 
    332       if len(resultsTupple) == 5: 
    333         (directions, startAddress, destinationAddress, start, destination) = resultsTupple 
     333      if len(resultsTupple) == 6: 
     334        (directions, startAddress, destinationAddress, start, destination, routeRequestSentTimestamp) = resultsTupple 
    334335        # remove any possible prev. route description, so new a new one for this route is created 
    335336        self.text = None 
     
    337338          polyline = directions['Directions']['Polyline']['points'] # the route is encoded as a polyline 
    338339          route = self.decode_line(polyline) # we decode the polyline to a list of points 
    339           self.processAndSaveResults(route, directions, startAddress, destinationAddress, start, destination) 
    340  
    341     autostart = self.get('autostartNavigationDefaultOn', 'closest') 
    342     if autostart == 'first' or autostart == 'closest': 
     340          self.processAndSaveResults(route, directions, startAddress, destinationAddress, start, destination, routeRequestSentTimestamp) 
     341 
     342    autostart = self.get('autostartNavigationDefaultOnAutoselectTurn', 'enabled') 
     343    if autostart == 'enabled': 
    343344      self.sendMessage('ms:turnByTurn:start:%s' % autostart) 
    344345    self.set('needRedraw', True) 
    345346     
    346   def processAndSaveResults(self, route, directions, startAddress, destinationAddress, start, destination): 
     347  def processAndSaveResults(self, route, directions, startAddress, destinationAddress, start, destination, routeRequestSentTimestamp): 
    347348    """process and save routing results""" 
    348349    self.route = route 
     350    self.routeRequestSentTimestamp = routeRequestSentTimestamp 
    349351    proj = self.m.get('projection', None) 
    350352    if proj: 
     
    484486      self.destinationAddress = destinationAddress 
    485487 
    486     """setup the first step selection menu""" 
    487     menus = self.m.get('menu', None) 
    488     if menus: 
    489       menus.clearMenu('routeSelectFirstStep', "set:menu:currentRouteTools") 
    490       menus.addItem('routeSelectFirstStep', 'step#first', 'generic', 'ms:turnByTurn:start:first|set:menu:None') 
    491       menus.addItem('routeSelectFirstStep', 'step#closest', 'generic', 'ms:turnByTurn:start:closest|set:menu:None') 
     488#    """setup the first step selection menu""" 
     489#    menus = self.m.get('menu', None) 
     490#    if menus: 
     491#      menus.clearMenu('routeSelectFirstStep', "set:menu:currentRouteTools") 
     492#      menus.addItem('routeSelectFirstStep', 'step#first', 'generic', 'ms:turnByTurn:start:first|set:menu:None') 
     493#      menus.addItem('routeSelectFirstStep', 'step#closest', 'generic', 'ms:turnByTurn:start:closest|set:menu:None') 
    492494 
    493495  def update(self): 
     
    667669  def getCurrentDirections(self): 
    668670    # return the current route 
    669     return self.directions 
     671    return (self.directions, self.routeRequestSentTimestamp) 
    670672 
    671673  #from: http://seewah.blogspot.com/2009/11/gpolyline-decoding-in-python.html 
     
    900902        if tbt.enabled(): 
    901903          menus.addItem('currentRouteTools', 'navigation#stop', 'generic', 'turnByTurn:stop|set:menu:None') 
    902           menus.addItem('currentRouteTools', 'navigation#restart', 'generic', 'turnByTurn:stop|set:menu:routeSelectFirstStep') 
     904          menus.addItem('currentRouteTools', 'navigation#restart', 'generic', 'turnByTurn:stop|ms:turnByTurn:start:closest|set:menu:None') 
    903905          self.set('needRedraw', True) # refresh the screen to show the changed button 
    904906        else: 
    905           menus.addItem('currentRouteTools', 'navigation#start', 'generic', 'set:menu:routeSelectFirstStep') 
     907          menus.addItem('currentRouteTools', 'navigation#start', 'generic', 'ms:turnByTurn:start:enabled|set:menu:None') 
    906908 
    907909      menus.addItem('currentRouteTools', 'clear', 'generic', 'route:clear|set:menu:None') 
  • modules/mod_turnByTurn.py

    r9c7798c r3ae2cba  
    5151     
    5252    # is navigation on ? 
    53     if self.enabled(): 
     53    if self.steps: 
    5454      # respect the navigation update interval 
    5555      timestamp = time.time() 
    5656      if timestamp - self.lastNavigationUpdate >= self.navigationUpdateInterval: 
    57         # get/compute/update necessary the values 
    58         pos = self.get('pos', None) # and current position 
    59         (lat1,lon1) = pos 
    60         currentStep = self.getCurrentStep() 
    61         lat2 = currentStep['Point']['coordinates'][1] 
    62         lon2 = currentStep['Point']['coordinates'][0] 
    63         currentDistance = geo.distance(lat1,lon1,lat2,lon2)*1000 # km to m 
    64         self.currentDistance = currentDistance # update current distance 
    65         distance = currentStep['Distance']['meters'] 
    66         pointReachedDistance = int(self.get('pointReachedDistance', 30)) 
    67  
    68         if distance>=currentDistance: 
    69           """this means we reached an optimal distance for saying the message""" 
    70           if self.espeakFirstTrigger == False: 
    71             print "triggering espeak nr. 1" 
    72             plaintextMessage = currentStep['descriptionEspeak'] 
    73             self.sayTurn(plaintextMessage, currentDistance) 
    74             self.espeakFirstTrigger = True # everything has been said :D 
    75         if currentDistance <= pointReachedDistance: 
    76           """this means we reached the point""" 
    77           self.switchToNextStep() # switch to next step 
    78           if self.espeakSecondTrigger == False: 
    79             print "triggering espeak nr. 2" 
    80             # say the message without distance 
    81             plaintextMessage = currentStep['descriptionEspeak'] 
    82             self.sayTurn(plaintextMessage, 0) 
    83             self.markCurrentStepAsVisited() # mark this point as visited 
    84             self.espeakSecondTrigger = True # everything has been said, again :D 
     57        self.lastNavigationUpdate = timestamp 
     58        self.doNavigationUpdate() 
     59 
     60  def doNavigationUpdate(self): 
     61    """do a navigation update""" 
     62    # get/compute/update necessary the values 
     63    pos = self.get('pos', None) # and current position 
     64    (lat1,lon1) = pos 
     65    currentStep = self.getCurrentStep() 
     66    lat2 = currentStep['Point']['coordinates'][1] 
     67    lon2 = currentStep['Point']['coordinates'][0] 
     68    currentDistance = geo.distance(lat1,lon1,lat2,lon2)*1000 # km to m 
     69    self.currentDistance = currentDistance # update current distance 
     70    distance = currentStep['Distance']['meters'] 
     71    pointReachedDistance = int(self.get('pointReachedDistance', 30)) 
     72 
     73    if distance>=currentDistance: 
     74      """this means we reached an optimal distance for saying the message""" 
     75      if self.espeakFirstTrigger == False: 
     76        print "triggering espeak nr. 1" 
     77        plaintextMessage = currentStep['descriptionEspeak'] 
     78        self.sayTurn(plaintextMessage, currentDistance) 
     79        self.espeakFirstTrigger = True # everything has been said :D 
     80    if currentDistance <= pointReachedDistance: 
     81      """this means we reached the point""" 
     82      self.switchToNextStep() # switch to next step 
     83      if self.espeakSecondTrigger == False: 
     84        print "triggering espeak nr. 2" 
     85        # say the message without distance 
     86        plaintextMessage = currentStep['descriptionEspeak'] 
     87        self.sayTurn(plaintextMessage, 0) 
     88        self.markCurrentStepAsVisited() # mark this point as visited 
     89        self.espeakSecondTrigger = True # everything has been said, again :D 
    8590 
    8691 
    8792  def handleMessage(self, message, type, args): 
    8893    if message == 'start': 
    89       self.startTBT() 
    9094      if type == 'ms': 
    91         if args == 'first': 
    92           if self.steps: # are there usable directions ? 
    93             self.currentStepIndex = 0 
    94         elif args == 'closest': 
    95           if self.steps: # are there usable directions ? 
    96             cs = self.getClosestStep() 
    97             id = cs['id'] 
    98             self.currentStepIndex = id 
     95        fromWhere = args 
     96        self.startTBT(fromWhere) 
    9997    elif message == 'stop': 
    10098      self.stopTBT() 
     
    121119          lon = currentStep['Point']['coordinates'][0] 
    122120          (pointX, pointY) = proj.ll2xy(lat, lon) 
    123           print (pointX, pointY) 
    124121          cr.set_source_rgb(1, 0, 0) 
    125122          cr.set_line_width(4) 
     
    130127  def drawScreenOverlay(self, cr): 
    131128    if self.steps: # is there something relevant to draw ? 
    132       proj = self.m.get('projection', None) # we also need the projection module 
    133  
     129     
    134130      # get current step 
    135131      currentStep = self.getCurrentStep() 
     
    148144        cr.set_source_rgba(1, 1, 1, 1) 
    149145        pg = pangocairo.CairoContext(cr) 
    150         # create a layout for your drawing area 
     146        # create a layout for our drawing area 
    151147        layout = pg.create_layout() 
    152148        message = currentStep['descriptionHtml'] 
     
    184180 
    185181  def sayTurn(self,message,distanceInMeters,forceLanguageCode=False): 
     182    """say a text-to-spech message about a turn 
     183       this basically wraps the simple say method from voice and adds some more information, 
     184       like current distance to the turn 
     185       """ 
    186186    voice = self.m.get('voice', None) 
    187187    units = self.m.get('units', None) 
     
    222222      return closestStep 
    223223 
     224  def getStep(self, id): 
     225    """return steps for valid index, None otherwise""" 
     226    maxIndex = len(self.steps) - 1 
     227    if id > maxIndex or id < -(maxIndex+1): 
     228      print "wrong turn index: %d, max index is: %d" % (id, maxIndex) 
     229      return None 
     230    else: 
     231      return self.steps[id] 
     232 
     233  def setStepAsCurrent(self, step): 
     234    """set a given step as current step""" 
     235    id = step['id'] 
     236    self.currentStepIndex = id 
     237 
    224238  def getCurrentStep(self): 
    225239    """return current step""" 
     
    253267      return False 
    254268 
    255   def startTBT(self): 
    256     # start Turn-by-turn navigation 
     269  def startTBT(self, fromWhere='first'): 
     270    """start Turn-by-turn navigation""" 
     271 
     272    # clean up any possible previous navigation data 
     273    self.goToInitialState() 
     274 
     275    """NOTE: turn and step are used interchangably in the documentation""" 
    257276    m = self.m.get('route', None) 
    258277    if m: 
    259       dirs = m.getCurrentDirections() 
     278      (dirs,routeRequestSentTimestamp) = m.getCurrentDirections() 
    260279      if dirs: # is the route nonempty ? 
    261280        self.sendMessage('notification:use at own risk, watch for cliffs, etc.#2') 
     
    266285          self.steps.append(step) 
    267286        self.steps = dirs['Directions']['Routes'][0]['Steps'] 
     287#        if fromWhere == 'first': 
     288#          self.currentStepIndex = 0 
     289#        elif fromWhere == 'closest': 
     290#          cs = self.getClosestStep() 
     291#          id = cs['id'] 
     292#          self.currentStepIndex = id 
     293        # some statistics 
     294        metersPerSecSpeed = self.get('metersPerSecSpeed', None) 
     295        dt = time.time() - routeRequestSentTimestamp 
     296        print "route lookup took: %f s" % dt 
     297        if dt and metersPerSecSpeed: 
     298          dm = dt * metersPerSecSpeed 
     299          print "distance traveled during lookup: %f m" % dm 
     300        """the duration of the road lookup and other variables are currently not used 
     301        in the heuristics but might be added later to make the heursitics more robust""" 
     302 
     303 
     304        """now we decide if we use the closest turn, or the next one, 
     305        as we might be already past it and on our way to the next turn""" 
     306        cs = self.getClosestStep() # get geographically closest step 
     307        pos = self.get('pos', None) # get current position 
     308        pReachedDist = int(self.get('pointReachedDistance', 30)) # get the trigger distance 
     309        nextTurnId = cs['id'] + 1 
     310        nextStep = self.getStep(nextTurnId) 
     311        # check if we have all the data needed for our heuristics 
     312        print "tbt: trying to guess correct step to start navigation" 
     313        if nextStep and pos and pReachedDist: 
     314          (lat,lon) = pos 
     315          (csLat,csLon) = (cs['Point']['coordinates'][1],cs['Point']['coordinates'][0]) 
     316          (nsLat,nsLon) = (nextStep['Point']['coordinates'][1],nextStep['Point']['coordinates'][0]) 
     317          pos2nextStep = geo.distance(lat,lon,nsLat,nsLon)*1000 
     318          pos2currentStep = geo.distance(lat,lon,csLat,csLon)*1000 
     319          currentStep2nextStep = geo.distance(csLat,csLon,nsLat,nsLon)*1000 
     320#          print "pos",(lat,lon) 
     321#          print "cs",(csLat,csLon) 
     322#          print "ns",(nsLat,nsLon) 
     323          print "position to next turn: %f m" % pos2nextStep 
     324          print "position to current turn: %f m" % pos2currentStep 
     325          print "current turn to next turn: %f m" % currentStep2nextStep 
     326          print "turn reached trigger distance: %f m" % pReachedDist 
     327 
     328          if pos2currentStep > pReachedDist: 
     329            """this means we are out of the "capture circle" of the closest step""" 
     330 
     331            """what is more distant, the closest or the next step ?""" 
     332            if pos2nextStep < currentStep2nextStep: 
     333              """we are mosty probably already past the closest step, 
     334              so we switch to the next step at once""" 
     335              print "tbt: already past closest turn, switching to next turn" 
     336              self.setStepAsCurrent(nextStep) 
     337            else: 
     338              """we have probably not yet reached the closest step, 
     339                 so we start navigation from it""" 
     340              print "tbt: closest turn not yet reached" 
     341              self.setStepAsCurrent(cs) 
     342 
     343          else: 
     344            """we are inside the  "capture circle" of the closest step, 
     345            this meens the navigation will trigger the voice message by itself 
     346            and correctly switch to next step 
     347            -> no need to switch to next step from here""" 
     348            print "tbt: inside reach distance of closest turn" 
     349            self.setStepAsCurrent(cs) 
     350 
     351        else: 
     352          """we dont have some of the data, that is needed to decide 
     353          if we start the navigation from the closest step of from the step that is after it 
     354          -> we just start from the closest step""" 
     355          """tbt: not enough data to decide, using closest turn""" 
     356          self.setStepAsCurrent(cs) 
     357    self.doNavigationUpdate() # run a navigation update 
    268358 
    269359  def stopTBT(self): 
    270     # stop Turn-by-turn navigation 
     360    """stop Turn-by-turn navigation""" 
    271361    self.goToInitialState() 
    272  
    273  
    274362 
    275363if(__name__ == "__main__"): 
Note: See TracChangeset for help on using the changeset viewer.