def handleRouteDownloadRequest(self, requestparam): mtype = "application/gpx+xml" route = None try: name = self.getRequestParam(requestparam, "name") if name is not None and not name == "": AVNLog.debug("download route name=%s", name) route = self.loadRoute(name) else: data = self.getRequestParam(requestparam, '_json') if data is None: AVNLog.error( "unable to find a route for download, returning an empty" ) return "" route = self.routeFromJsonString(data) if route is None: return "error - route not found" data = self.gpxFormat % (route.to_xml()) stream = StringIO.StringIO(data) return {'size': len(data), 'mimetype': mtype, 'stream': stream} except: AVNLog.error("exception in route download %s", traceback.format_exc()) return "error"
def routeFromJsonString(self,routeJson): try: dct=json.loads(routeJson) return self.routeFromJson(dct) except : AVNLog.error("error parsing json for route: %s",traceback.format_exc()) return None
def parseLeg(self, str): dct = json.loads(str) if dct.get('from') is None: return None if (dct.get('anchorDistance') is not None): rt = AVNRoutingLeg( None, gpx.GPXWaypoint(**self.convertToGpx(dct.get('from'))), None, False, -1, 0, False) rt.anchorDistance = float(dct.get('anchorDistance')) return rt if dct.get('to') is None: return None currentTarget = dct.get('currentTarget') if currentTarget is None: currentTarget = -1 rt = AVNRoutingLeg( dct.get('name'), gpx.GPXWaypoint(**self.convertToGpx(dct.get('from'))), gpx.GPXWaypoint(**self.convertToGpx(dct.get('to'))), dct.get('active'), currentTarget, dct.get('approachDistance'), dct.get('approach')) if dct.get('currentRoute') is not None: try: rt.currentRoute = self.routeFromJson(dct.get('currentRoute')) except: AVNLog.error("error parsing route from leg: %s", traceback.format_exc()) return rt
def loadRoute(self,name): rt=self.getRouteFromList(name) if rt is not None: return rt filename=self.getRouteFileName(name) try: rt=self.loadRouteFile(filename) except: AVNLog.error("unable to load route %s:%s"%(filename,traceback.format_exc(1))) return if rt is not None: self.addRouteToList(rt) return rt
def handleRouteDownloadRequest(self,requestparam): mtype = "application/gpx+xml" route=None try: name=self.getRequestParam(requestparam,"name") if name is not None and not name == "": AVNLog.debug("download route name=%s",name) route=self.loadRoute(name) else: data=self.getRequestParam(requestparam,'_json'); if data is None: AVNLog.error("unable to find a route for download, returning an empty") return "" route=self.routeFromJsonString(data) if route is None: return "error - route not found" data=self.gpxFormat%(route.to_xml()) stream=StringIO.StringIO(data) return {'size':len(data),'mimetype':mtype,'stream':stream} except: AVNLog.error("exception in route download %s",traceback.format_exc()) return "error"
def parseLeg(self,str): dct=json.loads(str) if dct.get('from') is None: return None if (dct.get('anchorDistance') is not None): rt = AVNRoutingLeg( None, gpx.GPXWaypoint(**self.convertToGpx(dct.get('from'))), None, False, -1, 0, False) rt.anchorDistance=float(dct.get('anchorDistance')) return rt if dct.get('to') is None: return None currentTarget=dct.get('currentTarget') if currentTarget is None: currentTarget=-1 rt= AVNRoutingLeg( dct.get('name'), gpx.GPXWaypoint(**self.convertToGpx(dct.get('from'))), gpx.GPXWaypoint(**self.convertToGpx(dct.get('to'))), dct.get('active'), currentTarget, dct.get('approachDistance'), dct.get('approach') ) if dct.get('currentRoute') is not None: try: rt.currentRoute=self.routeFromJson(dct.get('currentRoute')) except: AVNLog.error("error parsing route from leg: %s",traceback.format_exc()) return rt
def computeApproach(self): if self.currentLeg is None: self.startStopAlarm(False) return if not self.currentLeg.active: self.startStopAlarm(False) return curTPV = self.navdata.getMergedEntries("TPV", []) lat = curTPV.data.get('lat') lon = curTPV.data.get('lon') if lat is None or lon is None: self.startStopAlarm(False) return dst = AVNUtil.distanceM(self.wpToLatLon(self.currentLeg.toWP), (lat, lon)) AVNLog.debug("approach current distance=%f", float(dst)) if (dst > self.currentLeg.approachDistance): self.currentLeg.approach = False self.startStopAlarm(False) if (self.currentLeg.approach): #save leg self.setCurrentLeg(self.currentLeg) self.lastDistanceToCurrent = None self.lastDistanceToNext = None return if not self.currentLeg.approach: self.startStopAlarm(True) self.currentLeg.approach = True #save the leg self.setCurrentLeg(self.currentLeg) AVNLog.info("Route: approaching wp %d (%s) currentDistance=%f", self.currentLeg.currentTarget, unicode(self.currentLeg.toWP), float(dst)) if self.currentLeg.name is None: AVNLog.debug("Approach: no route active") return self.activeRouteName = self.currentLeg.name try: if self.activeRoute is None or self.activeRoute.name != self.activeRouteName: self.activeRoute = self.loadRoute(self.activeRouteName) if self.activeRoute is None: AVNLog.error( "unable to load route %s, cannot make approach handling", self.currentLeg.name) return except: AVNLog.error("exception when loading route %s:%s", self.activeRouteName, traceback.format_exc()) return nextWpNum = self.currentLeg.currentTarget + 1 hasNextWp = True nextDistance = 0 if nextWpNum >= self.activeRoute.get_points_no(): AVNLog.debug("already at last WP of route %d", (nextWpNum - 1)) hasNextWp = False else: nextWp = self.activeRoute.points[nextWpNum] nextDistance = AVNUtil.distanceM((lat, lon), self.wpToLatLon(nextWp)) if self.lastDistanceToNext is None or self.lastDistanceToNext is None: #first time in approach self.lastDistanceToNext = nextDistance self.lastDistanceToCurrent = dst return #check if the distance to own wp increases and to the next decreases diffcurrent = dst - self.lastDistanceToCurrent if (diffcurrent <= 0): #still decreasing self.lastDistanceToCurrent = dst self.lastDistanceToNext = nextDistance return diffnext = nextDistance - self.lastDistanceToNext if (diffnext > 0): #increases to next self.lastDistanceToCurrent = dst self.lastDistanceToNext = nextDistance return if not hasNextWp: AVNLog.info("last WP of route reached, switch of routing") self.currentLeg.active = False self.currentLeg.approach = False self.currentLeg.name = None self.setCurrentLeg(self.currentLeg) self.lastDistanceToCurrent = None self.lastDistanceToNext = None return #should we wait for some time??? AVNLog.info("switching to next WP num=%d, wp=%s", nextWpNum, unicode(nextWp)) self.currentLeg.currentTarget = nextWpNum self.currentLeg.fromWP = self.currentLeg.toWP self.currentLeg.toWP = nextWp self.currentLeg.approach = False self.lastDistanceToCurrent = None self.lastDistanceToNext = None self.setCurrentLeg(self.currentLeg)
def run(self): self.setName("[%s]%s" % (AVNLog.getThreadId(), self.getName())) interval = self.getIntParam('interval') routesdir = AVNUtil.replaceParam( os.path.expanduser(self.getStringParam("routesdir")), AVNConfig.filterBaseParam(self.getParam())) if routesdir == "": routesdir = os.path.join( self.getStringParam(AVNConfig.BASEPARAM.DATADIR), u'routes') self.routesdir = routesdir if not os.path.isdir(self.routesdir): AVNLog.info("creating routes directory %s" % (self.routesdir)) os.makedirs(self.routesdir, 0755) self.fillRouteInfos() self.currentLegFileName = os.path.join(self.routesdir, self.currentLegName) if os.path.exists(self.currentLegFileName): f = None try: f = open(self.currentLegFileName, "r") strleg = f.read(self.MAXROUTESIZE + 1000) self.currentLeg = self.parseLeg(strleg) if self.currentLeg.toWP is not None: distance = geo.length( [self.currentLeg.fromWP, self.currentLeg.toWP]) AVNLog.info( "read current leg, route=%s, from=%s, to=%s, length=%fNM" % (self.currentLeg.name, unicode(self.currentLeg.fromWP), unicode(self.currentLeg.toWP), distance / AVNUtil.NM)) else: AVNLog.info("read current leg, route=%s, from=%s, to=%s" % ( self.currentLeg.name, unicode(self.currentLeg.fromWP), "NONE", )) if self.currentLeg.name is not None: self.activeRouteName = self.currentLeg.name if self.currentLeg.currentRoute is not None: #this will also set the active route self.saveRoute(self.currentLeg.currentRoute) if self.currentLeg.name != self.currentLeg.currentRoute.name: AVNLog.error( "leg inconsistent, name in route %s different from name in leg %s, correcting to route name", self.currentLeg.name, self.currentLeg.currentRoute.name) self.currentLeg.name = self.currentLeg.currentRoute.name self.activeRouteName = self.currentLeg.name #write back the corrected leg self.setCurrentLeg(self.currentLeg) else: if self.currentLeg.name is not None: self.activeRoute = self.loadRoute(self.currentLeg.name) if self.activeRoute is None: self.activeRoute = gpx.GPXRoute( self.currentLeg.name) self.currentLeg.currentRoute = self.activeRoute except: AVNLog.error("error parsing current leg %s: %s" % (self.currentLegFileName, traceback.format_exc())) if f is not None: f.close() #TODO: open route else: AVNLog.info("no current leg %s found" % (self.currentLegFileName, )) AVNLog.info("router main loop started") while True: hasLeg = False hasRMB = False time.sleep(interval) if self.currentLeg and self.currentLeg.active: hasLeg = True if self.currentLeg.anchorDistance is not None: routerInfo = "Anchor watch, from %s, (anchor radius %dm)" % ( unicode(self.currentLeg.fromWP), int(self.currentLeg.anchorDistance)) else: routerInfo = "from %s, to %s, route=%s, activeWp=%d, approach=%s (approach radius %dm)" % ( unicode(self.currentLeg.fromWP) if self.currentLeg.fromWP else "NONE", unicode(self.currentLeg.toWP) if self.currentLeg.toWP else "NONE", self.currentLeg.name if self.currentLeg.name is not None else "NONE", self.currentLeg.currentTarget, "TRUE" if self.currentLeg.approach else "FALSE", int(self.currentLeg.approachDistance)) AVNLog.debug(routerInfo) self.setInfo("leg", routerInfo, AVNWorker.Status.RUNNING) try: if self.currentLeg is not None and self.currentLeg.anchorDistance is not None: self.computeAnchor() else: self.startStopAlarm(False, 'anchor') self.startStopAlarm(False, 'gps') computeRMB = self.getBoolParam("computeRMB") computeAPB = self.getBoolParam("computeAPB") if computeRMB or computeAPB: hasRMB = self.computeRMB(computeRMB, computeAPB) except Exception as e: AVNLog.warn("exception in computeRMB %s, retrying", traceback.format_exc()) try: self.computeApproach() except: AVNLog.warn("exception in computeApproach %s, retrying", traceback.format_exc()) if (not hasLeg): self.setInfo("leg", "no leg", AVNWorker.Status.INACTIVE) if (not hasRMB): self.setInfo("autopilot", "no autopilot data", AVNWorker.Status.INACTIVE) try: curTPV = self.navdata.getMergedEntries("TPV", []) lat = curTPV.data.get('lat') lon = curTPV.data.get('lon') if lat is not None and lon is not None: self.startStopAlarm(False, 'gps') except: pass AVNLog.debug("router main loop")
def computeApproach(self): if self.currentLeg is None: self.startStopAlarm(False) return if not self.currentLeg.active: self.startStopAlarm(False) return curGps=self.navdata.getDataByPrefix(AVNStore.BASE_KEY_GPS,1) lat=curGps.get('lat') lon=curGps.get('lon') if lat is None or lon is None: self.startStopAlarm(False) return dst = AVNUtil.distanceM(self.wpToLatLon(self.currentLeg.toWP),(lat,lon)); AVNLog.debug("approach current distance=%f",float(dst)) if (dst > self.currentLeg.approachDistance): self.currentLeg.approach=False self.startStopAlarm(False) if (self.currentLeg.approach): #save leg self.setCurrentLeg(self.currentLeg) self.lastDistanceToCurrent=None self.lastDistanceToNext=None return if not self.currentLeg.approach: self.startStopAlarm(True) self.currentLeg.approach=True #save the leg self.setCurrentLeg(self.currentLeg) AVNLog.info("Route: approaching wp %d (%s) currentDistance=%f",self.currentLeg.currentTarget,unicode(self.currentLeg.toWP),float(dst)) if self.currentLeg.name is None: AVNLog.debug("Approach: no route active") return self.activeRouteName=self.currentLeg.name try: if self.activeRoute is None or self.activeRoute.name != self.activeRouteName: self.activeRoute=self.loadRoute(self.activeRouteName) if self.activeRoute is None: AVNLog.error("unable to load route %s, cannot make approach handling",self.currentLeg.name) return except: AVNLog.error("exception when loading route %s:%s",self.activeRouteName,traceback.format_exc()) return nextWpNum=self.currentLeg.currentTarget+1 hasNextWp=True nextDistance=0 if nextWpNum >= self.activeRoute.get_points_no(): AVNLog.debug("already at last WP of route %d",(nextWpNum-1)) hasNextWp=False else: nextWp=self.activeRoute.points[nextWpNum] nextDistance=AVNUtil.distanceM((lat,lon),self.wpToLatLon(nextWp)) if self.lastDistanceToNext is None or self.lastDistanceToNext is None: #first time in approach self.lastDistanceToNext=nextDistance self.lastDistanceToCurrent=dst return #check if the distance to own wp increases and to the next decreases diffcurrent=dst-self.lastDistanceToCurrent if (diffcurrent <= 0): #still decreasing self.lastDistanceToCurrent=dst self.lastDistanceToNext=nextDistance return diffnext=nextDistance-self.lastDistanceToNext if (diffnext > 0): #increases to next self.lastDistanceToCurrent=dst self.lastDistanceToNext=nextDistance return if not hasNextWp: AVNLog.info("last WP of route reached, switch of routing") self.currentLeg.active=False self.currentLeg.approach=False self.currentLeg.name=None self.setCurrentLeg(self.currentLeg) self.lastDistanceToCurrent=None self.lastDistanceToNext=None return #should we wait for some time??? AVNLog.info("switching to next WP num=%d, wp=%s",nextWpNum,unicode(nextWp)) self.currentLeg.currentTarget=nextWpNum self.currentLeg.fromWP=self.currentLeg.toWP self.currentLeg.toWP=nextWp self.currentLeg.approach=False self.lastDistanceToCurrent=None self.lastDistanceToNext=None self.setCurrentLeg(self.currentLeg)
def run(self): self.setName("[%s]%s"%(AVNLog.getThreadId(),self.getName())) interval=self.getIntParam('interval') routesdir=AVNUtil.replaceParam(os.path.expanduser(self.getStringParam("routesdir")),AVNConfig.filterBaseParam(self.getParam())) if routesdir == "": routesdir=os.path.join(self.getStringParam(AVNConfig.BASEPARAM.DATADIR),u'routes') self.routesdir=routesdir if not os.path.isdir(self.routesdir): AVNLog.info("creating routes directory %s"%(self.routesdir)) os.makedirs(self.routesdir,0755) self.fillRouteInfos() self.currentLegFileName=os.path.join(self.routesdir,self.currentLegName) if os.path.exists(self.currentLegFileName): f=None try: f=open(self.currentLegFileName,"r") strleg=f.read(self.MAXROUTESIZE+1000) self.currentLeg=self.parseLeg(strleg) if self.currentLeg.toWP is not None: distance=geo.length([self.currentLeg.fromWP,self.currentLeg.toWP]) AVNLog.info("read current leg, route=%s, from=%s, to=%s, length=%fNM"%(self.currentLeg.name, unicode(self.currentLeg.fromWP),unicode(self.currentLeg.toWP),distance/AVNUtil.NM)) else: AVNLog.info("read current leg, route=%s, from=%s, to=%s"% (self.currentLeg.name, unicode(self.currentLeg.fromWP), "NONE", )) if self.currentLeg.name is not None: self.activeRouteName=self.currentLeg.name if self.currentLeg.currentRoute is not None: #this will also set the active route self.saveRoute(self.currentLeg.currentRoute) if self.currentLeg.name != self.currentLeg.currentRoute.name: AVNLog.error("leg inconsistent, name in route %s different from name in leg %s, correcting to route name",self.currentLeg.name, self.currentLeg.currentRoute.name) self.currentLeg.name=self.currentLeg.currentRoute.name self.activeRouteName=self.currentLeg.name #write back the corrected leg self.setCurrentLeg(self.currentLeg) else: if self.currentLeg.name is not None: self.activeRoute=self.loadRoute(self.currentLeg.name) if self.activeRoute is None: self.activeRoute=gpx.GPXRoute(self.currentLeg.name) self.currentLeg.currentRoute=self.activeRoute except: AVNLog.error("error parsing current leg %s: %s"%(self.currentLegFileName,traceback.format_exc())) if f is not None: f.close() #TODO: open route else: AVNLog.info("no current leg %s found"%(self.currentLegFileName,)) AVNLog.info("router main loop started") while True: hasLeg=False hasRMB=False time.sleep(interval) if self.currentLeg and self.currentLeg.active: hasLeg=True if self.currentLeg.anchorDistance is not None: routerInfo = "Anchor watch, from %s, (anchor radius %dm)" % ( unicode(self.currentLeg.fromWP), int(self.currentLeg.anchorDistance)) else: routerInfo="from %s, to %s, route=%s, activeWp=%d, approach=%s (approach radius %dm)"%(unicode(self.currentLeg.fromWP) if self.currentLeg.fromWP else "NONE",unicode(self.currentLeg.toWP) if self.currentLeg.toWP else "NONE", self.currentLeg.name if self.currentLeg.name is not None else "NONE", self.currentLeg.currentTarget, "TRUE" if self.currentLeg.approach else "FALSE",int(self.currentLeg.approachDistance)) AVNLog.debug(routerInfo) self.setInfo("leg",routerInfo ,AVNWorker.Status.RUNNING) try: if self.currentLeg is not None and self.currentLeg.anchorDistance is not None: self.computeAnchor() else: self.startStopAlarm(False,'anchor') self.startStopAlarm(False, 'gps') computeRMB=self.getBoolParam("computeRMB") computeAPB=self.getBoolParam("computeAPB") if computeRMB or computeAPB : hasRMB=self.computeRMB(computeRMB,computeAPB) except Exception as e: AVNLog.warn("exception in computeRMB %s, retrying",traceback.format_exc()) try: self.computeApproach() except: AVNLog.warn("exception in computeApproach %s, retrying",traceback.format_exc()) if (not hasLeg): self.setInfo("leg","no leg",AVNWorker.Status.INACTIVE) if (not hasRMB): self.setInfo("autopilot","no autopilot data",AVNWorker.Status.INACTIVE) try: curTPV = self.navdata.getMergedEntries("TPV", []) lat = curTPV.data.get('lat') lon = curTPV.data.get('lon') if lat is not None and lon is not None: self.startStopAlarm(False,'gps') except: pass AVNLog.debug("router main loop")