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 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 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 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")
