def projectBearing(self, bearing, startPos, searchArea): '''Projects bearing from startPos until it reaches the edge(s) of searchArea (list of lat/long tuples. Returns the First/Last position(s)''' coPoints = [] #for each line in the search are border, get any overlaps with startPos on bearing(+-180) for point in searchArea: dist = cuav_util.gps_distance(point[0], point[1], searchArea[searchArea.index(point)-1][0], searchArea[searchArea.index(point)-1][1]) theta2 = cuav_util.gps_bearing(point[0], point[1], searchArea[searchArea.index(point)-1][0], searchArea[searchArea.index(point)-1][1]) posn = self.Intersection(startPos, bearing, point, theta2) if posn != 0 and cuav_util.gps_distance(posn[0], posn[1], point[0], point[1]) < dist: coPoints.append(posn) posn = self.Intersection(startPos, (bearing + 180) % 360, point, theta2) if posn != 0 and cuav_util.gps_distance(posn[0], posn[1], point[0], point[1]) < dist: coPoints.append(posn) #if there's more than two points in coPoints, return the furthest away points if len(coPoints) < 2: #print str(len(coPoints)) + " point i-sect" return 0 elif len(coPoints) == 2: return coPoints else: dist = 0 for point in coPoints: for pt in coPoints: if cuav_util.gps_distance(pt[0], pt[1], point[0], point[1]) > dist: dist = cuav_util.gps_distance(pt[0], pt[1], point[0], point[1]) newcoPoints = [point, pt] return newcoPoints
def mouse_event_view(self, event): '''called on mouse events in View window''' x = event.X y = event.Y if self.current_view >= len(self.images): return image = self.images[self.current_view] latlon = cuav_util.gps_position_from_xy(x, y, image.pos, C=self.c_params, shape=image.shape, altitude=image.pos.altitude) if self.last_view_latlon is None or latlon is None: dist = '' else: distance = cuav_util.gps_distance(self.last_view_latlon[0], self.last_view_latlon[1], latlon[0], latlon[1]) bearing = cuav_util.gps_bearing(self.last_view_latlon[0], self.last_view_latlon[1], latlon[0], latlon[1]) dist = "dist %.1f bearing %.1f alt=%.1f shape=%s" % ( distance, bearing, image.pos.altitude, image.shape) print("-> %s %s %s" % (latlon, image.filename, dist)) self.last_view_latlon = latlon
def filter_radius(regions, latlon, radius): '''filter a list of regions using a search boundary''' ret = [] for r in regions: if r.latlon is None or cuav_util.gps_distance(latlon[0], latlon[1], r.latlon[0], r.latlon[1]) > radius: r.score = 0 ret.append(r) return ret
def filter_radius(regions, latlon, radius): """filter a list of regions using a search boundary""" ret = [] for r in regions: if r.latlon is None or cuav_util.gps_distance(latlon[0], latlon[1], r.latlon[0], r.latlon[1]) > radius: r.score = 0 ret.append(r) return ret
def altitudeCompensation(self, heightAGL, numMaxPoints=100, threshold=5): '''Creates height points (ASL) for each point in searchArea, entry and exit points such that the plane stays a constant altitude above the ground, constrained by a max number of waypoints''' maxDeltaAlt = 0 maxDeltaAltPoints = [] maxDeltapercentAlong = 0 EleModel = mp_elevation.ElevationModel() #make sure the SRTM tiles are downloaded EleModel.GetElevation(self.SearchPattern[0][0], self.SearchPattern[0][1]) #get the ASL height of the airfield home, entry and exit points and initial search pattern # and add the heightAGL to them self.airportHeight = EleModel.GetElevation(self.airfieldHome[0], self.airfieldHome[1]) self.airfieldHome = (self.airfieldHome[0], self.airfieldHome[1], heightAGL+EleModel.GetElevation(self.airfieldHome[0], self.airfieldHome[1])) for point in self.entryPoints: self.entryPoints[self.entryPoints.index(point)] = (point[0], point[1], heightAGL+EleModel.GetElevation(point[0], point[1])) for point in self.exitPoints: self.exitPoints[self.exitPoints.index(point)] = (point[0], point[1], heightAGL+EleModel.GetElevation(point[0], point[1])) for point in self.SearchPattern: self.SearchPattern[self.SearchPattern.index(point)] = (point[0], point[1], heightAGL+EleModel.GetElevation(point[0], point[1])) #keep looping through the search area waypoints and add new waypoints where needed #to maintain const height above terrain print "---Starting terrain tracking optimisation---" while True: maxDeltaAlt = 0 maxDeltaAltPoints = [] maxDeltaPointIndex = 0 for point in self.SearchPattern: if point != self.SearchPattern[-1]: dist = cuav_util.gps_distance(point[0], point[1], self.SearchPattern[self.SearchPattern.index(point)+1][0], self.SearchPattern[self.SearchPattern.index(point)+1][1]) theta = cuav_util.gps_bearing(point[0], point[1], self.SearchPattern[self.SearchPattern.index(point)+1][0], self.SearchPattern[self.SearchPattern.index(point)+1][1]) AltDiff = float(self.SearchPattern[self.SearchPattern.index(point)+1][2] - point[2]) if numpy.around(theta) == numpy.around(self.searchBearing) or numpy.around(theta) == numpy.around((self.searchBearing+180) % 360): #increment 10% along waypoint-to-waypoint and get max height difference for i in range(1, 9): partPoint = cuav_util.gps_newpos(point[0], point[1], theta, (i*dist/10)) partAlt = EleModel.GetElevation(partPoint[0], partPoint[1]) + heightAGL #print "Part = " + str(partAlt) + ", Orig = " + str(point[2]) if numpy.abs(point[2] + ((AltDiff*i)/10) - partAlt) > maxDeltaAlt: maxDeltaAlt = numpy.abs(point[2] + ((AltDiff*i)/10) - partAlt) maxDeltaAltPoint = (partPoint[0], partPoint[1], partAlt) maxDeltaPointIndex = self.SearchPattern.index(point)+1 #print "New max alt diff: " + str(maxDeltaAlt) + ", " + str(partAlt) #now put a extra waypoint in between the two maxDeltaAltPoints if maxDeltaAltPoint is not None: self.SearchPattern.insert(maxDeltaPointIndex, maxDeltaAltPoint) #print "There are " + str(len(self.SearchPattern)) + " points in the search pattern. Max Alt error is " + str(maxDeltaAlt) if len(self.SearchPattern) >= numMaxPoints or threshold > maxDeltaAlt: break print "---Done terrain tracking optimisation---"
def getPolygonLength(self): '''Returns the search pattern path length (metres)''' distance = 0 totPoint = self.entryPoints + self.SearchPattern + self.exitPoints for point in totPoint: if point != totPoint[-1]: distance = distance + cuav_util.gps_distance(point[0], point[1], totPoint[totPoint.index(point)-1][0], totPoint[totPoint.index(point)-1][1]) return int(distance)
def mouse_event_view(self, event): '''called on mouse events in View window''' x = event.X y = event.Y if self.current_view >= len(self.images): return image = self.images[self.current_view] latlon = cuav_util.gps_position_from_xy(x, y, image.pos, C=self.c_params) if self.last_view_latlon is None: dist = '' else: dist = "dist %.1f" % cuav_util.gps_distance(latlon[0], latlon[1], self.last_view_latlon[0], self.last_view_latlon[1]) print("-> %s %s %s" % (latlon, image.filename, dist)) self.last_view_latlon = latlon
def mouse_event_view(self, event): '''called on mouse events in View window''' x = event.X y = event.Y if self.current_view >= len(self.images): return image = self.images[self.current_view] latlon = cuav_util.gps_position_from_xy(x, y, image.pos, C=self.c_params, shape=image.shape) if self.last_view_latlon is None or latlon is None: dist = '' else: distance = cuav_util.gps_distance(self.last_view_latlon[0], self.last_view_latlon[1], latlon[0], latlon[1]) bearing = cuav_util.gps_bearing(self.last_view_latlon[0], self.last_view_latlon[1], latlon[0], latlon[1]) dist = "dist %.1f bearing %.1f alt=%.1f shape=%s" % (distance, bearing, image.pos.altitude, image.shape) print("-> %s %s %s" % (latlon, image.filename, dist)) self.last_view_latlon = latlon
def mouse_event_view(self, event): '''called on mouse events in View window''' x = event.X y = event.Y if self.current_view >= len(self.images): return image = self.images[self.current_view] latlon = cuav_util.gps_position_from_xy(x, y, image.pos, C=self.c_params) if self.last_view_latlon is None: dist = '' else: dist = "dist %.1f" % cuav_util.gps_distance( latlon[0], latlon[1], self.last_view_latlon[0], self.last_view_latlon[1]) print("-> %s %s %s" % (latlon, image.filename, dist)) self.last_view_latlon = latlon
def show_closest(self, latlon, selected): '''show closest camera image''' # first try to show the exact image selected if len(selected) != 0 and self.show_selected(selected[0]): return (lat, lon) = latlon closest = -1 closest_distance = -1 for idx in range(len(self.images)): pos = self.images[idx].pos if pos is not None: distance = cuav_util.gps_distance(lat, lon, pos.lat, pos.lon) if closest == -1 or distance < closest_distance: closest_distance = distance closest = idx if closest == -1: return self.current_view = closest image = self.images[closest] self.view_imagefile(image.filename)
def show_closest(self, latlon): '''show closest camera image''' (lat, lon) = latlon closest = -1 closest_distance = -1 for idx in range(len(self.images)): pos = self.images[idx].pos if pos is not None: distance = cuav_util.gps_distance(lat, lon, pos.lat, pos.lon) if closest == -1 or distance < closest_distance: closest_distance = distance closest = idx if closest == -1: return self.current_view = closest image = self.images[closest] img = cv.LoadImage(image.filename) if self.view_image is None or not self.view_image.is_alive(): import mp_image, wx self.view_image = mp_image.MPImage(title='View', events=[wx.EVT_MOUSE_EVENTS, wx.EVT_KEY_DOWN]) self.view_image.set_image(img, bgr=True)
def distance_from(self, region, center): '''return distance of a region from a center point''' (lat, lon) = region.latlon (clat, clon) = center return cuav_util.gps_distance(lat, lon, clat, clon)
def CreateSearchPattern(self, width=50.0, overlap=10.0, offset=10, wobble=10, alt=100): '''Generate the waypoints for the search pattern, using alternating strips width is the width (m) of each strip, overlap is the % overlap between strips, alt is the altitude (relative to ground) of the points''' self.SearchPattern = [] #find the nearest point to Airfield Home - use this as a starting point (if entry lanes are not used) if len(self.entryPoints) == 0: nearestdist = cuav_util.gps_distance(self.airfieldHome[0], self.airfieldHome[1], self.searchArea[0][0], self.searchArea[0][1]) nearest = self.searchArea[0] for point in self.searchArea: newdist = cuav_util.gps_distance(self.airfieldHome[0], self.airfieldHome[1], point[0], point[1]) if newdist < nearestdist: nearest = point nearestdist = newdist else: nearestdist = cuav_util.gps_distance(self.entryPoints[0][0], self.entryPoints[0][1], self.searchArea[0][0], self.searchArea[0][1]) nearest = self.searchArea[0] for point in self.searchArea: newdist = cuav_util.gps_distance(self.entryPoints[0][0], self.entryPoints[0][1], point[0], point[1]) #print "dist = " + str(newdist) if newdist < nearestdist: nearest = point nearestdist = newdist #print "Start = " + str(nearest) + ", dist = " + str(nearestdist) #the search pattern will run between the longest side from nearest bearing1 = cuav_util.gps_bearing(nearest[0], nearest[1], self.searchArea[self.searchArea.index(nearest)-1][0], self.searchArea[self.searchArea.index(nearest)-1][1]) bearing2 = cuav_util.gps_bearing(nearest[0], nearest[1], self.searchArea[self.searchArea.index(nearest)+1][0], self.searchArea[self.searchArea.index(nearest)+1][1]) dist1 = cuav_util.gps_distance(nearest[0], nearest[1], self.searchArea[self.searchArea.index(nearest)-1][0], self.searchArea[self.searchArea.index(nearest)-1][1]) dist2 = cuav_util.gps_distance(nearest[0], nearest[1], self.searchArea[self.searchArea.index(nearest)+1][0], self.searchArea[self.searchArea.index(nearest)+1][1]) if dist1 > dist2: self.searchBearing = bearing1 else: self.searchBearing = bearing2 #the search pattern will then run parallel between the two furthest points in the list #searchLine = (0, 0) #for point in self.searchArea: # newdist = cuav_util.gps_distance(point[0], point[1], self.searchArea[self.searchArea.index(point)-1][0], self.searchArea[self.searchArea.index(point)-1][1]) # if newdist > searchLine[0]: # searchLine = (newdist, cuav_util.gps_bearing(point[0], point[1], self.searchArea[self.searchArea.index(point)-1][0], self.searchArea[self.searchArea.index(point)-1][1])) #self.searchBearing = searchLine[1] #need to find the 90 degree bearing to searchBearing that is inside the search area. This #will be the bearing we increment the search rows by #need to get the right signs for the bearings, depending which quadrant the search area is in wrt nearest if not cuav_util.polygon_outside(cuav_util.gps_newpos(nearest[0], nearest[1], (self.searchBearing + 45) % 360, 10), self.searchArea): self.crossBearing = (self.searchBearing + 90) % 360 elif not cuav_util.polygon_outside(cuav_util.gps_newpos(nearest[0], nearest[1], (self.searchBearing + 135) % 360, 10), self.searchArea): self.crossBearing = (self.searchBearing + 90) % 360 self.searchBearing = (self.searchBearing + 180) % 360 elif not cuav_util.polygon_outside(cuav_util.gps_newpos(nearest[0], nearest[1], (self.searchBearing - 45) % 360, 10), self.searchArea): self.crossBearing = (self.searchBearing - 90) % 360 else: self.crossBearing = (self.searchBearing - 90) % 360 self.searchBearing = (self.searchBearing - 180) % 360 print "Search bearing is " + str(self.searchBearing) + "/" + str((self.searchBearing + 180) % 360) print "Cross bearing is: " + str(self.crossBearing) #the distance between runs is this: self.deltaRowDist = width - width*(float(overlap)/100) if self.deltaRowDist <= 0: print "Error, overlap % is too high" return print "Delta row = " + str(self.deltaRowDist) #expand the search area to 1/2 deltaRowDist to ensure full coverage #we are starting at the "nearest" and mowing the lawn parallel to "self.searchBearing" #first waypoint is right near the Search Area boundary (without being on it) (10% of deltaRowDist #on an opposite bearing (so behind the search area) nextWaypoint = cuav_util.gps_newpos(nearest[0], nearest[1], self.crossBearing, self.deltaRowDist/10) print "First = " + str(nextWaypoint) #self.SearchPattern.append(firstWaypoint) #mow the lawn, every 2nd row: while True: pts = self.projectBearing(self.searchBearing, nextWaypoint, self.searchArea) #print "Projecting " + str(nextWaypoint) + " along " + str(self.searchBearing) #check if we're outside the search area if pts == 0: break (nextW, nextnextW) = (pts[0], pts[1]) if cuav_util.gps_distance(nextWaypoint[0], nextWaypoint[1], nextW[0], nextW[1]) < cuav_util.gps_distance(nextWaypoint[0], nextWaypoint[1], nextnextW[0], nextnextW[1]): self.SearchPattern.append(cuav_util.gps_newpos(nextW[0], nextW[1], (self.searchBearing + 180) % 360, (offset+wobble))) self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt) self.SearchPattern.append(cuav_util.gps_newpos(nextnextW[0], nextnextW[1], self.searchBearing, (offset+wobble))) self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt) #now turn 90degrees from bearing and width distance nextWaypoint = cuav_util.gps_newpos(nextnextW[0], nextnextW[1], self.crossBearing, self.deltaRowDist*2) self.searchBearing = (self.searchBearing + 180) % 360 else: self.SearchPattern.append(cuav_util.gps_newpos(nextnextW[0], nextnextW[1], (self.searchBearing + 180) % 360, offset+wobble)) self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt) self.SearchPattern.append(cuav_util.gps_newpos(nextW[0], nextW[1], self.searchBearing, (offset+wobble))) self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt) #now turn 90degrees from bearing and width distance nextWaypoint = cuav_util.gps_newpos(nextW[0], nextW[1], self.crossBearing, self.deltaRowDist*2) self.searchBearing = (self.searchBearing + 180) % 360 print "Next = " + str(nextWaypoint) #go back and do the rows we missed. There still might be one more row to do in # the crossbearing direction, so check for that first nextWaypoint = cuav_util.gps_newpos(nextWaypoint[0], nextWaypoint[1], self.crossBearing, -self.deltaRowDist) pts = self.projectBearing(self.searchBearing, nextWaypoint, self.searchArea) if pts == 0: nextWaypoint = cuav_util.gps_newpos(nextWaypoint[0], nextWaypoint[1], self.crossBearing, -2*self.deltaRowDist) self.crossBearing = (self.crossBearing + 180) % 360 else: self.crossBearing = (self.crossBearing + 180) % 360 while True: pts = self.projectBearing(self.searchBearing, nextWaypoint, self.searchArea) #print "Projecting " + str(nextWaypoint) + " along " + str(self.searchBearing) #check if we're outside the search area if pts == 0: break (nextW, nextnextW) = (pts[0], pts[1]) if cuav_util.gps_distance(nextWaypoint[0], nextWaypoint[1], nextW[0], nextW[1]) < cuav_util.gps_distance(nextWaypoint[0], nextWaypoint[1], nextnextW[0], nextnextW[1]): self.SearchPattern.append(cuav_util.gps_newpos(nextW[0], nextW[1], (self.searchBearing + 180) % 360, offset)) self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt) self.SearchPattern.append(cuav_util.gps_newpos(nextnextW[0], nextnextW[1], self.searchBearing, offset)) self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt) #now turn 90degrees from bearing and width distance nextWaypoint = cuav_util.gps_newpos(nextnextW[0], nextnextW[1], self.crossBearing, self.deltaRowDist*2) self.searchBearing = (self.searchBearing + 180) % 360 else: self.SearchPattern.append(cuav_util.gps_newpos(nextnextW[0], nextnextW[1], (self.searchBearing + 180) % 360, offset)) self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt) self.SearchPattern.append(cuav_util.gps_newpos(nextW[0], nextW[1], self.searchBearing, offset)) self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt) #now turn 90degrees from bearing and width distance nextWaypoint = cuav_util.gps_newpos(nextW[0], nextW[1], self.crossBearing, self.deltaRowDist*2) self.searchBearing = (self.searchBearing + 180) % 360 print "Next(alt) = " + str(nextWaypoint) #add in the altitude points (relative to airfield home) for point in self.SearchPattern: self.SearchPattern[self.SearchPattern.index(point)] = (point[0], point[1], alt)