diff --git a/wadl/lib/fence.py b/wadl/lib/fence.py index e951481..a22a9d0 100644 --- a/wadl/lib/fence.py +++ b/wadl/lib/fence.py @@ -106,7 +106,7 @@ def parseFile(self, file): k = kml.KML() k.from_string(doc.encode('utf-8')) self.findPlacemarks(list(k.features())) - self.logger.info(f"found {len(self.areas)} objects") + self.logger.info(f"{len(self.areas)} polygons in {self.file.stem}") def findPlacemarks(self, features): # parse features throughout the KML File diff --git a/wadl/lib/maze.py b/wadl/lib/maze.py index 3338ac3..de8c0b5 100644 --- a/wadl/lib/maze.py +++ b/wadl/lib/maze.py @@ -11,7 +11,7 @@ import utm from shapely.geometry import Polygon, Point, LineString # lib -from wadl.lib.fence import Fence +from wadl.lib.fence import Fence, Areas from wadl.lib.route import RouteSet @@ -39,6 +39,7 @@ def __init__(self, step=40, rotation=0, home=None, + priority=None, routeParameters=None): super(Maze, self).__init__(Path(file)) self.logger = logging.getLogger(__name__) @@ -49,15 +50,26 @@ def __init__(self, # grid parameters self.theta = rotation self.step = step + # build grid graph self.buildGrid() self.nNode = len(self.graph) self.logger.info(f"generated maze with {self.nNode} nodes") + + # UAV path parameters self.home = home self.nNode = len(self.graph) # store size of nodes self.routeSet = RouteSet(self.home, self.UTMZone, routeParameters) + # resolve priority + if priority is not None: + self.setPriority(priority) + else: + self.priorityArea = None + self.prioritySet = None + self.routeSet["priority"] = self.prioritySet + def __len__(self): # number of nodes return len(self.graph) @@ -96,6 +108,7 @@ def buildGrid(self): utmCord = self.R.T @ np.array([x, y]) # store utm cord in graph self.graph.nodes[(i, j)]['UTM'] = utmCord + self.graph.nodes[(i, j)]['priority'] = False else: self.graph.remove_node((i, j)) @@ -112,6 +125,33 @@ def buildGrid(self): for i, node in enumerate(self.graph): self.graph.nodes[node]['index'] = i + def setPriority(self, file): + """Add a priority area to the survey. + + Args: + priority (str): file for priority area + """ + self.priorityArea = Areas(Path(file)) + # assign points based on priority + self.prioritySet = set() + for n0, n1 in self.graph.edges: + p0 = self.graph.nodes[n0]['UTM'] + p1 = self.graph.nodes[n1]['UTM'] + line = LineString([p0, p1]) + if self.isPriority(line): + self.graph.nodes[n0]['priority'] = True + self.graph.nodes[n1]['priority'] = True + self.prioritySet.add(tuple(map(int, tuple(p0)))) + self.prioritySet.add(tuple(map(int, tuple(p1)))) + self.logger.info(f"found {len(self.prioritySet)} priority points") + + def isPriority(self, line): + """checks if a line in in a priority polygon""" + for poly in self.priorityArea: + if line.intersects(poly): + return True + return False + def calcRouteStats(self): # log route data self.logger.info(f"\tgenerated {len(self.routeSet)} routes") @@ -121,7 +161,7 @@ def calcRouteStats(self): self.logger.info(f"{i}: {route.length:.2f}m \t{route.ToF:.2f}s") surveyTofs += route.ToF_surv transferTofs += route.ToF_tran - totalTime = surveyTofs + transferTofs + totalTime = surveyTofs + transferTofs ratio = surveyTofs/transferTofs self.stats = dict() lengths = [route.length for route in self.routeSet] @@ -279,8 +319,9 @@ def plotNodes(self, ax, color='k', nodes=None): nodes = self.graph.nodes for node in nodes: + marker = "s" if self.graph.nodes[node]['priority'] else "." ax.scatter(*self.graph.nodes[node]["UTM"], - color=color, s=5) + color=color, s=5, marker=marker) def plotEdges(self, ax, color='k', edges=None): # plot edges @@ -293,6 +334,9 @@ def plotEdges(self, ax, color='k', edges=None): ax.plot(line[:, 0], line[:, 1], color=color, linewidth=1) + def plotPriority(self, ax, color='m'): + self.priorityArea.plot(ax, color=color) + def plotRoutes(self, ax): cols = iter(plt.cm.rainbow(np.linspace(0, 1, len(self.routeSet)))) for route in self.routeSet: @@ -318,3 +362,5 @@ def plot(self, ax, showGrid=False, showRoutes=True): self.plotEdges(ax) if showRoutes: self.plotRoutes(ax) + if self.priorityArea is not None: + self.plotPriority(ax, color='tab:purple') # purple for priority diff --git a/wadl/lib/route.py b/wadl/lib/route.py index 8b2e725..d414007 100644 --- a/wadl/lib/route.py +++ b/wadl/lib/route.py @@ -35,6 +35,7 @@ def __init__(self, default=True): def setDefaults(self): self["limit"] = 13*60 # s self["speed"] = 4.0 # m/s + self["prio_speed"] = 3.0 # m/s self["altitude"] = 35.0 # m self["xfer_speed"] = 12.0 # m/s self["xfer_altitude"] = 70.0 # m @@ -60,6 +61,7 @@ def __init__(self, home, zone, routeParameters=None): # loggers self.logger = logging.getLogger(__name__) self.logger.setLevel(logging.DEBUG) + self.data = dict() self.home = home @@ -78,8 +80,11 @@ def __len__(self): def __iter__(self): return iter(self.routes) - def setData(self, data): - self.data = data + def __setitem__(self, key, value): + self.data[key] = value + + def __getitem__(self, key): + return self.data[key] def getLimit(self): # get the time limit @@ -98,7 +103,7 @@ def check(self, cords): """ route = Route(cords, self.zone, self.home) - route.build(self.routeParameters) + route.build(self.routeParameters, priority=self.data["priority"]) if route.check(): return True, route else: @@ -239,12 +244,12 @@ def calcLength(self, waypoints): ToF += dist/wp[3] return length, ToF - def build(self, routeParameters): + def build(self, routeParameters, priority=None): # build the path - # unpack parameters self.limit = routeParameters["limit"] spd = routeParameters["speed"] + priSpd = routeParameters["prio_speed"] alt = routeParameters["altitude"] xferSpd = routeParameters["xfer_speed"] xferAlt = routeParameters["xfer_altitude"] @@ -252,6 +257,9 @@ def build(self, routeParameters): xferDes = routeParameters["xfer_descend"] landALt = routeParameters["land_altitude"] + if priority is None: + priority = set() + # take off if self.home is not None: Hlat, Hlng = self.home @@ -260,8 +268,11 @@ def build(self, routeParameters): lat, lng = self.GPScords[0] self.waypoints.append([lat, lng, xferAlt, xferDes]) # push each waypoint - for lat, lng in self.GPScords[:-1]: - self.waypoints.append([lat, lng, alt, spd]) + for gpsPt, utmPt in zip(self.GPScords[:-1], self.UTMcords[:-1]): + lat, lng = gpsPt + roundedUTM = tuple(map(int, utmPt)) + s = priSpd if roundedUTM in priority else spd + self.waypoints.append([lat, lng, alt, s]) lat, lng = self.GPScords[-1] # last point to ascend to transfer self.waypoints.append([lat, lng, alt, xferAsc]) diff --git a/wadl/solver/metaGraph.py b/wadl/solver/metaGraph.py index e3cd7f7..a5543c1 100644 --- a/wadl/solver/metaGraph.py +++ b/wadl/solver/metaGraph.py @@ -141,7 +141,7 @@ def buildSubGraphs(self, subNodes): return subGraphs - def mergeSubGraphs(self, subGraphs, minSize=20, maxSize=50): + def mergeSubGraphs(self, subGraphs, minSize=25, maxSize=50): # merge small subGraphs into the most connected subGraph # if tie pick the smallest subgraph merged = dict() diff --git a/wadl/solver/pathTree.py b/wadl/solver/pathTree.py index 7bc00db..4409ac4 100644 --- a/wadl/solver/pathTree.py +++ b/wadl/solver/pathTree.py @@ -40,18 +40,18 @@ def makeNodes(self, routeSet): # make the initial nodes of the graph utmHome = [utm.from_latlon(*home)[0:2] for home in routeSet.home] self.tree.add_node("home", UTM=utmHome, homeDist=0) - for node, path in enumerate(self.subPaths): + for tile, path in enumerate(self.subPaths): UTMpath = [self.getUTM(pt) for pt in self.steamlinePath(path)] _, route = routeSet.check(UTMpath[:-1]) # unpack route metrics into the node - self.tree.add_node(node, + self.tree.add_node(tile, UTM=self.getUTM(path[0]), homeDist=route.len_tran, homeTime=route.ToF_tran, survDist=route.len_surv, survTime=route.ToF_surv ) - self.tree.add_edge("home", node) + self.tree.add_edge("home", tile) def makeEdges(self): for e1, e2 in self.pathGraph.edges: diff --git a/wadl/solver/solver.py b/wadl/solver/solver.py index fbc3cbc..c85777e 100644 --- a/wadl/solver/solver.py +++ b/wadl/solver/solver.py @@ -91,15 +91,16 @@ def solve(self, routeSet): # return solution time solveTime = time.time()-startTime self.logger.info("solution time: {:2.5f} sec".format(solveTime)) - routeSet.setData(self.getRouteData()) + nGraph, nSteps = self.getRouteData() + routeSet["nGraphs"] = nGraph + routeSet["nSteps"] = nSteps return solveTime def getRouteData(self): - data = dict() # calculate various metrics and push them to the routeSet container - data["nGraphs"] = len(self.metaGraph.subGraphs) - data["nSteps"] = sum([len(path)-1 for path in self.metaGraph.subPaths]) - return data + nGraphs = len(self.metaGraph.subGraphs) + nSteps = sum([len(path)-1 for path in self.metaGraph.subPaths]) + return nGraphs, nSteps def solveTiles(self): subPaths = [] diff --git a/wadl/survey.py b/wadl/survey.py index 7b24fbe..f239050 100644 --- a/wadl/survey.py +++ b/wadl/survey.py @@ -73,6 +73,7 @@ def addTask(self, file, **kwargs): rotation (int, optional): rotation of the grid by radians. limit (float, optional): default flight time limit home (srt, optional): key(s) of home location(s) + priority (wadl.lib.Areas): Areas object of high priority sections routeParamters (RouteParameters): Desired settings for each route in this task @@ -89,8 +90,12 @@ def addTask(self, file, **kwargs): self.tasks[file] = Maze(file, **kwargs) + def __getitem__(self, idx): + key = [*self.tasks][idx] + return self.tasks[key] + def at(self, sliced): - return self.tasks[[*self.tasks][sliced]] + return self.__getitem__(sliced) def setSolver(self, solver): self.solver = solver @@ -169,6 +174,7 @@ def plan(self, write=True, showPlot=False): except RuntimeError as e: self.logger.error(f"failure in task: {maze.name}") print(e) + print("\n") self.logger.info(f"task {maze.name} finished") self.logger.info("done planning")