Matthew

.gitignore

@@ -8,7 +8,6 @@ __pycache__/
 
 # Distribution / packaging
 .Python
-env/
 build/
 develop-eggs/
 dist/
@@ -24,6 +23,7 @@ wheels/
 *.egg-info/
 .installed.cfg
 *.egg
+MANIFEST
 
 # PyInstaller
 #  Usually these files are written by a python script from a template
@@ -52,6 +52,8 @@ coverage.xml
 
 # Django stuff:
 *.log
+.static_storage/
+.media/
 local_settings.py
 
 # Flask stuff:
@@ -79,13 +81,14 @@ celerybeat-schedule
 # SageMath parsed files
 *.sage.py
 
-# dotenv
+# Environments
 .env
-
-# virtualenv
 .venv
+env/
 venv/
 ENV/
+env.bak/
+venv.bak/
 
 # Spyder project settings
 .spyderproject
@@ -100,6 +103,8 @@ ENV/
 # mypy
 .mypy_cache/
 
-
-#venv
-venv/
+# other
+.DS_Store
+.vscode/
+env/
+tests/results

requirements.txt

@@ -1,4 +1,14 @@
+attrs==17.3.0
+cycler==0.10.0
 decorator==4.1.2
+geopy==1.11.0
+matplotlib==2.1.1
 networkx==2.0
 numpy==1.13.3
-scipy==1.0.0
+pluggy==0.6.0
+py==1.5.2
+pyparsing==2.2.0
+pytest==3.3.1
+python-dateutil==2.6.1
+pytz==2017.3
+six==1.11.0

src/nav/env.py

@@ -0,0 +1,121 @@
+# -*- coding: utf-8 -*-
+"""planner.py
+
+Description
+
+Todo:
+    * everything
+"""
+from typing import Dict, Any
+
+import numpy as np
+import matplotlib.pyplot as plt
+from geopy.distance import distance as distance_ll
+
+from nav.graph.graph import FlightGraph
+
+class Environment:
+    def __init__(self, boundary: np.ndarray,
+                 static_obs: np.ndarray, params: Dict[str,Any]):
+        """An Environment describes the static elements of the
+        flight environment for a mission.
+
+        Args:
+            boundary: Array of points describing flight boundary.
+                The array is structured [[latitudes],[longitudes]].
+            static_obs: Array of static objects.
+                The array is structured [[latitudes],[longitudes],[radii in ft]].
+
+        Todo:
+            * ...
+        """
+        # set all parameters
+        self.granularity = params['granularity']
+
+        # set all fields that are constant between missions
+        self.boundary_ll = boundary
+        self.min_ll = np.amin(self.boundary_ll, axis=1)[:,np.newaxis] # keep shape
+        self.max_ll = np.amax(self.boundary_ll, axis=1)[:,np.newaxis]
+        self.lat_in_ft, self.lon_in_ft = self._ll_in_ft()
+        self.boundary_ft = self.ll_to_ft(self.boundary_ll, copy=True)
+
+        # set all fields that are constant within a mission
+        self.static_obs_ll = static_obs
+        self.static_obs_ft = self.ll_to_ft(self.static_obs_ll, copy=True) 
+        self.graph = FlightGraph(self.boundary_ft, self.static_obs_ft,
+                                         self.granularity)
+
+    def ll_to_ft(self, points: np.ndarray, copy=False):
+        if copy:
+            modified_points = np.copy(points)
+        else:
+            modified_points = points #TODO check that this doesn't copy
+        modified_points[0:2] -= self.min_ll
+        modified_points[0] *= self.lat_in_ft
+        modified_points[1] *= self.lon_in_ft
+        return modified_points
+
+    def ft_to_ll(self, points: np.ndarray, copy=False):
+        if copy:
+            modified_points = np.copy(points)
+        else:
+            modified_points = points #TODO check that this doesn't copy
+        modified_points[0] /= self.lat_in_ft
+        modified_points[1] /= self.lon_in_ft
+        modified_points[0:2] += self.min_ll
+        return modified_points
+
+    def point_ll_to_ft(self, point):
+        return (
+            (point[0] - self.min_ll[0][0]) * self.lat_in_ft,
+            (point[1] - self.min_ll[1][0]) * self.lon_in_ft,
+        )
+
+    def point_ft_to_ll(self, point):
+        return (
+            (point[0] / self.lat_in_ft) + self.min_ll[0][0],
+            (point[1] / self.lon_in_ft) + self.min_ll[1][0],
+        )
+
+    def display(self, ax):
+        # Draw Path
+        ax.plot(Environment._wrap1(self.boundary_ft[1]),
+                Environment._wrap1(self.boundary_ft[0]))
+
+        # Draw Obstacles
+        for obs_idx in range(self.static_obs_ft.shape[1]):
+            y = self.static_obs_ft[0,obs_idx]
+            x = self.static_obs_ft[1,obs_idx]
+            r = self.static_obs_ft[2,obs_idx]
+            circle = plt.Circle((x, y), r, color='r', zorder=0)
+            ax.add_artist(circle)
+
+        # Draw Graph
+        nodes_ft = self.graph.base_nodes_ft
+        ax.scatter(nodes_ft[1], nodes_ft[0], s=1)
+
+        # set axis labels and limits to match ft and ll
+        ax.set_xlabel("feet east")
+        ax.set_xlim([np.amin(self.boundary_ft[1]),np.amax(self.boundary_ft[1])])
+        ax.set_ylabel("feet north")
+        ax.set_ylim([np.amin(self.boundary_ft[0]),np.amax(self.boundary_ft[0])])
+
+        ax_lon = ax.twiny() # second set of axes for lat,lon
+        ax_lat = ax.twinx() # second set of axes for lat,lon
+        ax_lon.set_xlabel("longitude")
+        ax_lon.set_xlim([np.amin(self.boundary_ll[1]),np.amax(self.boundary_ll[1])])
+        ax_lat.set_ylabel("latitude")
+        ax_lat.set_ylim([np.amin(self.boundary_ll[0]),np.amax(self.boundary_ll[0])])
+
+    def _ll_in_ft(self):
+        min_lat, min_lon = (self.min_ll[0,0], self.min_ll[1,0])
+        max_lat, max_lon = (self.max_ll[0,0], self.max_ll[1,0])
+        lat_size_ll = max_lat - min_lat
+        lon_size_ll = max_lon - min_lon
+        lat_size_ft = distance_ll((min_lat, min_lon), (max_lat, min_lon)).ft
+        lon_size_ft = distance_ll((min_lat, min_lon), (min_lat, max_lon)).ft
+        return (lat_size_ft / lat_size_ll, lon_size_ft / lon_size_ll)
+
+    def _wrap1(array):
+        return np.append(array, array[0])
+

src/nav/graph/graph.py

@@ -0,0 +1,69 @@
+# -*- coding: utf-8 -*-
+"""graph.py
+Builds the initial graph, within the flight boundary_ft and outside obstacles.
+TODO:
+    Remove nodes outside non-rectangular flight boundary_ft
+"""
+from nav.graph.xgrid import xgrid_graph
+from nav.graph.polygon import Polygon
+
+from typing import Dict, Any
+
+import networkx as nx
+import numpy as np
+import matplotlib.pyplot as plt
+
+class FlightGraph:
+    """#TODO"""
+    def __init__(self, boundary_ft: np.ndarray, static_obs_ft: np.ndarray,
+                 granularity: float):
+        """Builds a space-filling graph within flight boundaries and removes
+        nodes in obstacles.
+
+        Args:
+            boundary_ft: the ft locations of the polygon defining the flight boundary
+            static_obs_ft: the locations and sizes of the stationary obstacles
+            granularity: ft separating graph nodes
+
+        Todo:
+            * ...
+        """
+        # space filling graph
+        self.granularity = granularity
+        lat_size_ft = np.amax(boundary_ft[0])
+        lon_size_ft = np.amax(boundary_ft[1])
+        lat_node_count = int(lat_size_ft // granularity) + 1
+        lon_node_count = int(lon_size_ft // granularity) + 1
+        base_graph = xgrid_graph(lat_node_count, lon_node_count)
+
+        # remove nodes in obstacles and outside boundary
+        rows = np.arange(lat_node_count)
+        cols = np.arange(lon_node_count)
+        nodes_rc = np.array([np.tile(rows, len(cols)), np.repeat(cols, len(rows))])
+        nodes_ft = self.granularity * nodes_rc
+        node_idx_to_remove = set() # all nodes to remove
+
+        # find nodes in obstacles
+        for obs_idx in range(static_obs_ft.shape[1]):  #TODO vectorize out
+            dist_sqr = np.sum((nodes_ft - static_obs_ft[0:2, obs_idx, np.newaxis]) \
+                              ** 2, axis=0)
+            node_idx_inside_obs = np.flatnonzero(dist_sqr \
+                                            <= (static_obs_ft[2, obs_idx] ** 2))
+            node_idx_to_remove.update(node_idx_inside_obs.tolist())
+
+        # find nodes outside boundary #TODO vectorize
+        self.polygon = Polygon(boundary_ft)
+        for node_idx in range(nodes_ft.shape[1]):
+            lat = nodes_ft[0, node_idx]
+            lon = nodes_ft[1, node_idx]
+            if (lat, lon) not in self.polygon:
+                node_idx_to_remove.add(node_idx)
+
+        # remove nodes
+        for node_idx in node_idx_to_remove:
+            base_graph.remove_node((nodes_rc[0, node_idx],
+                                         nodes_rc[1, node_idx]))
+
+        self.base_graph = base_graph
+        self.base_nodes_rc = np.array(list(base_graph)).T
+        self.base_nodes_ft = self.base_nodes_rc * self.granularity

src/nav/graph/initial.py

@@ -1,63 +1,46 @@
 # -*- coding: utf-8 -*-
-"""build_initial_graph.py
-Builds the initial graph, within the flight boundary and outside obstacles.
-Todo:
-    * Add flight boundary check with polygon
+"""initial.py
+Builds the initial graph, within the flight boundary_ft and outside obstacles.
+TODO:
+    Remove nodes outside non-rectangular flight boundary_ft
 """
-from .xgrid import xgrid_graph
+from nav.graph.xgrid import xgrid_graph
 
 from nav.utility.classes import Location
 from nav.utility.classes import Obstacle
 
-from typing import List
-from typing import Tuple
+from typing import Dict, Any
 
 import networkx as nx
 import numpy as np
 import matplotlib.pyplot as plt
 
-def build(boundary: List[Location], stat_obstacles: List[Obstacle],
-          granularity: float) -> nx.Graph:
+def build_graph(boundary_ft: np.ndarray, static_obs_ft: np.ndarray,
+                granularity: float) -> nx.Graph:
     """Builds a space-filling graph within flight boundaries and removes nodes in obstacles.
     Args:
-        boundary: the gps locations of the polygon defining the flight boundary
-        stat_obstacles: the locations and sizes of the stationary obstacles
-        granularity: the distance between graph nodes for (lat, lon)
+        boundary_ft: the ft locations of the polygon defining the flight boundary
+        static_obs_ft: the locations and sizes of the stationary obstacles
+        granularity: ft separating graph nodes
     Returns:
         graph: a graph filling the space, whose nodes are Locations
     """
     # space filling graph
-    min_boundary = Location(*(min(coord) for coord in zip(*boundary)))
-    max_boundary = Location(*(max(coord) for coord in zip(*boundary)))
-    graph_size = Location.granularity_diff(max_boundary, min_boundary, granularity)
-
-    graph = xgrid_graph(*graph_size)
+    lat_node_count = int(np.amin(boundary_ft[0]) // granularity)
+    lon_node_count = int(np.amin(boundary_ft[1]) // granularity)
+    graph = xgrid_graph(lat_node_count, lon_node_count)
 
-    # TODO remove nodes in obstacles
-    graph_origin = min_boundary
-    for obs in stat_obstacles:
-        to_remove = set()
+    # removes nodes in obstacles #TODO vectorize
+    to_remove = set()
+    for obs_idx in range(static_obs_ft.shape[1]):
         for node in graph:
-            loc = Location.from_grid(node, graph_origin, granularity)
-            if loc in obs:
+            distance = static_obs_ft[0:2, obs_idx] - np.array([[node[0]], [node[1]]])
+            r = static_obs_ft[2]
+            if np.sqrt(np.sum(distance**2)) < r:
                 to_remove.add(node)
-        for node in to_remove:
-            graph.remove_node(node)
+    for node in to_remove:
+        graph.remove_node(node)
 
-    # TODO return mapping from location to node, node to location?
+    # TODO remove nodes outside boundary_ft
 
-    return graph, graph_origin
-
-# test if run as main
-if __name__ == "__main__":
-    boundary = [Location(-50.0, -50.0), Location(-50.0, 150.0),
-                Location(150.0, -50.0), Location(150.0, 150.0)]
-    stat_obstacles = [Obstacle(Location(0.0, 0.0), 20),
-                      Obstacle(Location(30.0, 30.0), 10)]
-    granularity = 20.0
-
-    initial_graph, _ = build(boundary, stat_obstacles, granularity)
-    plt.figure()
-    pos = nx.get_node_attributes(initial_graph, 'pos')
-    nx.draw(initial_graph, pos, with_labels=True, font_weight='bold')
-    plt.show()
+    return graph