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, 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)
