def test_closest_interpolated_distance(self):
utm = distance.proj_utm(zone=18, north=True)
# Test velocity filter.
waypoint = self.waypoints_from_data([(38, -76, 100)])[0]
entries = [(38, -76, 140), (40, -78, 600)]
logs = self.create_uas_logs(self.user, entries)
d = UasTelemetry.closest_interpolated_distance(logs[0], logs[1],
waypoint, utm)
self.assertAlmostEqual(40.0, d, delta=3)
# Test telemetry rate filter.
waypoint = self.waypoints_from_data([(38.145147, -76.427583, 100)])[0]
entries = [(38.145148, -76.427645, 100), (38.145144, -76.427400, 100)]
logs = self.create_uas_logs(self.user, entries)
logs[1].timestamp = logs[0].timestamp + datetime.timedelta(seconds=2)
d = UasTelemetry.closest_interpolated_distance(logs[0], logs[1],
waypoint, utm)
self.assertAlmostEqual(17.792, d, delta=3)
# Test interpolation (waypoint is halfway between telemetry logs).
waypoint = self.waypoints_from_data([(38.145146, -76.427522, 80)])[0]
entries = [(38.145148, -76.427645, 100), (38.145144, -76.427400, 100)]
logs = self.create_uas_logs(self.user, entries)
logs[1].timestamp = logs[0].timestamp + datetime.timedelta(seconds=1)
d = UasTelemetry.closest_interpolated_distance(logs[0], logs[1],
waypoint, utm)
self.assertAlmostEqual(20.0, d, delta=3)
def test_closest_interpolated_distance(self):
utm = distance.proj_utm(zone=18, north=True)
# Test velocity filter.
waypoint = self.waypoints_from_data([(38, -76, 100)])[0]
entries = [(38, -76, 140), (40, -78, 600)]
logs = self.create_uas_logs(self.user, entries)
d = UasTelemetry.closest_interpolated_distance(logs[0], logs[1],
waypoint, utm)
self.assertAlmostEqual(40.0, d, delta=3)
# Test telemetry rate filter.
waypoint = self.waypoints_from_data([(38.145147, -76.427583, 100)])[0]
entries = [(38.145148, -76.427645, 100), (38.145144, -76.427400, 100)]
logs = self.create_uas_logs(self.user, entries)
logs[1].timestamp = logs[0].timestamp + datetime.timedelta(seconds=2)
d = UasTelemetry.closest_interpolated_distance(logs[0], logs[1],
waypoint, utm)
self.assertAlmostEqual(17.792, d, delta=3)
# Test interpolation (waypoint is halfway between telemetry logs).
waypoint = self.waypoints_from_data([(38.145146, -76.427522, 80)])[0]
entries = [(38.145148, -76.427645, 100), (38.145144, -76.427400, 100)]
logs = self.create_uas_logs(self.user, entries)
logs[1].timestamp = logs[0].timestamp + datetime.timedelta(seconds=1)
d = UasTelemetry.closest_interpolated_distance(logs[0], logs[1],
waypoint, utm)
self.assertAlmostEqual(20.0, d, delta=3)
class TestDistanceToLine(TestCase):
"""Tests distance_to_line."""
# Use UTM 18N for all test cases.
utm = distance.proj_utm(zone=18, north=True)
def evaluate_inputs(self, inputs):
"""Evaluates a list of test cases."""
for start, end, point, expected in inputs:
dist = distance.distance_to_line(start, end, point, utm=self.utm)
self.assertAlmostEqual(expected, dist, delta=5) # ft
def test_at_points(self):
"""Test inputs that are at or near the end points."""
self.evaluate_inputs([
# (start, end, point, dist)
((38, -76, 100), (38, -77, 100), (38, -76, 100), 0),
((38, -76, 100), (38, -77, 100), (38, -77, 100), 0),
((38, -76, 100), (38, -77, 100), (38, -77, 0), 100),
((38, -76, 100), (38, -77, 100), (38, -77, 200), 100),
]) # yapf: disable
def test_midpoint(self):
"""Test inputs that are along the middle of the line."""
self.evaluate_inputs([
(
(38.145148, -76.427645, 100), # start
(38.145144, -76.426400, 100), # end
(38.145000, -76.427081, 100), # point
58, # dist
),
(
(38.145148, -76.427645, 100), # start
(38.145144, -76.426400, 200), # end
(38.145000, -76.427081, 100), # point
70, # dist
),
]) # yapf: disable
def test_beyond_points(self):
"""Test inputs that are beyond the end of the line segment."""
self.evaluate_inputs([
(
(38.145148, -76.427645, 100), # start
(38.145144, -76.426400, 100), # end
(38.145165, -76.427923, 100), # point
80, # dist
),
(
(38.145148, -76.427645, 100), # start
(38.145144, -76.426400, 100), # end
(38.145591, -76.426127, 200), # point
207, # dist
),
]) # yapf: disable
def test_determine_interpolated_collision(self):
utm = distance.proj_utm(zone=18, north=True)
(olat, olon, orad, oheight) = TESTDATA_STATOBST_INTERP_OBS
pos = GpsPosition(latitude=olat, longitude=olon)
pos.save()
obst = StationaryObstacle(gps_position=pos,
cylinder_radius=orad,
cylinder_height=oheight)
for (inside, uas_details) in TESTDATA_STATOBST_INTERP_TELEM:
logs = self.create_uas_logs(self.user, uas_details)
self.assertEqual(
obst.determine_interpolated_collision(logs[0], logs[1], utm),
inside)
def evaluate_collision_with_uas(self, uas_telemetry_logs):
"""Evaluates whether the Uas logs indicate a collision.
Args:
uas_telemetry_logs: A list of UasTelemetry logs sorted by timestamp
for which to evaluate.
Returns:
Whether a UAS telemetry log reported indicates a collision with the
obstacle.
"""
obst_pos = self.get_position()
zone, north = distance.utm_zone(obst_pos[0], obst_pos[1])
utm = distance.proj_utm(zone, north)
for i, cur_log in enumerate(uas_telemetry_logs):
(lat, lon, alt) = self.get_position(cur_log.timestamp)
if self.contains_pos(lat, lon, alt, cur_log.uas_position):
return True
if i > 0:
if self.determine_interpolated_collision(
uas_telemetry_logs[i - 1], cur_log, utm):
return True
return False
def evaluate_collision_with_uas(self, uas_telemetry_logs):
"""Evaluates whether the Uas logs indicate a collision.
Args:
uas_telemetry_logs: A list of UasTelemetry logs sorted by timestamp
for which to evaluate.
Returns:
Whether a UAS telemetry log reported indicates a collision with the
obstacle.
"""
zone, north = distance.utm_zone(self.gps_position.latitude,
self.gps_position.longitude)
utm = distance.proj_utm(zone, north)
for i, cur_log in enumerate(uas_telemetry_logs):
cur_log = uas_telemetry_logs[i]
if self.contains_pos(cur_log.uas_position):
return True
if i > 0:
if self.determine_interpolated_collision(
uas_telemetry_logs[i - 1], cur_log, utm):
return True
return False
def satisfied_waypoints(self, uas_telemetry_logs):
"""Determines whether the UAS satisfied the waypoints.
Waypoints must be satisfied in order. The entire pattern may be
restarted at any point. The best (most waypoints satisfied) attempt
will be returned.
Assumes that waypoints are at least
settings.SATISFIED_WAYPOINT_DIST_MAX_FT apart.
Args:
uas_telemetry_logs: A list of UAS Telemetry logs.
Returns:
satisifed: Total number of waypoints hit (must be in order).
satisfied_track: Total number of waypoints hit while remaining
along the track. Once the UAV leaves the track
no more waypoints will count towards this metric.
closest: Dictionary of closest approach distances to each
waypoint.
"""
# Use a common projection in distance_to_line based on the home
# position.
zone, north = distance.utm_zone(self.home_pos.latitude,
self.home_pos.longitude)
utm = distance.proj_utm(zone, north)
waypoints = self.mission_waypoints.order_by('order')
best = {"satisfied": 0, "satisfied_track": 0}
current = {"satisfied": 0, "satisfied_track": 0}
closest = {}
def best_run(prev_best, current):
"""Returns the best of the current run and the previous best."""
if current["satisfied"] > prev_best["satisfied"]:
return current
elif current["satisfied"] == prev_best["satisfied"] and \
current["satisfied_track"] > prev_best["satisfied_track"]:
return current
return prev_best
prev_wpt, curr_wpt = -1, 0
track_ok = True
for uas_log in uas_telemetry_logs:
# At any point the UAV may restart the waypoint pattern, at which
# point we reset the counters.
d0 = uas_log.uas_position.distance_to(waypoints[0].position)
if d0 < settings.SATISFIED_WAYPOINT_DIST_MAX_FT:
best = best_run(best, current)
# The check below will set satisfied and satisfied_track to 1.
current = {"satisfied": 0, "satisfied_track": 0}
prev_wpt, curr_wpt = -1, 0
track_ok = True
# The UAS may pass closer to the waypoint after achieving the capture
# threshold. so continue to look for better passes of the previous
# waypoint until the next is reched.
if prev_wpt >= 0:
dp = uas_log.uas_position.distance_to(waypoints[
prev_wpt].position)
if dp < closest[prev_wpt]:
closest[prev_wpt] = dp
# If the UAS has satisfied all of the waypoints, await starting the
# waypoint pattern again.
if curr_wpt >= len(waypoints):
continue
# Once we have passed the first waypoint, ensure that the UAS
# remains along the waypoint track. We can skip this once they
# fail (until the entire pattern is restarted).
if prev_wpt >= 0 and track_ok:
start = (waypoints[prev_wpt].position.gps_position.latitude,
waypoints[prev_wpt].position.gps_position.longitude,
waypoints[prev_wpt].position.altitude_msl)
end = (waypoints[curr_wpt].position.gps_position.latitude,
waypoints[curr_wpt].position.gps_position.longitude,
waypoints[curr_wpt].position.altitude_msl)
point = (uas_log.uas_position.gps_position.latitude,
uas_log.uas_position.gps_position.longitude,
uas_log.uas_position.altitude_msl)
d = distance.distance_to_line(start, end, point, utm)
if d > settings.WAYPOINT_TRACK_DIST_MAX_FT:
track_ok = False
waypoint = waypoints[curr_wpt]
d = uas_log.uas_position.distance_to(waypoint.position)
if curr_wpt not in closest or d < closest[curr_wpt]:
closest[curr_wpt] = d
if d < settings.SATISFIED_WAYPOINT_DIST_MAX_FT:
current["satisfied"] += 1
if track_ok:
current["satisfied_track"] += 1
curr_wpt += 1
prev_wpt += 1
best = best_run(best, current)
return best["satisfied"], best["satisfied_track"], closest
def satisfied_waypoints(self, uas_telemetry_logs):
"""Determines whether the UAS satisfied the waypoints.
Waypoints must be satisfied in order. The entire pattern may be
restarted at any point. The best (most waypoints satisfied) attempt
will be returned.
Assumes that waypoints are at least
settings.SATISFIED_WAYPOINT_DIST_MAX_FT apart.
Args:
uas_telemetry_logs: A list of UAS Telemetry logs.
Returns:
satisifed: Total number of waypoints hit (must be in order).
satisfied_track: Total number of waypoints hit while remaining
along the track. Once the UAV leaves the track no
more waypoints will count towards this metric.
closest: Dictionary of closest approach distances to
each waypoint.
"""
# Use a common projection in distance_to_line based on the home
# position.
zone, north = distance.utm_zone(self.home_pos.latitude,
self.home_pos.longitude)
utm = distance.proj_utm(zone, north)
waypoints = self.mission_waypoints.order_by('order')
best = {"satisfied": 0, "satisfied_track": 0}
current = {"satisfied": 0, "satisfied_track": 0}
closest = {}
def best_run(prev_best, current):
"""Returns the best of the current run and the previous best."""
if current["satisfied"] > prev_best["satisfied"]:
return current
elif current["satisfied"] == prev_best["satisfied"] and \
current["satisfied_track"] > prev_best["satisfied_track"]:
return current
return prev_best
prev_wpt, curr_wpt = -1, 0
track_ok = True
for uas_log in uas_telemetry_logs:
# At any point the UAV may restart the waypoint pattern, at which
# point we reset the counters.
d0 = uas_log.uas_position.distance_to(waypoints[0].position)
if d0 < settings.SATISFIED_WAYPOINT_DIST_MAX_FT:
best = best_run(best, current)
# The check below will set satisfied and satisfied_track to 1.
current = {"satisfied": 0, "satisfied_track": 0}
prev_wpt, curr_wpt = -1, 0
track_ok = True
# The UAS may pass closer to the waypoint after achieving the capture
# threshold. so continue to look for better passes of the previous
# waypoint until the next is reched.
if prev_wpt >= 0:
dp = uas_log.uas_position.distance_to(
waypoints[prev_wpt].position)
if dp < closest[prev_wpt]:
closest[prev_wpt] = dp
# If the UAS has satisfied all of the waypoints, await starting the
# waypoint pattern again.
if curr_wpt >= len(waypoints):
continue
# Once we have passed the first waypoint, ensure that the UAS
# remains along the waypoint track. We can skip this once they
# fail (until the entire pattern is restarted).
if prev_wpt >= 0 and track_ok:
start = (waypoints[prev_wpt].position.gps_position.latitude,
waypoints[prev_wpt].position.gps_position.longitude,
waypoints[prev_wpt].position.altitude_msl)
end = (waypoints[curr_wpt].position.gps_position.latitude,
waypoints[curr_wpt].position.gps_position.longitude,
waypoints[curr_wpt].position.altitude_msl)
point = (uas_log.uas_position.gps_position.latitude,
uas_log.uas_position.gps_position.longitude,
uas_log.uas_position.altitude_msl)
d = distance.distance_to_line(start, end, point, utm)
if d > settings.WAYPOINT_TRACK_DIST_MAX_FT:
track_ok = False
waypoint = waypoints[curr_wpt]
d = uas_log.uas_position.distance_to(waypoint.position)
if curr_wpt not in closest or d < closest[curr_wpt]:
closest[curr_wpt] = d
if d < settings.SATISFIED_WAYPOINT_DIST_MAX_FT:
current["satisfied"] += 1
if track_ok:
current["satisfied_track"] += 1
curr_wpt += 1
prev_wpt += 1
best = best_run(best, current)
return best["satisfied"], best["satisfied_track"], closest
def test_interpolated_collision(self):
# Get test data
user = User.objects.create_user('testuser', '*****@*****.**',
'testpass')
user.save()
utm = distance.proj_utm(zone=18, north=True)
(obst_rad, obst_speed, obst_pos, log_details) = TESTDATA_MOVOBST_INTERP
# Create the obstacle
obst = MovingObstacle()
obst.speed_avg = obst_speed
obst.sphere_radius = obst_rad
obst.save()
for pos_id in xrange(len(obst_pos)):
(lat, lon, alt) = obst_pos[pos_id]
gpos = GpsPosition()
gpos.latitude = lat
gpos.longitude = lon
gpos.save()
apos = AerialPosition()
apos.gps_position = gpos
apos.altitude_msl = alt
apos.save()
wpt = Waypoint()
wpt.order = pos_id
wpt.position = apos
wpt.save()
obst.waypoints.add(wpt)
obst.save()
for (inside, log_list) in log_details:
logs = []
for i in range(len(log_list)):
lat, lon, alt = log_list[i]
pos = GpsPosition()
pos.latitude = lat
pos.longitude = lon
pos.save()
apos = AerialPosition()
apos.altitude_msl = alt
apos.gps_position = pos
apos.save()
log = UasTelemetry()
log.user = user
log.uas_position = apos
log.uas_heading = 0
log.save()
if i == 0:
log.timestamp = timezone.now().replace(year=1970,
month=1,
day=1,
hour=0,
minute=0,
second=0,
microsecond=0)
if i > 0:
log.timestamp = (
logs[i - 1].timestamp + datetime.timedelta(seconds=1))
logs.append(log)
self.assertEqual(
obst.determine_interpolated_collision(logs[0], logs[1], utm),
inside)
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 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 satisfied_waypoints(cls, home_pos, waypoints, uas_telemetry_logs):
"""Determines whether the UAS satisfied the waypoints.
Waypoints must be satisfied in order. The entire pattern may be
restarted at any point. The best (most waypoints satisfied) attempt
will be returned.
Assumes that waypoints are at least
settings.SATISFIED_WAYPOINT_DIST_MAX_FT apart.
Args:
home_pos: The home position for projections.
waypoints: A list of waypoints to check against.
uas_telemetry_logs: A list of UAS Telemetry logs to evaluate.
Returns:
A list of auvsi_suas.proto.WaypointEvaluation.
"""
# Use a common projection in distance_to_line based on the home
# position.
zone, north = distance.utm_zone(home_pos.latitude, home_pos.longitude)
utm = distance.proj_utm(zone, north)
best = {}
current = {}
closest = {}
def score_waypoint(distance):
"""Scores a single waypoint."""
return max(
0, float(settings.SATISFIED_WAYPOINT_DIST_MAX_FT - distance) /
settings.SATISFIED_WAYPOINT_DIST_MAX_FT)
def score_waypoint_sequence(sequence):
"""Returns scores given distances to a sequence of waypoints."""
score = {}
for i in range(0, len(waypoints)):
score[i] = \
score_waypoint(sequence[i]) if i in sequence else 0.0
return score
def best_run(prev_best, current):
"""Returns the best of the current run and the previous best."""
prev_best_scores = score_waypoint_sequence(prev_best)
current_scores = score_waypoint_sequence(current)
if sum(current_scores.values()) > sum(prev_best_scores.values()):
return current, current_scores
return prev_best, prev_best_scores
prev_wpt, curr_wpt = -1, 0
for uas_log in uas_telemetry_logs:
# At any point the UAV may restart the waypoint pattern, at which
# point we reset the counters.
d0 = uas_log.uas_position.distance_to(waypoints[0].position)
if d0 < settings.SATISFIED_WAYPOINT_DIST_MAX_FT:
best = best_run(best, current)[0]
# Reset current to default values.
current = {}
prev_wpt, curr_wpt = -1, 0
# The UAS may pass closer to the waypoint after achieving the capture
# threshold. so continue to look for better passes of the previous
# waypoint until the next is reched.
if prev_wpt >= 0:
dp = uas_log.uas_position.distance_to(waypoints[
prev_wpt].position)
if dp < closest[prev_wpt]:
closest[prev_wpt] = dp
current[prev_wpt] = dp
# If the UAS has satisfied all of the waypoints, await starting the
# waypoint pattern again.
if curr_wpt >= len(waypoints):
continue
d = uas_log.uas_position.distance_to(waypoints[curr_wpt].position)
if curr_wpt not in closest or d < closest[curr_wpt]:
closest[curr_wpt] = d
if d < settings.SATISFIED_WAYPOINT_DIST_MAX_FT:
current[curr_wpt] = d
curr_wpt += 1
prev_wpt += 1
best, scores = best_run(best, current)
# Convert to evaluation.
waypoint_evals = []
for ix, waypoint in enumerate(waypoints):
waypoint_eval = mission_pb2.WaypointEvaluation()
waypoint_eval.id = ix
waypoint_eval.score_ratio = scores.get(ix, 0)
if ix in best:
waypoint_eval.closest_for_scored_approach_ft = best[ix]
if ix in closest:
waypoint_eval.closest_for_mission_ft = closest[ix]
waypoint_evals.append(waypoint_eval)
return waypoint_evals
def satisfied_waypoints(cls, home_pos, waypoints, uas_telemetry_logs):
"""Determines whether the UAS satisfied the waypoints.
Waypoints must be satisfied in order. The entire pattern may be
restarted at any point. The best (most waypoints satisfied) attempt
will be returned.
Assumes that waypoints are at least
settings.SATISFIED_WAYPOINT_DIST_MAX_FT apart.
Args:
home_pos: The home position for projections.
waypoints: A list of waypoints to check against.
uas_telemetry_logs: A list of UAS Telemetry logs to evaluate.
Returns:
A list of auvsi_suas.proto.WaypointEvaluation.
"""
# Form utm for use as projection in distance calcluations.
zone, north = distance.utm_zone(home_pos.latitude, home_pos.longitude)
utm = distance.proj_utm(zone, north)
# Reduce telemetry from telemetry to waypoint hits.
# This will make future processing more efficient via data reduction.
# While iterating, compute the best distance seen for feedback.
best = {}
hits = []
for iu, start_log in enumerate(uas_telemetry_logs):
end_log = None
if iu + 1 < len(uas_telemetry_logs):
end_log = uas_telemetry_logs[iu + 1]
for iw, waypoint in enumerate(waypoints):
dist = cls.closest_interpolated_distance(
start_log, end_log, waypoint, utm)
best[iw] = min(best.get(iw, dist), dist)
score = cls.score_waypoint(dist)
if score > 0:
hits.append((iw, dist, score))
# Remove redundant hits which wouldn't be part of best sequence.
# This will make future processing more efficient via data reduction.
hits = [
max(g, key=lambda x: x[2])
for _, g in itertools.groupby(hits, lambda x: x[0])
]
# Find highest scoring sequence via dynamic programming.
# Implement recurrence relation:
# S(iw, ih) = s[iw, ih] + max_{k=[0,ih)} S(iw-1, k)
dp = defaultdict(lambda: defaultdict(lambda: (0, None, None)))
highest_total = None
highest_total_pos = (None, None)
for iw in xrange(len(waypoints)):
for ih, (hiw, hdist, hscore) in enumerate(hits):
# Compute score for assigning current hit to current waypoint.
score = hscore if iw == hiw else 0.0
# Compute best total score, which includes this match score and
# best of all which could come before it.
prev_iw = iw - 1
total_score = score
total_score_back = (None, None)
if prev_iw >= 0:
for prev_ih in xrange(ih + 1):
(prev_total_score, _) = dp[prev_iw][prev_ih]
new_total_score = prev_total_score + score
if new_total_score > total_score:
total_score = new_total_score
total_score_back = (prev_iw, prev_ih)
dp[iw][ih] = (total_score, total_score_back)
# Track highest score seen.
if total_score > highest_total:
highest_total = total_score
highest_total_pos = (iw, ih)
# Traceback sequence to get scores and distance for score.
scores = defaultdict(lambda: (0, None))
cur_pos = highest_total_pos
while cur_pos != (None, None):
cur_iw, cur_ih = cur_pos
hiw, hdist, hscore = hits[cur_ih]
if cur_iw == hiw:
scores[cur_iw] = (hscore, hdist)
_, cur_pos = dp[cur_iw][cur_ih]
# Convert to evaluation.
waypoint_evals = []
for iw, waypoint in enumerate(waypoints):
score, dist = scores[iw]
waypoint_eval = mission_pb2.WaypointEvaluation()
waypoint_eval.id = iw
waypoint_eval.score_ratio = score
if dist is not None:
waypoint_eval.closest_for_scored_approach_ft = dist
if iw in best:
waypoint_eval.closest_for_mission_ft = best[iw]
waypoint_evals.append(waypoint_eval)
return waypoint_evals
def satisfied_waypoints(self, uas_telemetry_logs):
"""Determines whether the UAS satisfied the waypoints.
Waypoints must be satisfied in order. The entire pattern may be
restarted at any point. The best (most waypoints satisfied) attempt
will be returned.
Assumes that waypoints are at least
settings.SATISFIED_WAYPOINT_DIST_MAX_FT apart.
Args:
uas_telemetry_logs: A list of UAS Telemetry logs.
Returns:
best: Dictionary of distance to waypoints of the highest scoring
attempt.
scores: Dictionary of individual waypoint scores of the highest
scoring attempt.
closest: Dictionary of closest approach distances to
each waypoint.
"""
# Use a common projection in distance_to_line based on the home
# position.
zone, north = distance.utm_zone(self.home_pos.latitude,
self.home_pos.longitude)
utm = distance.proj_utm(zone, north)
waypoints = self.mission_waypoints.order_by('order')
best = {}
current = {}
closest = {}
def score_waypoint(distance):
"""Scores a single waypoint."""
return max(
0,
float(settings.SATISFIED_WAYPOINT_DIST_MAX_FT - distance) /
settings.SATISFIED_WAYPOINT_DIST_MAX_FT)
def score_waypoint_sequence(sequence):
"""Returns scores given distances to a sequence of waypoints."""
score = {}
for i in range(0, len(waypoints)):
score[i] = \
score_waypoint(sequence[i]) if i in sequence else 0.0
return score
def best_run(prev_best, current):
"""Returns the best of the current run and the previous best."""
prev_best_scores = score_waypoint_sequence(prev_best)
current_scores = score_waypoint_sequence(current)
if sum(current_scores.values()) > sum(prev_best_scores.values()):
return current, current_scores
return prev_best, prev_best_scores
prev_wpt, curr_wpt = -1, 0
for uas_log in uas_telemetry_logs:
# At any point the UAV may restart the waypoint pattern, at which
# point we reset the counters.
d0 = uas_log.uas_position.distance_to(waypoints[0].position)
if d0 < settings.SATISFIED_WAYPOINT_DIST_MAX_FT:
best = best_run(best, current)[0]
# Reset current to default values.
current = {}
prev_wpt, curr_wpt = -1, 0
# The UAS may pass closer to the waypoint after achieving the capture
# threshold. so continue to look for better passes of the previous
# waypoint until the next is reched.
if prev_wpt >= 0:
dp = uas_log.uas_position.distance_to(
waypoints[prev_wpt].position)
if dp < closest[prev_wpt]:
closest[prev_wpt] = dp
current[prev_wpt] = dp
# If the UAS has satisfied all of the waypoints, await starting the
# waypoint pattern again.
if curr_wpt >= len(waypoints):
continue
d = uas_log.uas_position.distance_to(waypoints[curr_wpt].position)
if curr_wpt not in closest or d < closest[curr_wpt]:
closest[curr_wpt] = d
if d < settings.SATISFIED_WAYPOINT_DIST_MAX_FT:
current[curr_wpt] = d
curr_wpt += 1
prev_wpt += 1
best, scores = best_run(best, current)
return best, scores, closest
def test_interpolated_collision(self):
# Get test data
user = User.objects.create_user('testuser', '*****@*****.**',
'testpass')
user.save()
utm = distance.proj_utm(zone=18, north=True)
(obst_rad, obst_speed, obst_pos, log_details) = TESTDATA_MOVOBST_INTERP
# Create the obstacle
obst = MovingObstacle()
obst.speed_avg = obst_speed
obst.sphere_radius = obst_rad
obst.save()
for pos_id in xrange(len(obst_pos)):
(lat, lon, alt) = obst_pos[pos_id]
gpos = GpsPosition()
gpos.latitude = lat
gpos.longitude = lon
gpos.save()
apos = AerialPosition()
apos.gps_position = gpos
apos.altitude_msl = alt
apos.save()
wpt = Waypoint()
wpt.order = pos_id
wpt.position = apos
wpt.save()
obst.waypoints.add(wpt)
obst.save()
for (inside, log_list) in log_details:
logs = []
for i in range(len(log_list)):
lat, lon, alt = log_list[i]
pos = GpsPosition()
pos.latitude = lat
pos.longitude = lon
pos.save()
apos = AerialPosition()
apos.altitude_msl = alt
apos.gps_position = pos
apos.save()
log = UasTelemetry()
log.user = user
log.uas_position = apos
log.uas_heading = 0
log.save()
if i == 0:
log.timestamp = timezone.now().replace(year=1970,
month=1,
day=1,
hour=0,
minute=0,
second=0,
microsecond=0)
if i > 0:
log.timestamp = (logs[i - 1].timestamp +
datetime.timedelta(seconds=1))
logs.append(log)
self.assertEqual(
obst.determine_interpolated_collision(logs[0], logs[1], utm),
inside)
def satisfied_waypoints(cls, home_pos, waypoints, uas_telemetry_logs):
"""Determines whether the UAS satisfied the waypoints.
Waypoints must be satisfied in order. The entire pattern may be
restarted at any point. The best (most waypoints satisfied) attempt
will be returned.
Assumes that waypoints are at least
settings.SATISFIED_WAYPOINT_DIST_MAX_FT apart.
Args:
home_pos: The home position for projections.
waypoints: A list of waypoints to check against.
uas_telemetry_logs: A list of UAS Telemetry logs to evaluate.
Returns:
A list of auvsi_suas.proto.WaypointEvaluation.
"""
# Form utm for use as projection in distance calcluations.
zone, north = distance.utm_zone(home_pos.latitude, home_pos.longitude)
utm = distance.proj_utm(zone, north)
# Reduce telemetry from telemetry to waypoint hits.
# This will make future processing more efficient via data reduction.
# While iterating, compute the best distance seen for feedback.
best = {}
hits = []
for iu, start_log in enumerate(uas_telemetry_logs):
end_log = None
if iu + 1 < len(uas_telemetry_logs):
end_log = uas_telemetry_logs[iu + 1]
for iw, waypoint in enumerate(waypoints):
dist = cls.closest_interpolated_distance(start_log, end_log,
waypoint, utm)
best[iw] = min(best.get(iw, dist), dist)
score = cls.score_waypoint(dist)
if score > 0:
hits.append((iw, dist, score))
# Remove redundant hits which wouldn't be part of best sequence.
# This will make future processing more efficient via data reduction.
hits = [max(g,
key=lambda x: x[2])
for _, g in itertools.groupby(hits, lambda x: x[0])]
# Find highest scoring sequence via dynamic programming.
# Implement recurrence relation:
# S(iw, ih) = s[iw, ih] + max_{k=[0,ih)} S(iw-1, k)
dp = defaultdict(lambda: defaultdict(lambda: (0, None, None)))
highest_total = None
highest_total_pos = (None, None)
for iw in xrange(len(waypoints)):
for ih, (hiw, hdist, hscore) in enumerate(hits):
# Compute score for assigning current hit to current waypoint.
score = hscore if iw == hiw else 0.0
# Compute best total score, which includes this match score and
# best of all which could come before it.
prev_iw = iw - 1
total_score = score
total_score_back = (None, None)
if prev_iw >= 0:
for prev_ih in xrange(ih + 1):
(prev_total_score, _) = dp[prev_iw][prev_ih]
new_total_score = prev_total_score + score
if new_total_score > total_score:
total_score = new_total_score
total_score_back = (prev_iw, prev_ih)
dp[iw][ih] = (total_score, total_score_back)
# Track highest score seen.
if total_score > highest_total:
highest_total = total_score
highest_total_pos = (iw, ih)
# Traceback sequence to get scores and distance for score.
scores = defaultdict(lambda: (0, None))
cur_pos = highest_total_pos
while cur_pos != (None, None):
cur_iw, cur_ih = cur_pos
hiw, hdist, hscore = hits[cur_ih]
if cur_iw == hiw:
scores[cur_iw] = (hscore, hdist)
_, cur_pos = dp[cur_iw][cur_ih]
# Convert to evaluation.
waypoint_evals = []
for iw, waypoint in enumerate(waypoints):
score, dist = scores[iw]
waypoint_eval = mission_pb2.WaypointEvaluation()
waypoint_eval.id = iw
waypoint_eval.score_ratio = score
if dist is not None:
waypoint_eval.closest_for_scored_approach_ft = dist
if iw in best:
waypoint_eval.closest_for_mission_ft = best[iw]
waypoint_evals.append(waypoint_eval)
return waypoint_evals
