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