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