def isOutsideSearchAreaBoundingBox(self, lat, longy):
'''Checks if the long/lat pair is inside the search area
bounding box. Returns true if it is inside'''
if cuav_util.polygon_outside((lat, longy), self.boundingBox):
return 1
else:
return 0
def isOutsideSearchAreaBoundingBox(self, lat, longy):
'''Checks if the long/lat pair is inside the search area
bounding box. Returns true if it is inside'''
if cuav_util.polygon_outside((lat, longy), self.boundingBox):
return 1
else:
return 0
def filter_boundary(regions, boundary, pos=None):
'''filter a list of regions using a search boundary'''
ret = []
for r in regions:
if pos is None:
continue
if pos.altitude < 10:
r.score = 0
#print pos
if r.latlon is None or cuav_util.polygon_outside(r.latlon, boundary):
r.score = 0
ret.append(r)
return ret
def process(args):
'''process a set of files'''
global slipmap, mosaic
scan_count = 0
files = []
for a in args:
if os.path.isdir(a):
files.extend(glob.glob(os.path.join(a, '*.pgm')))
else:
files.append(a)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
joes = []
if opts.mavlog:
mpos = mav_position.MavInterpolator(gps_lag=opts.gps_lag)
mpos.set_logfile(opts.mavlog)
else:
mpos = None
if opts.boundary:
boundary = cuav_util.polygon_load(opts.boundary)
else:
boundary = None
if opts.mosaic:
slipmap = mp_slipmap.MPSlipMap(service='GoogleSat', elevation=True, title='Map')
icon = slipmap.icon('planetracker.png')
slipmap.add_object(mp_slipmap.SlipIcon('plane', (0,0), icon, layer=3, rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
C_params = cam_params.CameraParams(lens=opts.lens)
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..',
'cuav', 'data', 'chameleon1_arecont0.json')
C_params.load(path)
mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params)
if boundary is not None:
mosaic.set_boundary(boundary)
if opts.joe:
joes = cuav_util.polygon_load(opts.joe)
if boundary:
for i in range(len(joes)):
joe = joes[i]
if cuav_util.polygon_outside(joe, boundary):
print("Error: joe outside boundary", joe)
return
icon = slipmap.icon('flag.png')
slipmap.add_object(mp_slipmap.SlipIcon('joe%u' % i, (joe[0],joe[1]), icon, layer=4))
joelog = cuav_joe.JoeLog('joe.log')
if opts.view:
viewer = mp_image.MPImage(title='Image')
frame_time = 0
for f in files:
if mpos:
frame_time = cuav_util.parse_frame_time(f)
try:
if opts.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, opts.max_deltat,roll=roll)
slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
except mav_position.MavInterpolatorException as e:
print e
pos = None
else:
pos = None
# check for any events from the map
if opts.mosaic:
slipmap.check_events()
mosaic.check_events()
if f.endswith('.pgm'):
pgm = cuav_util.PGM(f)
im = pgm.array
if pgm.eightbit:
im_8bit = im
else:
im_8bit = numpy.zeros((960,1280,1),dtype='uint8')
if opts.gamma != 0:
scanner.gamma_correct(im, im_8bit, opts.gamma)
else:
scanner.reduce_depth(im, im_8bit)
im_full = numpy.zeros((960,1280,3),dtype='uint8')
scanner.debayer(im_8bit, im_full)
im_640 = numpy.zeros((480,640,3),dtype='uint8')
scanner.downsample(im_full, im_640)
else:
im_orig = cv.LoadImage(f)
(w,h) = cuav_util.image_shape(im_orig)
im_full = im_orig
im_640 = cv.CreateImage((640, 480), 8, 3)
cv.Resize(im_full, im_640, cv.CV_INTER_NN)
im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
im_full = numpy.ascontiguousarray(cv.GetMat(im_full))
count = 0
total_time = 0
img_scan = im_640
t0=time.time()
for i in range(opts.repeat):
if opts.fullres:
regions = scanner.scan(im_full)
regions = cuav_region.RegionsConvert(regions, 1280, 960)
else:
regions = scanner.scan(img_scan)
regions = cuav_region.RegionsConvert(regions)
count += 1
t1=time.time()
if opts.filter:
regions = cuav_region.filter_regions(im_full, regions, frame_time=frame_time, min_score=opts.minscore,
filter_type=opts.filter_type)
scan_count += 1
# optionally link all the images with joe into a separate directory
# for faster re-running of the test with just joe images
if pos and opts.linkjoe and len(regions) > 0:
cuav_util.mkdir_p(opts.linkjoe)
if not cuav_util.polygon_outside((pos.lat, pos.lon), boundary):
joepath = os.path.join(opts.linkjoe, os.path.basename(f))
if os.path.exists(joepath):
os.unlink(joepath)
os.symlink(f, joepath)
if pos and len(regions) > 0:
joelog.add_regions(frame_time, regions, pos, f, width=1280, height=960, altitude=opts.altitude)
if boundary:
regions = cuav_region.filter_boundary(regions, boundary, pos)
region_count += len(regions)
if opts.mosaic and len(regions) > 0:
composite = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)), regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if opts.compress:
jpeg = scanner.jpeg_compress(im_full, opts.quality)
jpeg_filename = f[:-4] + '.jpg'
if os.path.exists(jpeg_filename):
print('jpeg %s already exists' % jpeg_filename)
continue
chameleon.save_file(jpeg_filename, jpeg)
if opts.view:
if opts.fullres:
img_view = im_full
else:
img_view = img_scan
mat = cv.fromarray(img_view)
for r in regions:
(x1,y1,x2,y2) = r.tuple()
(w,h) = cuav_util.image_shape(img_view)
x1 = x1*w//1280
x2 = x2*w//1280
y1 = y1*h//960
y2 = y2*h//960
cv.Rectangle(mat, (max(x1-2,0),max(y1-2,0)), (x2+2,y2+2), (255,0,0), 2)
cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
viewer.set_image(mat)
total_time += (t1-t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' % (
f, count/total_time, region_count, scan_count, num_files))
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)
def process(args):
'''process a set of files'''
global slipmap, mosaic
scan_count = 0
files = scan_image_directory(args.imagedir)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
joes = []
if args.mavlog:
mpos = mav_position.MavInterpolator(gps_lag=args.gps_lag)
mpos.set_logfile(args.mavlog)
else:
mpos = None
if args.boundary:
boundary = cuav_util.polygon_load(args.boundary)
else:
boundary = None
if args.mosaic:
slipmap = mp_slipmap.MPSlipMap(service='GoogleSat', elevation=True, title='Map')
icon = slipmap.icon('redplane.png')
slipmap.add_object(mp_slipmap.SlipIcon('plane', (0,0), icon, layer=3, rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
if args.camera_params:
C_params = cam_params.CameraParams.fromfile(args.camera_params.name)
else:
im_orig = cv2.imread(files[0])
(w,h) = cuav_util.image_shape(im_orig)
C_params = cam_params.CameraParams(lens=args.lens, sensorwidth=args.sensorwidth, xresolution=w, yresolution=h)
mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params)
if boundary is not None:
mosaic.set_boundary(boundary)
if args.joe:
joes = cuav_util.polygon_load(args.joe)
if boundary:
for i in range(len(joes)):
joe = joes[i]
if cuav_util.polygon_outside(joe, boundary):
print("Error: joe outside boundary", joe)
return
icon = slipmap.icon('flag.png')
slipmap.add_object(mp_slipmap.SlipIcon('joe%u' % i, (joe[0],joe[1]), icon, layer=4))
joelog = cuav_joe.JoeLog('joe.log')
if args.view:
viewer = mp_image.MPImage(title='Image')
frame_time = 0
scan_parms = {
'MinRegionArea' : args.min_region_area,
'MaxRegionArea' : args.max_region_area,
'MinRegionSize' : args.min_region_size,
'MaxRegionSize' : args.max_region_size,
'MaxRarityPct' : args.max_rarity_pct,
'RegionMergeSize' : args.region_merge,
'SaveIntermediate' : float(0),
#'SaveIntermediate' : float(args.debug),
'MetersPerPixel' : args.meters_per_pixel100 * args.altitude / 100.0
}
filenum = 0
for f in files:
filenum += 1
if mpos:
frame_time = cuav_util.parse_frame_time(f)
try:
if args.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, args.max_deltat,roll=roll)
slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
except mav_position.MavInterpolatorException as e:
print(e)
pos = None
else:
pos = None
# check for any events from the map
if args.mosaic:
slipmap.check_events()
mosaic.check_events()
im_orig = cv2.imread(f)
(w,h) = cuav_util.image_shape(im_orig)
im_full = im_orig
im_half = cv2.resize(im_orig, (0,0), fx=0.5, fy=0.5)
im_half = numpy.ascontiguousarray(im_half)
im_full = numpy.ascontiguousarray(im_full)
count = 0
total_time = 0
if args.fullres:
img_scan = im_full
else:
img_scan = im_half
t0=time.time()
for i in range(args.repeat):
regions = scanner.scan(img_scan, scan_parms)
regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full))
count += 1
t1=time.time()
if args.filter:
regions = cuav_region.filter_regions(im_full, regions, min_score=args.minscore,
filter_type=args.filter_type)
if len(regions) > 0 and args.debug:
composite = cuav_region.CompositeThumbnail(im_full, regions, thumb_size=args.thumb_size)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
thumb_num = 0
for thumb in thumbs:
print("thumb %u score %f" % (thumb_num, regions[thumb_num].score))
cv2.imwrite('%u_thumb%u.jpg' % (filenum,thumb_num), thumb)
thumb_num += 1
scan_count += 1
# optionally link all the images with joe into a separate directory
# for faster re-running of the test with just joe images
if pos and args.linkjoe and len(regions) > 0:
cuav_util.mkdir_p(args.linkjoe)
if not cuav_util.polygon_outside((pos.lat, pos.lon), boundary):
joepath = os.path.join(args.linkjoe, os.path.basename(f))
if os.path.exists(joepath):
os.unlink(joepath)
os.symlink(f, joepath)
if pos and len(regions) > 0:
joelog.add_regions(frame_time, regions, pos, f, width=w, height=h, altitude=args.altitude, C=C_params)
if boundary:
regions = cuav_region.filter_boundary(regions, boundary, pos)
region_count += len(regions)
if args.mosaic and len(regions) > 0 and pos:
composite = cuav_region.CompositeThumbnail(im_full, regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if args.view:
if args.fullres:
img_view = im_full
else:
img_view = img_scan
#mat = cv.fromarray(img_view)
for r in regions:
r.draw_rectangle(img_view, colour=(255,0,0), linewidth=min(max(w/600,1),3), offset=max(w/200,1))
img_view = cv2.cvtColor(img_view, cv2.COLOR_BGR2RGB)
viewer.set_image(img_view)
total_time += (t1-t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' % (
f, count/total_time, region_count, scan_count, num_files))
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)
