def closest_interpolated_distance(start_log, end_log, waypoint, utm):
"""Finds the closest distance to the waypoint by interpolating between
start_log and end_log.
Args:
start_log: A UAS telemetry log.
end_log: A UAS telemetry log.
waypoint: The waypoint for which to find the closest distance.
utm: The UTM Proj projection to project into.
Returns:
The closest distance to the waypoint in feet.
"""
# If no provided end, use start distance.
if end_log is None:
return start_log.uas_position.distance_to(waypoint.position)
# Verify that aircraft velocity is within bounds. Don't interpolate if
# it isn't because the data is probably erroneous.
d = start_log.uas_position.distance_to(end_log.uas_position)
t = (end_log.timestamp - start_log.timestamp).total_seconds()
if (t > settings.MAX_TELMETRY_INTERPOLATE_INTERVAL_SEC
or (d / t) > settings.MAX_AIRSPEED_FT_PER_SEC):
return start_log.uas_position.distance_to(waypoint.position)
def uas_position_to_tuple(pos):
return (pos.gps_position.latitude, pos.gps_position.longitude,
pos.altitude_msl)
# Linearly interpolate between start and end telemetry and find the
# closest distance to the waypoint.
start = uas_position_to_tuple(start_log.uas_position)
end = uas_position_to_tuple(end_log.uas_position)
point = uas_position_to_tuple(waypoint.position)
return distance.distance_to_line(start, end, point, utm)
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 = (start_log.uas_position.gps_position.latitude,
start_log.uas_position.gps_position.longitude,
start_log.uas_position.altitude_msl)
end = (end_log.uas_position.gps_position.latitude,
end_log.uas_position.gps_position.longitude,
end_log.uas_position.altitude_msl)
epoch_time = timezone.now().replace(year=1970,
month=1,
day=1,
hour=0,
minute=0,
second=0,
microsecond=0)
start_seconds = (start_log.timestamp - epoch_time).total_seconds()
end_seconds = (end_log.timestamp - epoch_time).total_seconds()
for s in np.arange(start_seconds, end_seconds,
settings.MOVING_OBSTACLE_INTERPOLATION_INTERVAL):
t = epoch_time + timedelta(seconds=s)
d = distance.distance_to_line(start, end, self.get_position(t),
utm)
if d <= self.sphere_radius:
return True
return False
def closest_interpolated_distance(start_log, end_log, waypoint, utm):
"""Finds the closest distance to the waypoint by interpolating between
start_log and end_log.
Args:
start_log: A UAS telemetry log.
end_log: A UAS telemetry log.
waypoint: The waypoint for which to find the closest distance.
utm: The UTM Proj projection to project into.
Returns:
The closest distance to the waypoint in feet.
"""
# If no provided end, use start distance.
if end_log is None:
return start_log.uas_position.distance_to(waypoint.position)
# Verify that aircraft velocity is within bounds. Don't interpolate if
# it isn't because the data is probably erroneous.
d = start_log.uas_position.distance_to(end_log.uas_position)
t = (end_log.timestamp - start_log.timestamp).total_seconds()
if (t > settings.MAX_TELMETRY_INTERPOLATE_INTERVAL_SEC or
(d / t) > settings.MAX_AIRSPEED_FT_PER_SEC):
return start_log.uas_position.distance_to(waypoint.position)
def uas_position_to_tuple(pos):
return (pos.gps_position.latitude, pos.gps_position.longitude,
pos.altitude_msl)
# Linearly interpolate between start and end telemetry and find the
# closest distance to the waypoint.
start = uas_position_to_tuple(start_log.uas_position)
end = uas_position_to_tuple(end_log.uas_position)
point = uas_position_to_tuple(waypoint.position)
return distance.distance_to_line(start, end, point, utm)
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 = (start_log.uas_position.gps_position.latitude,
start_log.uas_position.gps_position.longitude,
start_log.uas_position.altitude_msl)
end = (end_log.uas_position.gps_position.latitude,
end_log.uas_position.gps_position.longitude,
end_log.uas_position.altitude_msl)
epoch_time = timezone.now().replace(year=1970,
month=1,
day=1,
hour=0,
minute=0,
second=0,
microsecond=0)
start_seconds = (start_log.timestamp - epoch_time).total_seconds()
end_seconds = (end_log.timestamp - epoch_time).total_seconds()
for s in np.arange(start_seconds, end_seconds,
settings.MOVING_OBSTACLE_INTERPOLATION_INTERVAL):
t = epoch_time + timedelta(seconds=s)
d = distance.distance_to_line(start, end, self.get_position(t),
utm)
if d <= self.sphere_radius:
return True
return False
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 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 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
