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)