Priority Point

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

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

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,19 +244,22 @@ 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"]
         xferAsc = routeParameters["xfer_ascend"]
         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])

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

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:

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 = []

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