def kml(self, kml): """ Appends kml nodes describing this flyzone. Args: kml: A simpleKML Container to which the fly zone will be added """ zone_name = 'Fly Zone {}'.format(self.pk) pol = kml.newpolygon(name=zone_name) fly_zone = [] flyzone_num = 1 for point in self.boundary_pts.all(): gps = point.position.gps_position coord = (gps.longitude, gps.latitude, units.feet_to_meters(point.position.altitude_msl)) fly_zone.append(coord) # Add boundary marker wp = kml.newpoint(name=str(flyzone_num), coords=[coord]) wp.description = str(point) wp.visibility = False flyzone_num += 1 fly_zone.append(fly_zone[0]) pol.outerboundaryis = fly_zone # Search Area Style pol.style.linestyle.color = Color.red pol.style.linestyle.width = 3 pol.style.polystyle.color = Color.changealphaint(50, Color.green)
def test_kml_simple(self): coordinates = [ (-76.0, 38.0, 0.0), (-76.0, 38.0, 10.0), (-76.0, 38.0, 20.0), (-76.0, 38.0, 30.0), (-76.0, 38.0, 100.0), (-76.0, 38.0, 30.0), (-76.0, 38.0, 60.0), ] # Create Coordinates start = TakeoffOrLandingEvent(user=self.user, uas_in_air=True) start.save() for coord in coordinates: self.create_log_element(*coord) end = TakeoffOrLandingEvent(user=self.user, uas_in_air=False) end.save() kml = Kml() UasTelemetry.kml(user=self.user, logs=UasTelemetry.by_user(self.user), kml=kml, kml_doc=kml) for coord in coordinates: tag = self.coord_format.format(coord[1], coord[0], units.feet_to_meters(coord[2])) self.assertTrue(tag in kml.kml())
def uas_telemetry_kml(user, flight_logs, kml, kml_doc): """ Appends kml nodes describing the given user's flight as described by the log array given. Args: user: A Django User to get username from flight_logs: A sequence of UasTelemetry logs per flight period. kml: A simpleKML Container to which the flight data will be added kml_doc: The simpleKML Document to which schemas will be added Returns: None """ # Lazily create folder iff there is data. kml_folder = None for i, logs in enumerate(flight_logs): name = '%s Flight %d' % (user.username, i + 1) logs = UasTelemetry.dedupe(UasTelemetry.filter_bad(logs)) coords = [] angles = [] when = [] for entry in logs: # Spatial Coordinates coord = (entry.longitude, entry.latitude, units.feet_to_meters(entry.altitude_msl)) coords.append(coord) # Time Elements time = entry.timestamp.strftime(KML_DATETIME_FORMAT) when.append(time) # Degrees heading, tilt, and roll angle = (entry.uas_heading, 0.0, 0.0) angles.append(angle) # Ignore tracks with no data. if not coords or not angles or not when: continue # Start folder if not done so already. if not kml_folder: kml_folder = kml.newfolder(name=user.username) # Create a new track in the folder trk = kml_folder.newgxtrack(name=name) trk.altitudemode = AltitudeMode.absolute # Append flight data trk.newwhen(when) trk.newgxcoord(coords) trk.newgxangle(angles) # Set styling trk.extrude = 1 # Extend path to ground trk.style.linestyle.width = 2 trk.style.linestyle.color = Color.blue trk.iconstyle.icon.href = KML_PLANE_ICON
def test_kml_simple(self): coordinates = [ (0, -76.0, 38.0, 0.0, 0), (1, -76.0, 38.0, 10.0, 0), (2, -76.0, 38.0, 20.0, 0), (3, -76.0, 38.0, 30.0, 0), (4, -76.0, 38.0, 100.0, 0), (5, -76.0, 38.0, 30.0, 0), (6, -76.0, 38.0, 60.0, 0), ] # Create Coordinates start = TakeoffOrLandingEvent(user=self.user, uas_in_air=True) start.save() start.timestamp = self.now start.save() for coord in coordinates: self.create_log_element(*coord) end = TakeoffOrLandingEvent(user=self.user, uas_in_air=False) end.save() end.timestamp = self.now + datetime.timedelta(seconds=7) end.save() kml = Kml() UasTelemetry.kml( user=self.user, logs=UasTelemetry.by_user(self.user), kml=kml, kml_doc=kml.document) for coord in coordinates: tag = self.coord_format.format(coord[2], coord[1], units.feet_to_meters(coord[3])) self.assertTrue(tag in kml.kml())
def live_kml(cls, kml, timespan, resolution=100): """ Appends kml nodes describing current paths of the obstacles Args: kml: A simpleKML Container to which the obstacle data will be added timespan: A timedelta to look backwards in time resolution: integer number of milliseconds between obstacle positions Returns: None """ def track(obstacle, span, dt): curr = timezone.now() last = curr - span time = curr while time >= last: yield obstacle.get_position(time) time -= dt for obstacle in MovingObstacle.objects.all(): dt = timedelta(milliseconds=resolution) linestring = kml.newlinestring(name="Obstacle") coords = [] for pos in track(obstacle, timespan, dt): # Longitude, Latitude, Altitude coord = (pos[1], pos[0], units.feet_to_meters(pos[2])) coords.append(coord) linestring.coords = coords linestring.altitudemode = AltitudeMode.absolute linestring.extrude = 1 linestring.style.linestyle.color = Color.red linestring.style.polystyle.color = Color.changealphaint(100, Color.red)
def live_kml(cls, kml, timespan, resolution=100): """ Appends kml nodes describing current paths of the obstacles Args: kml: A simpleKML Container to which the obstacle data will be added timespan: A timedelta to look backwards in time resolution: integer number of milliseconds between obstacle positions Returns: None """ def track(obstacle, span, dt): curr = timezone.now() last = curr - span time = curr while time >= last: yield obstacle.get_position(time) time -= dt for obstacle in MovingObstacle.objects.all(): dt = timedelta(milliseconds=resolution) linestring = kml.newlinestring(name="Obstacle") coords = [] for pos in track(obstacle, timespan, dt): # Longitude, Latitude, Altitude coord = (pos[1], pos[0], units.feet_to_meters(pos[2])) coords.append(coord) linestring.coords = coords linestring.altitudemode = AltitudeMode.absolute linestring.extrude = 1 linestring.style.linestyle.color = Color.red linestring.style.polystyle.color = Color.changealphaint( 100, Color.red)
def kml(self, path, kml, kml_doc): """ Appends kml nodes describing the given user's flight as described by the log array given. No nodes are added if less than two log entries are given. Args: path: A list of UasTelemetry elements. kml: A simpleKML Container to which the flight data will be added kml_doc: The simpleKML Document to which schemas will be added Returns: None """ # KML Compliant Datetime Formatter kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ" icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png' kml_output_resolution = 100 # milliseconds coords = [] when = [] ranges = [] if len(path) < 2: return dt = timedelta(milliseconds=kml_output_resolution) for pos, uav, time in self.times(path, dt): # Spatial Coordinates (longitude, latitude, altitude) coord = (pos[1], pos[0], units.feet_to_meters(pos[2])) coords.append(coord) # Time Elements when.append(time.strftime(kml_datetime_format)) # Distance Elements uas_range = distance.distance_to(*uav + pos) ranges.append(uas_range) # Create a new track in the folder trk = kml.newgxtrack(name='Obstacle Path {}'.format(self.id)) trk.altitudemode = AltitudeMode.absolute # Create a schema for extended data: proximity schema = kml_doc.newschema() schema.newgxsimplearrayfield(name='proximity', type=Types.float, displayname='UAS Proximity [ft]') trk.extendeddata.schemadata.schemaurl = schema.id # Append flight data trk.newwhen(when) trk.newgxcoord(coords) trk.extendeddata.schemadata.newgxsimplearraydata('proximity', ranges) # Set styling trk.extrude = 1 # Extend path to ground trk.style.linestyle.width = 2 trk.style.linestyle.color = Color.red trk.iconstyle.icon.href = icon
def kml(cls, user, logs, kml, kml_doc): """ Appends kml nodes describing the given user's flight as described by the log array given. Args: user: A Django User to get username from logs: A list of UasTelemetry elements kml: A simpleKML Container to which the flight data will be added kml_doc: The simpleKML Document to which schemas will be added Returns: None """ kml_folder = kml.newfolder(name=user.username) flights = TakeoffOrLandingEvent.flights(user) if len(flights) == 0: return logs = UasTelemetry.dedupe(UasTelemetry.filter_bad(logs)) for i, flight in enumerate(flights): name = '%s Flight %d' % (user.username, i + 1) flight_logs = filter(lambda x: flight.within(x.timestamp), logs) coords = [] angles = [] when = [] for entry in logs: pos = entry.uas_position.gps_position # Spatial Coordinates coord = (pos.longitude, pos.latitude, units.feet_to_meters(entry.uas_position.altitude_msl)) coords.append(coord) # Time Elements time = entry.timestamp.strftime(KML_DATETIME_FORMAT) when.append(time) # Degrees heading, tilt, and roll angle = (entry.uas_heading, 0.0, 0.0) angles.append(angle) # Create a new track in the folder trk = kml_folder.newgxtrack(name=name) trk.altitudemode = AltitudeMode.absolute # Append flight data trk.newwhen(when) trk.newgxcoord(coords) trk.newgxangle(angles) # Set styling trk.extrude = 1 # Extend path to ground trk.style.linestyle.width = 2 trk.style.linestyle.color = Color.blue trk.iconstyle.icon.href = KML_PLANE_ICON
def test_feet_to_meters(self): """Performs a data-driven test of the conversion.""" cases = [ # (feet, meters) ( 0, 0), ( 1, 0.3048), ( 10, 3.048), (1000, 304.8), ] # yapf: disable for (feet, meters_actual) in cases: self.assertAlmostEqual(meters_actual, units.feet_to_meters(feet), delta=0.1)
def __init__(self, *args, **kwargs): super(TestGenerateKMLWithFixture, self).__init__(*args, **kwargs) self.folders = ['Teams', 'Missions'] self.users = ['testuser', 'user0', 'user1'] self.coordinates = [(lat, lon, units.feet_to_meters(alt)) for lat, lon, alt in [ (-76.0, 38.0, 0.0), (-76.0, 38.0, 10.0), (-76.0, 38.0, 20.0), (-76.0, 38.0, 30.0), (-76.0, 38.0, 100.0), (-76.0, 38.0, 30.0), (-77.0, 38.0, 60.0), ]]
def __init__(self, *args, **kwargs): super(TestGenerateKMLWithFixture, self).__init__(*args, **kwargs) self.folders = ['Teams', 'Missions'] self.users = ['testuser', 'user0', 'user1'] self.coordinates = [ (lat, lon, units.feet_to_meters(alt)) for lat, lon, alt in [ (-76.0, 38.0, 0.0), (-76.0, 38.0, 10.0), (-76.0, 38.0, 20.0), (-76.0, 38.0, 30.0), (-76.0, 38.0, 100.0), (-76.0, 38.0, 30.0), (-77.0, 38.0, 60.0), ] ]
def uas_telemetry_live_kml(kml, timespan): users = User.objects.order_by('username').all() for user in users: try: log = UasTelemetry.by_user(user).latest('timestamp') except UasTelemetry.DoesNotExist: continue if log.timestamp < timezone.now() - timespan: continue point = kml.newpoint(name=user.username, coords=[(log.longitude, log.latitude, units.feet_to_meters(log.altitude_msl))]) point.iconstyle.icon.href = KML_PLANE_ICON point.iconstyle.heading = log.uas_heading point.extrude = 1 # Extend path to ground point.altitudemode = AltitudeMode.absolute
def kml(self, time_periods, kml, kml_doc): """ Appends kml nodes describing the given user's flight as described by the log array given. No nodes are added if less than two log entries are given. Args: time_periods: The time period over which to generate positions. kml: A simpleKML Container to which the flight data will be added kml_doc: The simpleKML Document to which schemas will be added Returns: None """ # KML Compliant Datetime Formatter kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ" icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png' # Generate track data for all time periods. coords = [] when = [] ranges = [] for period in time_periods: t = period.start while t < period.end: (lat, lon, alt) = self.get_position(t) coords.append((lon, lat, units.feet_to_meters(alt))) when.append(t.strftime(kml_datetime_format)) t += timedelta(milliseconds=100) # Create a new track in the folder trk = kml.newgxtrack(name='Obstacle Path {}'.format(self.id)) trk.altitudemode = AltitudeMode.absolute # TODO: Add back proximity information lost when fixing #316. # Append flight data trk.newwhen(when) trk.newgxcoord(coords) # Set styling trk.extrude = 1 # Extend path to ground trk.style.linestyle.width = 2 trk.style.linestyle.color = Color.red trk.iconstyle.icon.href = icon
def kml(self, kml): """ Appends kml nodes describing this flyzone. Args: kml: A simpleKML Container to which the fly zone will be added """ zone_name = 'Fly Zone {}'.format(self.pk) pol = kml.newpolygon(name=zone_name) fly_zone = [] for point in self.boundary_pts.order_by('order'): gps = point.position.gps_position coord = (gps.longitude, gps.latitude, units.feet_to_meters(point.position.altitude_msl)) fly_zone.append(coord) fly_zone.append(fly_zone[0]) pol.outerboundaryis = fly_zone pol.style.linestyle.color = Color.red pol.style.linestyle.width = 3 pol.style.polystyle.color = Color.changealphaint(50, Color.green)
def fly_zone_kml(fly_zone, kml): """ Appends kml nodes describing the flyzone. Args: fly_zone: The FlyZone for which to add KML kml: A simpleKML Container to which the fly zone will be added """ zone_name = 'Fly Zone {}'.format(fly_zone.pk) pol = kml.newpolygon(name=zone_name) fly_zone_points = [] for point in fly_zone.boundary_pts.order_by('order'): coord = (point.longitude, point.latitude, units.feet_to_meters(point.altitude_msl)) fly_zone_points.append(coord) fly_zone_points.append(fly_zone_points[0]) pol.outerboundaryis = fly_zone_points pol.style.linestyle.color = Color.red pol.style.linestyle.width = 3 pol.style.polystyle.color = Color.changealphaint(50, Color.green)
def live_kml(cls, kml, timespan): users = User.objects.all() for user in users: period_logs = UasTelemetry.by_user(user)\ .filter(timestamp__gt=timezone.now() - timespan) if len(period_logs) < 1: continue linestring = kml.newlinestring(name=user.username) coords = [] for entry in period_logs: pos = entry.uas_position.gps_position # Spatial Coordinates coord = (pos.longitude, pos.latitude, units.feet_to_meters(entry.uas_position.altitude_msl)) coords.append(coord) linestring.coords = coords linestring.altitudemode = AltitudeMode.absolute linestring.extrude = 1 linestring.style.linestyle.color = Color.blue linestring.style.polystyle.color = Color.changealphaint( 100, Color.blue)
def live_kml(cls, kml, timespan): users = User.objects.all() for user in users: period_logs = UasTelemetry.by_user(user)\ .filter(timestamp__gt=timezone.now() - timespan) if len(period_logs) < 1: continue linestring = kml.newlinestring(name=user.username) coords = [] for entry in period_logs: pos = entry.uas_position.gps_position # Spatial Coordinates coord = (pos.longitude, pos.latitude, units.feet_to_meters(entry.uas_position.altitude_msl)) coords.append(coord) linestring.coords = coords linestring.altitudemode = AltitudeMode.absolute linestring.extrude = 1 linestring.style.linestyle.color = Color.blue linestring.style.polystyle.color = Color.changealphaint(100, Color.blue)
def kml(self, kml, kml_doc): """ Appends kml nodes describing this mission configurations. Args: kml: A simpleKML Container to which the mission data will be added kml_doc: The simpleKML Document to which schemas will be added """ mission_name = 'Mission {}'.format(self.pk) kml_folder = kml.newfolder(name=mission_name) # Static Points locations = { 'Home Position': self.home_pos, 'Emergent LKP': self.emergent_last_known_pos, 'Off Axis': self.off_axis_odlc_pos, 'Air Drop': self.air_drop_pos, } for key, point in locations.iteritems(): gps = (point.longitude, point.latitude) wp = kml_folder.newpoint(name=key, coords=[gps]) wp.description = str(point) # Waypoints waypoints_folder = kml_folder.newfolder(name='Waypoints') linestring = waypoints_folder.newlinestring(name='Waypoints') waypoints = [] waypoint_num = 1 for waypoint in self.mission_waypoints.order_by('order'): gps = waypoint.position.gps_position coord = (gps.longitude, gps.latitude, units.feet_to_meters(waypoint.position.altitude_msl)) waypoints.append(coord) # Add waypoint marker wp = waypoints_folder.newpoint(name=str(waypoint_num), coords=[coord]) wp.description = str(waypoint) wp.altitudemode = AltitudeMode.absolute wp.extrude = 1 wp.visibility = False waypoint_num += 1 linestring.coords = waypoints # Waypoints Style linestring.altitudemode = AltitudeMode.absolute linestring.extrude = 1 linestring.style.linestyle.color = Color.black linestring.style.polystyle.color = Color.changealphaint( 100, Color.green) # Search Area search_area_folder = kml_folder.newfolder(name='Search Area') search_area = [] search_area_num = 1 for point in self.search_grid_points.order_by('order'): gps = point.position.gps_position coord = (gps.longitude, gps.latitude, units.feet_to_meters(point.position.altitude_msl)) search_area.append(coord) # Add boundary marker wp = search_area_folder.newpoint(name=str(search_area_num), coords=[coord]) wp.description = str(point) wp.visibility = False search_area_num += 1 if search_area: # Create search area polygon. pol = search_area_folder.newpolygon(name='Search Area') search_area.append(search_area[0]) pol.outerboundaryis = search_area # Search Area Style. pol.style.linestyle.color = Color.black pol.style.linestyle.width = 2 pol.style.polystyle.color = Color.changealphaint(50, Color.blue) # Stationary Obstacles. stationary_obstacles_folder = kml_folder.newfolder( name='Stationary Obstacles') # TODO: Implement # Moving Obstacles. users = User.objects.all() moving_obstacle_periods = [] for user in users: moving_obstacle_periods.extend(TakeoffOrLandingEvent.flights(user)) moving_obstacles_folder = kml_folder.newfolder(name='Moving Obstacles') for obstacle in self.moving_obstacles.all(): obstacle.kml(moving_obstacle_periods, moving_obstacles_folder, kml_doc)
def distance_to_line(start, end, point, utm): """Compute the closest distance from point to a line segment. Based on the point-line distance derived in: http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html (l_1 - p) dot (l_2 - p) t = - ----------------------- |l_2 - l_1|^2 We clamp t to 0 <= t <= 1 to ensure we calculate distance to a point on the line segment. The closest point can be determined using t: p_c = l_1 + t * (l_2 - l_1) And the distance is: d = |p - p_c| Arguments are points in the form (lat, lon, alt MSL (ft)). Args: start: Defines the start of the line. end: Defines the end of the line. point: Free point to compute distance from. utm: The UTM Proj projection to project into. If start, end, or point are well outside of this projection, this function returns infinite. Returns: Closest distance in ft from point to the line. """ try: # Convert points to UTM projection. # We need a cartesian coordinate system to perform the calculation. lat, lon, ftmsl = start x, y = pyproj.transform(wgs84, utm, lon, lat) l1 = np.array([x, y, units.feet_to_meters(ftmsl)]) lat, lon, ftmsl = end x, y = pyproj.transform(wgs84, utm, lon, lat) l2 = np.array([x, y, units.feet_to_meters(ftmsl)]) lat, lon, ftmsl = point x, y = pyproj.transform(wgs84, utm, lon, lat) p = np.array([x, y, units.feet_to_meters(ftmsl)]) except RuntimeError: # pyproj throws RuntimeError if the coordinates are grossly beyond the # bounds of the projection. We simplify this to "infinite" distance. return float("inf") d1 = l1 - p d2 = l2 - l1 num = np.dot(d1, d2) dem = np.linalg.norm(l2 - l1)**2 t = -num / dem if t < 0: t = 0 elif t > 1: t = 1 p_c = l1 + t * (l2 - l1) dist = np.linalg.norm(p - p_c) return units.meters_to_feet(dist)
def kml(self, kml): """ Appends kml nodes describing this mission configurations. Args: kml: A simpleKML Container to which the mission data will be added """ mission_name = 'Mission {}'.format(self.pk) kml_folder = kml.newfolder(name=mission_name) # Static Points locations = { 'Home Position': self.home_pos, 'Emergent LKP': self.emergent_last_known_pos, 'Off Axis': self.off_axis_target_pos, 'SRIC': self.sric_pos, 'Air Drop': self.air_drop_pos, } for key, point in locations.iteritems(): gps = (point.longitude, point.latitude) wp = kml_folder.newpoint(name=key, coords=[gps]) wp.description = str(point) # Waypoints waypoints_folder = kml_folder.newfolder(name='Waypoints') linestring = waypoints_folder.newlinestring(name='Waypoints') waypoints = [] waypoint_num = 1 for waypoint in self.mission_waypoints.all(): gps = waypoint.position.gps_position coord = (gps.longitude, gps.latitude, units.feet_to_meters(waypoint.position.altitude_msl)) waypoints.append(coord) # Add waypoint marker wp = waypoints_folder.newpoint(name=str(waypoint_num), coords=[coord]) wp.description = str(waypoint) wp.altitudemode = AltitudeMode.absolute wp.extrude = 1 wp.visibility = False waypoint_num += 1 linestring.coords = waypoints # Waypoints Style linestring.altitudemode = AltitudeMode.absolute linestring.extrude = 1 linestring.style.linestyle.color = Color.black linestring.style.polystyle.color = Color.changealphaint(100, Color.green) # Search Area search_area_folder = kml_folder.newfolder(name='Search Area') search_area = [] search_area_num = 1 for point in self.search_grid_points.all(): gps = point.position.gps_position coord = (gps.longitude, gps.latitude, units.feet_to_meters(point.position.altitude_msl)) search_area.append(coord) # Add boundary marker wp = search_area_folder.newpoint(name=str(search_area_num), coords=[coord]) wp.description = str(point) wp.visibility = False search_area_num += 1 if search_area: # Create search area polygon. pol = search_area_folder.newpolygon(name='Search Area') search_area.append(search_area[0]) pol.outerboundaryis = search_area # Search Area Style. pol.style.linestyle.color = Color.black pol.style.linestyle.width = 2 pol.style.polystyle.color = Color.changealphaint(50, Color.blue) # Stationary Obstacles stationary_obstacles_folder = kml_folder.newfolder( name='Stationary Obstacles')
def determine_interpolated_collision(self, start_log, end_log, utm): """Determines whether the UAS collided with the obstacle by interpolating between start and end telemetry. Args: start_log: A UAS telemetry log. end_log: A UAS telemetry log. utm: The UTM Proj projection to project into. Returns: True if the UAS collided with the obstacle, False otherwise. """ start_lat = start_log.uas_position.gps_position.latitude start_lon = start_log.uas_position.gps_position.longitude start_alt = start_log.uas_position.altitude_msl end_lat = end_log.uas_position.gps_position.latitude end_lon = end_log.uas_position.gps_position.longitude end_alt = end_log.uas_position.altitude_msl latc = self.gps_position.latitude lonc = self.gps_position.longitude # First, check if altitude for both logs is above cylinder height. if start_alt > self.cylinder_height and end_alt > self.cylinder_height: return False # We want to check if the line drawn between start_log and end_log # ever crosses the obstacle. # We do this by looking at only the x, y dimensions and checking if the # 2d line intersects with the circle (cylindrical obstacle # cross-section). We then use the line equation to determine the # altitude at which the intersection occurs. If the altitude is between # 0 and self.cylinder_height, a collision occured. # Reference: https://math.stackexchange.com/questions/980089/how-to-check-if-a-3d-line-segment-intersects-a-cylinder # Convert points to UTM projection. # We need a cartesian coordinate system to perform the calculation. try: x1, y1 = pyproj.transform(distance.wgs84, utm, start_lon, start_lat) z1 = units.feet_to_meters(start_alt) x2, y2 = pyproj.transform(distance.wgs84, utm, end_lon, end_lat) z2 = units.feet_to_meters(end_alt) cx, cy = pyproj.transform(distance.wgs84, utm, lonc, latc) except RuntimeError: # pyproj throws RuntimeError if the coordinates are grossly beyond # the bounds of the projection. We do not count this as a collision. return False rm = units.feet_to_meters(self.cylinder_radius) hm = units.feet_to_meters(self.cylinder_height) # Calculate equation of line and substitute into circle equation. # Equation of obstacle circle: # (x - latc)^2 + (y - laty)^2 = self.cylinder_radius^2 if x2 - x1 == 0: # If delta X is 0, substitute X as constant and solve for y. p = [1, -2 * cy, cy**2 + (x1 - cx)**2 - rm**2] roots = np.roots(p) for root in roots: # Solve for altitude and check if within bounds. zcalc = ((root - y1) * (z2 - z1) / (y2 - y1)) + z1 if np.isreal(root) and zcalc <= hm: return True else: # Calculate slope and intercept of line between start and end log. m = (y2 - y1) / (x2 - x1) b = y1 - m * x1 # Substitute in line equation and solve for x. p = [ m**2 + 1, (2 * m * (b - cy)) - (2 * cx), cx**2 + (b - cy)**2 - rm**2 ] roots = np.roots(p) for root in roots: # Solve for altitude and check if within bounds. zcalc = ((root - x1) * (z2 - z1) / (x2 - x1)) + z1 if np.isreal(root) and zcalc <= hm: return True return False
def mission_kml(mission, kml, kml_doc): """ Appends kml nodes describing the mission. Args: mission: The mission to add to the KML. kml: A simpleKML Container to which the mission data will be added kml_doc: The simpleKML Document to which schemas will be added Returns: The KML folder for the mission data. """ mission_name = 'Mission {}'.format(mission.pk) kml_folder = kml.newfolder(name=mission_name) # Transform from WGS84 to UTM at home position. wgs_to_utm = pyproj.transformer.Transformer.from_proj( distance.proj_wgs84, distance.proj_utm(mission.home_pos.latitude, mission.home_pos.longitude)) # Transform from WGS84 to Web Mercator. wgs_to_web_mercator = pyproj.transformer.Transformer.from_proj( distance.proj_wgs84, distance.proj_web_mercator) # Flight boundaries. fly_zone_folder = kml_folder.newfolder(name='Fly Zones') for flyzone in mission.fly_zones.all(): fly_zone_kml(flyzone, fly_zone_folder) # Static points. locations = [ ('Home', mission.home_pos, KML_HOME_ICON), ('Emergent LKP', mission.emergent_last_known_pos, KML_ODLC_ICON), ('Off Axis', mission.off_axis_odlc_pos, KML_ODLC_ICON), ('Air Drop', mission.air_drop_pos, KML_DROP_ICON), ('Map Center', mission.map_center_pos, KML_MAP_CENTER_ICON), ] for key, point, icon in locations: gps = (point.longitude, point.latitude) p = kml_folder.newpoint(name=key, coords=[gps]) p.iconstyle.icon.href = icon p.description = str(point) # ODLCs. oldc_folder = kml_folder.newfolder(name='ODLCs') for odlc in mission.odlcs.select_related().all(): name = 'ODLC %d' % odlc.pk gps = (odlc.location.longitude, odlc.location.latitude) p = oldc_folder.newpoint(name=name, coords=[gps]) p.iconstyle.icon.href = KML_ODLC_ICON p.description = name # Waypoints waypoints_folder = kml_folder.newfolder(name='Waypoints') linestring = waypoints_folder.newlinestring(name='Waypoints') waypoints = [] for i, waypoint in enumerate(mission.mission_waypoints.order_by('order')): coord = (waypoint.longitude, waypoint.latitude, units.feet_to_meters(waypoint.altitude_msl)) waypoints.append(coord) # Add waypoint marker p = waypoints_folder.newpoint(name='Waypoint %d' % (i + 1), coords=[coord]) p.iconstyle.icon.href = KML_WAYPOINT_ICON p.description = str(waypoint) p.altitudemode = AltitudeMode.absolute p.extrude = 1 linestring.coords = waypoints linestring.altitudemode = AltitudeMode.absolute linestring.extrude = 1 linestring.style.linestyle.color = Color.green linestring.style.polystyle.color = Color.changealphaint(100, Color.green) # Search Area search_area = [] for point in mission.search_grid_points.order_by('order'): coord = (point.longitude, point.latitude, units.feet_to_meters(point.altitude_msl)) search_area.append(coord) if search_area: search_area.append(search_area[0]) pol = kml_folder.newpolygon(name='Search Area') pol.outerboundaryis = search_area pol.style.linestyle.color = Color.blue pol.style.linestyle.width = 2 pol.style.polystyle.color = Color.changealphaint(50, Color.blue) # Map Area map_x, map_y = wgs_to_web_mercator.transform( mission.map_center_pos.longitude, mission.map_center_pos.latitude) map_height = units.feet_to_meters(mission.map_height_ft) map_width = (map_height * 16) / 9 map_points = [ (map_x - map_width / 2, map_y - map_height / 2), (map_x + map_width / 2, map_y - map_height / 2), (map_x + map_width / 2, map_y + map_height / 2), (map_x - map_width / 2, map_y + map_height / 2), (map_x - map_width / 2, map_y - map_height / 2), ] map_points = [ wgs_to_web_mercator.transform( px, py, direction=pyproj.enums.TransformDirection.INVERSE) for (px, py) in map_points ] map_points = [(x, y, 0) for (x, y) in map_points] map_pol = kml_folder.newpolygon(name='Map') map_pol.outerboundaryis = map_points map_pol.style.linestyle.color = Color.green map_pol.style.linestyle.width = 2 map_pol.style.polystyle.color = Color.changealphaint(50, Color.green) # Stationary Obstacles. stationary_obstacles_folder = kml_folder.newfolder( name='Stationary Obstacles') for obst in mission.stationary_obstacles.all(): cx, cy = wgs_to_utm.transform(obst.longitude, obst.latitude) rm = units.feet_to_meters(obst.cylinder_radius) hm = units.feet_to_meters(obst.cylinder_height) obst_points = [] for angle in np.linspace(0, 2 * math.pi, num=KML_OBST_NUM_POINTS): px = cx + rm * math.cos(angle) py = cy + rm * math.sin(angle) lon, lat = wgs_to_utm.transform( px, py, direction=pyproj.enums.TransformDirection.INVERSE) obst_points.append((lon, lat, hm)) pol = stationary_obstacles_folder.newpolygon(name='Obstacle %d' % obst.pk) pol.outerboundaryis = obst_points pol.altitudemode = AltitudeMode.absolute pol.extrude = 1 pol.style.linestyle.color = Color.yellow pol.style.linestyle.width = 2 pol.style.polystyle.color = Color.changealphaint(50, Color.yellow) return kml_folder
def kml(cls, user, logs, kml, kml_doc): """ Appends kml nodes describing the given user's flight as described by the log array given. Args: user: A Django User to get username from logs: A list of UasTelemetry elements kml: A simpleKML Container to which the flight data will be added kml_doc: The simpleKML Document to which schemas will be added Returns: None """ # KML Compliant Datetime Formatter kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ" icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png' threshold = 1 # Degrees kml_folder = kml.newfolder(name=user.username) flights = TakeoffOrLandingEvent.flights(user) if len(flights) == 0: return logs = filter(lambda log: cls._is_bad_position(log, threshold), logs) for i, flight in enumerate(flights): label = 'Flight {}'.format(i + 1) # Flights are one-indexed kml_flight = kml_folder.newfolder(name=label) flight_logs = filter(lambda x: flight.within(x.timestamp), logs) if len(flight_logs) < 2: continue coords = [] angles = [] when = [] for entry in flight_logs: pos = entry.uas_position.gps_position # Spatial Coordinates coord = (pos.longitude, pos.latitude, units.feet_to_meters(entry.uas_position.altitude_msl)) coords.append(coord) # Time Elements time = entry.timestamp.strftime(kml_datetime_format) when.append(time) # Degrees heading, tilt, and roll angle = (entry.uas_heading, 0.0, 0.0) angles.append(angle) # Create a new track in the folder trk = kml_flight.newgxtrack(name='Flight Path') trk.altitudemode = AltitudeMode.absolute # Append flight data trk.newwhen(when) trk.newgxcoord(coords) trk.newgxangle(angles) # Set styling trk.extrude = 1 # Extend path to ground trk.style.linestyle.width = 2 trk.style.linestyle.color = Color.blue trk.iconstyle.icon.href = icon for obstacle in MovingObstacle.objects.all(): obstacle.kml(path=flight_logs, kml=kml_flight, kml_doc=kml_doc)
def kml(cls, user, logs, kml, kml_doc): """ Appends kml nodes describing the given user's flight as described by the log array given. Args: user: A Django User to get username from logs: A list of UasTelemetry elements kml: A simpleKML Container to which the flight data will be added kml_doc: The simpleKML Document to which schemas will be added Returns: None """ # KML Compliant Datetime Formatter kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ" icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png' kml_folder = kml.newfolder(name=user.username) flights = TakeoffOrLandingEvent.flights(user) if len(flights) == 0: return logs = UasTelemetry.dedupe(UasTelemetry.filter_bad(logs)) for i, flight in enumerate(flights): label = 'Flight {}'.format(i + 1) # Flights are one-indexed kml_flight = kml_folder.newfolder(name=label) flight_logs = filter(lambda x: flight.within(x.timestamp), logs) if len(flight_logs) < 2: continue coords = [] angles = [] when = [] for entry in flight_logs: pos = entry.uas_position.gps_position # Spatial Coordinates coord = (pos.longitude, pos.latitude, units.feet_to_meters(entry.uas_position.altitude_msl)) coords.append(coord) # Time Elements time = entry.timestamp.strftime(kml_datetime_format) when.append(time) # Degrees heading, tilt, and roll angle = (entry.uas_heading, 0.0, 0.0) angles.append(angle) # Create a new track in the folder trk = kml_flight.newgxtrack(name='Flight Path') trk.altitudemode = AltitudeMode.absolute # Append flight data trk.newwhen(when) trk.newgxcoord(coords) trk.newgxangle(angles) # Set styling trk.extrude = 1 # Extend path to ground trk.style.linestyle.width = 2 trk.style.linestyle.color = Color.blue trk.iconstyle.icon.href = icon
def mission_kml(mission, kml, kml_doc): """ Appends kml nodes describing the mission. Args: mission: The mission to add to the KML. kml: A simpleKML Container to which the mission data will be added kml_doc: The simpleKML Document to which schemas will be added Returns: The KML folder for the mission data. """ mission_name = 'Mission {}'.format(mission.pk) kml_folder = kml.newfolder(name=mission_name) # Flight boundaries. fly_zone_folder = kml_folder.newfolder(name='Fly Zones') for flyzone in mission.fly_zones.all(): fly_zone_kml(flyzone, fly_zone_folder) # Static points. locations = [ ('Home', mission.home_pos, KML_HOME_ICON), ('Emergent LKP', mission.emergent_last_known_pos, KML_ODLC_ICON), ('Off Axis', mission.off_axis_odlc_pos, KML_ODLC_ICON), ('Air Drop', mission.air_drop_pos, KML_DROP_ICON), ] for key, point, icon in locations: gps = (point.longitude, point.latitude) p = kml_folder.newpoint(name=key, coords=[gps]) p.iconstyle.icon.href = icon p.description = str(point) # ODLCs. oldc_folder = kml_folder.newfolder(name='ODLCs') for odlc in mission.odlcs.all(): name = 'ODLC %d' % odlc.pk gps = (odlc.location.longitude, odlc.location.latitude) p = oldc_folder.newpoint(name=name, coords=[gps]) p.iconstyle.icon.href = KML_ODLC_ICON p.description = name # Waypoints waypoints_folder = kml_folder.newfolder(name='Waypoints') linestring = waypoints_folder.newlinestring(name='Waypoints') waypoints = [] for i, waypoint in enumerate(mission.mission_waypoints.order_by('order')): gps = waypoint.position.gps_position coord = (gps.longitude, gps.latitude, units.feet_to_meters(waypoint.position.altitude_msl)) waypoints.append(coord) # Add waypoint marker p = waypoints_folder.newpoint(name='Waypoint %d' % (i + 1), coords=[coord]) p.iconstyle.icon.href = KML_WAYPOINT_ICON p.description = str(waypoint) p.altitudemode = AltitudeMode.absolute p.extrude = 1 linestring.coords = waypoints linestring.altitudemode = AltitudeMode.absolute linestring.extrude = 1 linestring.style.linestyle.color = Color.green linestring.style.polystyle.color = Color.changealphaint(100, Color.green) # Search Area search_area = [] for point in mission.search_grid_points.order_by('order'): gps = point.position.gps_position coord = (gps.longitude, gps.latitude, units.feet_to_meters(point.position.altitude_msl)) search_area.append(coord) if search_area: search_area.append(search_area[0]) pol = kml_folder.newpolygon(name='Search Area') pol.outerboundaryis = search_area pol.style.linestyle.color = Color.blue pol.style.linestyle.width = 2 pol.style.polystyle.color = Color.changealphaint(50, Color.blue) # Stationary Obstacles. stationary_obstacles_folder = kml_folder.newfolder( name='Stationary Obstacles') for obst in mission.stationary_obstacles.all(): gpos = obst.gps_position zone, north = distance.utm_zone(gpos.latitude, gpos.longitude) proj = distance.proj_utm(zone, north) cx, cy = proj(gpos.longitude, gpos.latitude) rm = units.feet_to_meters(obst.cylinder_radius) hm = units.feet_to_meters(obst.cylinder_height) obst_points = [] for angle in np.linspace(0, 2 * math.pi, num=KML_OBST_NUM_POINTS): px = cx + rm * math.cos(angle) py = cy + rm * math.sin(angle) lon, lat = proj(px, py, inverse=True) obst_points.append((lon, lat, hm)) pol = stationary_obstacles_folder.newpolygon(name='Obstacle %d' % obst.pk) pol.outerboundaryis = obst_points pol.altitudemode = AltitudeMode.absolute pol.extrude = 1 pol.style.linestyle.color = Color.yellow pol.style.linestyle.width = 2 pol.style.polystyle.color = Color.changealphaint(50, Color.yellow) return kml_folder
def determine_interpolated_collision(self, start_log, end_log, utm): """Determines whether the UAS collided with the obstacle by interpolating between start and end telemetry. Args: start_log: A UAS telemetry log. end_log: A UAS telemetry log. utm: The UTM Proj projection to project into. Returns: True if the UAS collided with the obstacle, False otherwise. """ start_lat = start_log.uas_position.gps_position.latitude start_lon = start_log.uas_position.gps_position.longitude start_alt = start_log.uas_position.altitude_msl end_lat = end_log.uas_position.gps_position.latitude end_lon = end_log.uas_position.gps_position.longitude end_alt = end_log.uas_position.altitude_msl latc = self.gps_position.latitude lonc = self.gps_position.longitude # First, check if altitude for both logs is above cylinder height. if start_alt > self.cylinder_height and end_alt > self.cylinder_height: return False # We want to check if the line drawn between start_log and end_log # ever crosses the obstacle. # We do this by looking at only the x, y dimensions and checking if the # 2d line intersects with the circle (cylindrical obstacle # cross-section). We then use the line equation to determine the # altitude at which the intersection occurs. If the altitude is between # 0 and self.cylinder_height, a collision occured. # Reference: https://math.stackexchange.com/questions/980089/how-to-check-if-a-3d-line-segment-intersects-a-cylinder # Convert points to UTM projection. # We need a cartesian coordinate system to perform the calculation. try: x1, y1 = pyproj.transform(distance.wgs84, utm, start_lon, start_lat) z1 = units.feet_to_meters(start_alt) x2, y2 = pyproj.transform(distance.wgs84, utm, end_lon, end_lat) z2 = units.feet_to_meters(end_alt) cx, cy = pyproj.transform(distance.wgs84, utm, lonc, latc) except RuntimeError: # pyproj throws RuntimeError if the coordinates are grossly beyond # the bounds of the projection. We do not count this as a collision. return False rm = units.feet_to_meters(self.cylinder_radius) hm = units.feet_to_meters(self.cylinder_height) # Calculate equation of line and substitute into circle equation. # Equation of obstacle circle: # (x - latc)^2 + (y - laty)^2 = self.cylinder_radius^2 if x2 - x1 == 0: # If delta X is 0, substitute X as constant and solve for y. p = [1, -2 * cy, cy**2 + (x1 - cx)**2 - rm**2] roots = np.roots(p) for root in roots: # Solve for altitude and check if within bounds. zcalc = ((root - y1) * (z2 - z1) / (y2 - y1)) + z1 if np.isreal(root) and zcalc <= hm: return True else: # Calculate slope and intercept of line between start and end log. m = (y2 - y1) / (x2 - x1) b = y1 - m * x1 # Substitute in line equation and solve for x. p = [m**2 + 1, (2 * m * (b - cy)) - (2 * cx), cx**2 + (b - cy)**2 - rm**2] roots = np.roots(p) for root in roots: # Solve for altitude and check if within bounds. zcalc = ((root - x1) * (z2 - z1) / (x2 - x1)) + z1 if np.isreal(root) and zcalc <= hm: return True return False