def init(_mpstate): '''initialise module''' global mpstate mpstate = _mpstate mpstate.map_state = module_state() service = 'GoogleSat' import platform if platform.system() == 'Windows': # windows has trouble with Google tile URLs service = 'MicrosoftSat' mpstate.map = mp_slipmap.MPSlipMap(service=service, elevation=True, title='Map') mpstate.map_functions = {'draw_lines': draw_lines} # setup a plane icon icon = mpstate.map.icon('planetracker.png') mpstate.map.add_object( mp_slipmap.SlipIcon('plane', (0, 0), icon, layer=3, rotation=0, follow=True, trail=mp_slipmap.SlipTrail())) mpstate.map.add_callback(functools.partial(map_callback)) mpstate.command_map['map'] = (cmd_map, "map control")
def __init__(self, mpstate): super(MapModule, self).__init__(mpstate, "map", "map display", public=True) self.lat = None self.lon = None self.heading = 0 self.wp_change_time = 0 self.fence_change_time = 0 self.rally_change_time = 0 self.have_simstate = False self.have_vehicle = {} self.move_wp = -1 self.moving_wp = None self.moving_fencepoint = None self.moving_rally = None self.mission_list = None self.icon_counter = 0 self.click_position = None self.click_time = 0 self.draw_line = None self.draw_callback = None self.have_global_position = False self.vehicle_type_name = 'plane' self.ElevationMap = mp_elevation.ElevationModel() self.map_settings = mp_settings.MPSettings([ ('showgpspos', int, 0), ('showgps2pos', int, 1), ('showsimpos', int, 0), ('showahrs2pos', int, 0), ('brightness', float, 1), ('rallycircle', bool, False), ('loitercircle', bool, False) ]) service = 'OviHybrid' if 'MAP_SERVICE' in os.environ: service = os.environ['MAP_SERVICE'] import platform mpstate.map = mp_slipmap.MPSlipMap(service=service, elevation=True, title='Map') mpstate.map_functions = {'draw_lines': self.draw_lines} mpstate.map.add_callback(functools.partial(self.map_callback)) self.add_command('map', self.cmd_map, "map control", ['icon', 'set (MAPSETTING)']) self.add_completion_function('(MAPSETTING)', self.map_settings.completion) self.default_popup = MPMenuSubMenu('Popup', items=[]) self.add_menu( MPMenuItem('Fly To', 'Fly To', '# guided ', handler=MPMenuCallTextDialog(title='Altitude (m)', default=100))) self.add_menu(MPMenuItem('Set Home', 'Set Home', '# map sethome ')) self.add_menu( MPMenuItem('Terrain Check', 'Terrain Check', '# terrain check')) self.add_menu( MPMenuItem('Show Position', 'Show Position', 'showPosition'))
def init(_mpstate): '''initialise module''' global mpstate mpstate = _mpstate mpstate.map_state = module_state() service='YahooSat' if 'MAP_SERVICE' in os.environ: service = os.environ['MAP_SERVICE'] import platform mpstate.map = mp_slipmap.MPSlipMap(service=service, elevation=True, title='Map') mpstate.map_functions = { 'draw_lines' : draw_lines } mpstate.map.add_callback(functools.partial(map_callback)) mpstate.command_map['map'] = (cmd_map, "map control") mpstate.completions['map'] = ['icon', 'set (MAPSETTING)'] mpstate.completion_functions['(MAPSETTING)'] = mpstate.map_state.settings.completion
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 process(args): '''process a set of files''' global slipmap, mosaic scan_count = 0 files = [] if os.path.isdir(args.directory): files.extend(file_list(args.directory, ['jpg', 'pgm', 'png'])) else: if args.directory.find('*') != -1: files.extend(glob.glob(args.directory)) else: files.append(args.directory) files.sort() num_files = len(files) print("num_files=%u" % num_files) region_count = 0 slipmap = mp_slipmap.MPSlipMap(service=args.service, 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())) for flag in args.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = slipmap.icon(icon) slipmap.add_object( mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False)) if args.mission: from pymavlink import mavwp wp = mavwp.MAVWPLoader() wp.load(args.mission.name) plist = wp.polygon_list() if len(plist) > 0: for i in range(len(plist)): slipmap.add_object( mp_slipmap.SlipPolygon('Mission-%s-%u' % (args.mission.name, i), plist[i], layer='Mission', linewidth=2, colour=(255, 255, 255))) if args.mavlog: mpos = mav_position.MavInterpolator() mpos.set_logfile(args.mavlog.name) else: mpos = None if args.gammalog is not None: gamma = parse_gamma_log(args.gammalog) else: gamma = None if args.kmzlog: kmzpos = mav_position.KmlPosition(args.kmzlog.name) else: kmzpos = None if args.triggerlog: triggerpos = mav_position.TriggerPosition(args.triggerlog.name) else: triggerpos = None # create a simple lens model using the focal length if args.camera_params: C_params = cam_params.CameraParams.fromfile(args.camera_params.name) else: C_params = cam_params.CameraParams(lens=args.lens, sensorwidth=args.sensorwidth, xresolution=args.xresolution, yresolution=args.yresolution) if args.target: target = args.target.split(',') else: target = [0, 0, 0] camera_settings = MPSettings([ MPSetting('roll_stabilised', bool, args.roll_stabilised, 'Roll Stabilised'), MPSetting('altitude', int, args.altitude, 'Altitude', range=(0, 10000), increment=1), MPSetting( 'minalt', int, 30, 'MinAltitude', range=(0, 10000), increment=1), MPSetting('mpp100', float, 0.0977, 'MPPat100m', range=(0, 10000), increment=0.001), MPSetting('rotate180', bool, args.rotate_180, 'rotate180'), MPSetting('filter_type', str, 'compactness', 'Filter Type', choice=['simple', 'compactness']), MPSetting('target_lattitude', float, float(target[0]), 'target latitude', increment=1.0e-7), MPSetting('target_longitude', float, float(target[1]), 'target longitude', increment=1.0e-7), MPSetting('target_radius', float, float(target[2]), 'target radius', increment=1), MPSetting('quality', int, 75, 'Compression Quality', range=(1, 100), increment=1), MPSetting('thumbsize', int, args.thumbsize, 'Thumbnail Size', range=(10, 200), increment=1), MPSetting('minscore', int, args.minscore, 'Min Score', range=(0, 1000), increment=1, tab='Scoring'), MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1, digits=2, tab='Display'), ], title='Camera Settings') image_settings = MPSettings([ MPSetting('MinRegionArea', float, 0.05, range=(0, 100), increment=0.05, digits=2, tab='Image Processing'), MPSetting('MaxRegionArea', float, 4.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MinRegionSize', float, 0.02, range=(0, 100), increment=0.05, digits=2), MPSetting('MaxRegionSize', float, 3.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MaxRarityPct', float, 0.02, range=(0, 100), increment=0.01, digits=2), MPSetting('RegionMergeSize', float, 1.0, range=(0, 100), increment=0.1, digits=1), MPSetting('BlueEmphasis', bool, args.blue_emphasis), MPSetting('SaveIntermediate', bool, args.debug) ], title='Image Settings') mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params, camera_settings=camera_settings, image_settings=image_settings, start_menu=True, classify=args.categories, thumb_size=args.mosaic_thumbsize) joelog = cuav_joe.JoeLog(None) if args.view: viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True) start_time = time.time() for f in files: if not mosaic.started(): print("Waiting for startup") if args.start: mosaic.has_started = True while not mosaic.started(): mosaic.check_events() time.sleep(0.01) if mpos: # get the position by interpolating telemetry data from the MAVLink log file # this assumes that the filename contains the timestamp if gamma is not None: frame_time = parse_gamma_time(f, gamma) else: frame_time = cuav_util.parse_frame_time(f) frame_time += args.time_offset if camera_settings.roll_stabilised: roll = 0 else: roll = None try: pos = mpos.position(frame_time, roll=roll) except Exception: print("No position available for %s" % frame_time) # skip this frame continue elif kmzpos is not None: pos = kmzpos.position(f) elif triggerpos is not None: pos = triggerpos.position(f) else: # get the position using EXIF data pos = mav_position.exif_position(f) pos.time += args.time_offset # update the plane icon on the map if pos is not None: slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw) if camera_settings.altitude > 0: pos.altitude = camera_settings.altitude # check for any events from the map slipmap.check_events() mosaic.check_events() im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180) if im_orig is None: continue (w, h) = cuav_util.image_shape(im_orig) if False: 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 = im_orig im_full = numpy.ascontiguousarray(cv.GetMat(im_full)) count = 0 total_time = 0 t0 = time.time() img_scan = im_full scan_parms = {} for name in image_settings.list(): scan_parms[name] = image_settings.get(name) scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate']) scan_parms['BlueEmphasis'] = float(scan_parms['BlueEmphasis']) if pos is not None: (sw, sh) = cuav_util.image_shape(img_scan) altitude = pos.altitude if altitude < camera_settings.minalt: altitude = camera_settings.minalt scan_parms[ 'MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0 regions = scanner.scan(img_scan, scan_parms) else: regions = scanner.scan(img_scan) regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full)) count += 1 t1 = time.time() frame_time = pos.time if pos: for r in regions: r.latlon = cuav_util.gps_position_from_image_region( r, pos, w, h, altitude=altitude, C=C_params) if camera_settings.target_radius > 0 and pos is not None: regions = cuav_region.filter_radius( regions, (camera_settings.target_lattitude, camera_settings.target_longitude), camera_settings.target_radius) regions = cuav_region.filter_regions( im_full, regions, frame_time=frame_time, min_score=camera_settings.minscore, filter_type=camera_settings.filter_type) scan_count += 1 if pos and len(regions) > 0: altitude = camera_settings.altitude if altitude <= 0: altitude = None joelog.add_regions(frame_time, regions, pos, f, width=w, height=h, altitude=altitude) mosaic.add_image(pos.time, f, pos) region_count += len(regions) if 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 args.view: img_view = img_scan (wview, hview) = cuav_util.image_shape(img_view) mat = cv.fromarray(img_view) for r in regions: r.draw_rectangle(mat, (255, 0, 0)) cv.CvtColor(mat, mat, cv.CV_BGR2RGB) viewer.set_image(mat) viewer.set_title('Image: ' + os.path.basename(f)) if args.saveview: cv.CvtColor(mat, mat, cv.CV_RGB2BGR) cv.SaveImage('view-' + os.path.basename(f), mat) total_time += (t1 - t0) if t1 != t0: print('%s scan %.1f fps %u regions [%u/%u]' % (os.path.basename(f), count / total_time, region_count, scan_count, num_files)) #raw_input("hit ENTER when ready") print("All images processed (%u seconds)" % (time.time() - start_time)) while True: # check for any events from the map slipmap.check_events() mosaic.check_events() time.sleep(0.2)
def mavflightview(filename): print("Loading %s ..." % filename) mlog = mavutil.mavlink_connection(filename) wp = mavwp.MAVWPLoader() if opts.mission is not None: wp.load(opts.mission) path = [] while True: m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT']) if m is None: break if m.get_type() == 'GLOBAL_POSITION_INT' and mlog.check_condition( opts.condition): if opts.mode is not None and mlog.flightmode.lower( ) != opts.mode.lower(): continue lat = m.lat * 1.0e-7 lng = m.lon * 1.0e-7 if lat != 0 or lng != 0: if mlog.flightmode in colourmap: point = (lat, lng, colourmap[mlog.flightmode]) else: point = (lat, lng) path.append(point) if m.get_type() == 'MISSION_ITEM': wp.set(m, m.seq) if len(path) == 0: print("No points to plot") return bounds = mp_util.polygon_bounds(path) (lat, lon) = (bounds[0] + bounds[2], bounds[1]) (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50) ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2], lon + bounds[3]) while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width - 20 or mp_util.gps_distance( lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20): ground_width += 10 path_obj = mp_slipmap.SlipPolygon('FlightPath', path, layer='FlightPath', linewidth=2, colour=(255, 0, 180)) mission = wp.polygon() if len(mission) > 1: mission_obj = mp_slipmap.SlipPolygon('Mission', wp.polygon(), layer='Mission', linewidth=2, colour=(255, 255, 255)) else: mission_obj = None if opts.imagefile: create_imagefile(opts.imagefile, (lat, lon), ground_width, path_obj, mission_obj) else: map = mp_slipmap.MPSlipMap(title=filename, service=opts.service, elevation=True, width=600, height=600, ground_width=ground_width, lat=lat, lon=lon) map.add_object(path_obj) if mission_obj is not None: map.add_object(mission_obj) for flag in opts.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = map.icon(icon) map.add_object( mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False))
def mavflightview_show(path, wp, fen, options, title=None): if not title: title='MAVFlightView' bounds = mp_util.polygon_bounds(path[0]) (lat, lon) = (bounds[0]+bounds[2], bounds[1]) (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50) ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3]) while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20): ground_width += 10 path_objs = [] for i in range(len(path)): if len(path[i]) != 0: path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath', linewidth=2, colour=(255,0,180))) plist = wp.polygon_list() mission_obj = None if len(plist) > 0: mission_obj = [] for i in range(len(plist)): mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission', linewidth=2, colour=(255,255,255))) else: mission_obj = None fence = fen.polygon() if len(fence) > 1: fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence', linewidth=2, colour=(0,255,0)) else: fence_obj = None if options.imagefile: create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj) else: global multi_map if options.multi and multi_map is not None: map = multi_map else: map = mp_slipmap.MPSlipMap(title=title, service=options.service, elevation=True, width=600, height=600, ground_width=ground_width, lat=lat, lon=lon, debug=options.debug) if options.multi: multi_map = map for path_obj in path_objs: map.add_object(path_obj) if mission_obj is not None: display_waypoints(wp, map) if fence_obj is not None: map.add_object(fence_obj) for flag in options.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = map.icon(icon) map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False)) source = getattr(options, "colour_source", "flightmode") if source != "flightmode": print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
joe = cPickle.load(self.log) except Exception: raise StopIteration return joe if __name__ == "__main__": from MAVProxy.modules.mavproxy_map import mp_slipmap from cuav.lib import cuav_mosaic from optparse import OptionParser parser = OptionParser("cuav_joe.py [options]") parser.add_option("--minscore", type='int', default=1000, help="min score") (opts, args) = parser.parse_args() sm = mp_slipmap.MPSlipMap(lat=-26.6360, lon=151.8436, elevation=True, service='GoogleSat') joelog = JoeIterator(sys.argv[1]) tidx = 0 thumb_filename = None for joe in joelog: thumb = cuav_util.LoadImage(joe.thumb_filename) if thumb_filename != joe.thumb_filename: tidx = 0 else: tidx += 1 (w, h) = cuav_util.image_shape(thumb) count = w // h thumbs = cuav_mosaic.ExtractThumbs(thumb, count) r = getattr(joe, 'r', None)
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(file_list(a, ['jpg', 'pgm', 'png'])) else: if a.find('*') != -1: files.extend(glob.glob(a)) else: files.append(a) files.sort() num_files = len(files) print("num_files=%u" % num_files) region_count = 0 slipmap = mp_slipmap.MPSlipMap(service=opts.service, 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 opts.mission: from pymavlink import mavwp wp = mavwp.MAVWPLoader() wp.load(opts.mission) boundary = wp.polygon() slipmap.add_object( mp_slipmap.SlipPolygon('mission', boundary, layer=1, linewidth=1, colour=(255, 255, 255))) if opts.mavlog: mpos = mav_position.MavInterpolator() mpos.set_logfile(opts.mavlog) else: mpos = None if opts.kmzlog: kmzpos = mav_position.KmlPosition(opts.kmzlog) else: kmzpos = None if opts.triggerlog: triggerpos = mav_position.TriggerPosition(opts.triggerlog) else: triggerpos = None # create a simple lens model using the focal length C_params = cam_params.CameraParams(lens=opts.lens, sensorwidth=opts.sensorwidth) if opts.camera_params: C_params.load(opts.camera_params) camera_settings = MPSettings([ MPSetting('roll_stabilised', bool, True, 'Roll Stabilised'), MPSetting( 'altitude', int, 0, 'Altitude', range=(0, 10000), increment=1), MPSetting('filter_type', str, 'simple', 'Filter Type', choice=['simple', 'compactness']), MPSetting('fullres', bool, False, 'Full Resolution'), MPSetting('quality', int, 75, 'Compression Quality', range=(1, 100), increment=1), MPSetting('thumbsize', int, 60, 'Thumbnail Size', range=(10, 200), increment=1), MPSetting('minscore', int, 75, 'Min Score', range=(0, 1000), increment=1, tab='Scoring'), MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1, digits=2, tab='Display') ], title='Camera Settings') image_settings = MPSettings([ MPSetting('MinRegionArea', float, 0.15, range=(0, 100), increment=0.05, digits=2, tab='Image Processing'), MPSetting('MaxRegionArea', float, 2.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MinRegionSize', float, 0.1, range=(0, 100), increment=0.05, digits=2), MPSetting( 'MaxRegionSize', float, 2, range=(0, 100), increment=0.1, digits=1), MPSetting('MaxRarityPct', float, 0.02, range=(0, 100), increment=0.01, digits=2), MPSetting('RegionMergeSize', float, 3.0, range=(0, 100), increment=0.1, digits=1), MPSetting('SaveIntermediate', bool, False) ], title='Image Settings') mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params, camera_settings=camera_settings, image_settings=image_settings, start_menu=True) joelog = cuav_joe.JoeLog(None) if opts.view: viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True) if camera_settings.filter_type == 'compactness': calculate_compactness = True print("Using compactness filter") else: calculate_compactness = False for f in files: if not mosaic.started(): print("Waiting for startup") while not mosaic.started(): mosaic.check_events() time.sleep(0.01) if mpos: # get the position by interpolating telemetry data from the MAVLink log file # this assumes that the filename contains the timestamp frame_time = cuav_util.parse_frame_time(f) + opts.time_offset if camera_settings.roll_stabilised: roll = 0 else: roll = None try: pos = mpos.position(frame_time, roll=roll) except Exception: print("No position available for %s" % frame_time) # skip this frame continue elif kmzpos is not None: pos = kmzpos.position(f) elif triggerpos is not None: pos = triggerpos.position(f) else: # get the position using EXIF data pos = mav_position.exif_position(f) pos.time += opts.time_offset # update the plane icon on the map if pos is not None: slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw) if camera_settings.altitude > 0: pos.altitude = camera_settings.altitude # check for any events from the map slipmap.check_events() mosaic.check_events() im_orig = cuav_util.LoadImage(f) (w, h) = cuav_util.image_shape(im_orig) if not opts.camera_params: C_params.set_resolution(w, h) 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 t0 = time.time() if camera_settings.fullres: img_scan = im_full else: img_scan = im_640 scan_parms = {} for name in image_settings.list(): scan_parms[name] = image_settings.get(name) scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate']) if pos is not None: (sw, sh) = cuav_util.image_shape(img_scan) mpp = cuav_util.meters_per_pixel(pos, C=C_params) if mpp is not None: scan_parms['MetersPerPixel'] = mpp * (w / float(sw)) regions = scanner.scan(img_scan, scan_parms) else: regions = scanner.scan(img_scan) regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full), calculate_compactness) count += 1 t1 = time.time() frame_time = pos.time regions = cuav_region.filter_regions( im_full, regions, frame_time=frame_time, min_score=camera_settings.minscore, filter_type=camera_settings.filter_type) scan_count += 1 mosaic.add_image(pos.time, f, pos) if pos and len(regions) > 0: altitude = camera_settings.altitude if altitude <= 0: altitude = None joelog.add_regions(frame_time, regions, pos, f, width=w, height=h, altitude=altitude) region_count += len(regions) if 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.view: img_view = img_scan (wview, hview) = cuav_util.image_shape(img_view) mat = cv.fromarray(img_view) for r in regions: r.draw_rectangle(mat, (255, 0, 0)) cv.CvtColor(mat, mat, cv.CV_BGR2RGB) viewer.set_image(mat) viewer.set_title('Image: ' + os.path.basename(f)) total_time += (t1 - t0) if t1 != t0: print('%s scan %.1f fps %u regions [%u/%u]' % (os.path.basename(f), count / total_time, region_count, scan_count, num_files))
def mavflightview_mav(mlog, options=None, title=None): '''create a map for a log file''' if not title: title = 'MAVFlightView' wp = mavwp.MAVWPLoader() if options.mission is not None: wp.load(options.mission) fen = mavwp.MAVFenceLoader() if options.fence is not None: fen.load(options.fence) path = [[]] instances = {} ekf_counter = 0 nkf_counter = 0 types = ['MISSION_ITEM', 'CMD'] if options.types is not None: types.extend(options.types.split(',')) else: types.extend(['GPS', 'GLOBAL_POSITION_INT']) if options.rawgps or options.dualgps: types.extend(['GPS', 'GPS_RAW_INT']) if options.rawgps2 or options.dualgps: types.extend(['GPS2_RAW', 'GPS2']) if options.ekf: types.extend(['EKF1', 'GPS']) if options.nkf: types.extend(['NKF1', 'GPS']) if options.ahr2: types.extend(['AHR2', 'AHRS2', 'GPS']) print("Looking for types %s" % str(types)) last_timestamps = {} while True: try: m = mlog.recv_match(type=types) if m is None: break except Exception: break type = m.get_type() if type == 'MISSION_ITEM': try: while m.seq > wp.count(): print("Adding dummy WP %u" % wp.count()) wp.set(m, wp.count()) wp.set(m, m.seq) except Exception: pass continue if type == 'CMD': m = mavutil.mavlink.MAVLink_mission_item_message( 0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng, m.Alt) try: while m.seq > wp.count(): print("Adding dummy WP %u" % wp.count()) wp.set(m, wp.count()) wp.set(m, m.seq) except Exception: pass continue if not mlog.check_condition(options.condition): continue if options.mode is not None and mlog.flightmode.lower( ) != options.mode.lower(): continue if type in ['GPS', 'GPS2']: status = getattr(m, 'Status', None) if status is None: status = getattr(m, 'FixType', None) if status is None: print("Can't find status on GPS message") print(m) break if status < 2: continue # flash log lat = m.Lat lng = getattr(m, 'Lng', None) if lng is None: lng = getattr(m, 'Lon', None) if lng is None: print("Can't find longitude on GPS message") print(m) break elif type in ['EKF1', 'ANU1']: pos = mavextra.ekf1_pos(m) if pos is None: continue ekf_counter += 1 if ekf_counter % options.ekf_sample != 0: continue (lat, lng) = pos elif type in ['NKF1']: pos = mavextra.ekf1_pos(m) if pos is None: continue nkf_counter += 1 if nkf_counter % options.nkf_sample != 0: continue (lat, lng) = pos elif type in ['ANU5']: (lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7) elif type in ['AHR2', 'POS', 'CHEK']: (lat, lng) = (m.Lat, m.Lng) elif type == 'AHRS2': (lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7) else: lat = m.lat * 1.0e-7 lng = m.lon * 1.0e-7 # automatically add new types to instances if type not in instances: instances[type] = len(instances) while len(instances) >= len(path): path.append([]) instance = instances[type] if abs(lat) > 0.01 or abs(lng) > 0.01: colour = colour_for_point(mlog, (lat, lng), instance, options) point = (lat, lng, colour) if options.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[ type] > 1.0 / options.rate: last_timestamps[type] = m._timestamp path[instance].append(point) if len(path[0]) == 0: print("No points to plot") return bounds = mp_util.polygon_bounds(path[0]) (lat, lon) = (bounds[0] + bounds[2], bounds[1]) (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50) ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2], lon + bounds[3]) while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width - 20 or mp_util.gps_distance( lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20): ground_width += 10 path_objs = [] for i in range(len(path)): if len(path[i]) != 0: path_objs.append( mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, title), path[i], layer='FlightPath', linewidth=2, colour=(255, 0, 180))) plist = wp.polygon_list() mission_obj = None if len(plist) > 0: mission_obj = [] for i in range(len(plist)): mission_obj.append( mp_slipmap.SlipPolygon('Mission-%s-%u' % (title, i), plist[i], layer='Mission', linewidth=2, colour=(255, 255, 255))) else: mission_obj = None fence = fen.polygon() if len(fence) > 1: fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence', linewidth=2, colour=(0, 255, 0)) else: fence_obj = None if options.imagefile: create_imagefile(options, options.imagefile, (lat, lon), ground_width, path_objs, mission_obj, fence_obj) else: global multi_map if options.multi and multi_map is not None: map = multi_map else: map = mp_slipmap.MPSlipMap(title=title, service=options.service, elevation=True, width=600, height=600, ground_width=ground_width, lat=lat, lon=lon, debug=options.debug) if options.multi: multi_map = map for path_obj in path_objs: map.add_object(path_obj) if mission_obj is not None: display_waypoints(wp, map) if fence_obj is not None: map.add_object(fence_obj) for flag in options.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = map.icon(icon) map.add_object( mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False)) source = getattr(options, "colour_source", "flightmode") if source != "flightmode": print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
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))
args.camerares) print "Strip width = " + str(groundWidth) gen.CreateSearchPattern(width=groundWidth, overlap=args.overlap, offset=args.searchAreaOffset, wobble=args.wobble, alt=args.altitude) gen.ExportSearchPattern() #start a map sm = mp_slipmap.MPSlipMap( lat=gen.getMapPolygon( loiterInSearchArea=args.loiterInSearchArea)[0][0], lon=gen.getMapPolygon( loiterInSearchArea=args.loiterInSearchArea)[0][1], elevation=True, service='GoogleSat') sm.add_object( mp_slipmap.SlipPolygon('Search Pattern', gen.getMapPolygon(), layer=1, linewidth=2, colour=(0, 255, 0))) #get the search pattern distance print "Total Distance = " + str(gen.getPolygonLength()) + "m" #and export to MAVProxy gen.exportToMAVProxy(args.loiterInSearchArea, args.basealt, args.outname)
def mavflightview(filename): print("Loading %s ..." % filename) mlog = mavutil.mavlink_connection(filename) wp = mavwp.MAVWPLoader() if opts.mission is not None: wp.load(opts.mission) path = [] path2 = [] types = ['MISSION_ITEM'] if opts.rawgps: types.extend(['GPS', 'GPS_RAW_INT']) if opts.rawgps2: types.extend(['GPS2_RAW', 'GPS2']) if opts.dualgps: types.extend(['GPS2_RAW', 'GPS2', 'GPS_RAW_INT', 'GPS']) if len(types) == 1: types.extend(['GPS', 'GLOBAL_POSITION_INT']) while True: m = mlog.recv_match(type=types) if m is None: break if m.get_type() == 'MISSION_ITEM': wp.set(m, m.seq) continue if not mlog.check_condition(opts.condition): continue if opts.mode is not None and mlog.flightmode.lower( ) != opts.mode.lower(): continue if m.get_type() in ['GPS', 'GPS2']: status = getattr(m, 'Status', None) if status is None: status = getattr(m, 'FixType', None) if status is None: print("Can't find status on GPS message") print(m) break if status < 2: continue # flash log lat = m.Lat lng = getattr(m, 'Lng', None) if lng is None: lng = getattr(m, 'Lon', None) if lng is None: print("Can't find longitude on GPS message") print(m) break else: lat = m.lat * 1.0e-7 lng = m.lon * 1.0e-7 secondary = opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2'] if lat != 0 or lng != 0: if getattr(mlog, 'flightmode', '') in colourmap: colour = colourmap[mlog.flightmode] if secondary: (r, g, b) = colour (r, g, b) = (r + 50, g + 50, b + 50) if r > 255: r = 205 if g > 255: g = 205 if b > 255: b = 205 colour = (r, g, b) point = (lat, lng, colour) else: point = (lat, lng) if secondary: path2.append(point) else: path.append(point) if len(path) == 0: print("No points to plot") return bounds = mp_util.polygon_bounds(path) (lat, lon) = (bounds[0] + bounds[2], bounds[1]) (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50) ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2], lon + bounds[3]) while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width - 20 or mp_util.gps_distance( lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20): ground_width += 10 path_obj = mp_slipmap.SlipPolygon('FlightPath-%s' % filename, path, layer='FlightPath', linewidth=2, colour=(255, 0, 180)) if len(path2) != 0: path2_obj = mp_slipmap.SlipPolygon('FlightPath2-%s' % filename, path2, layer='FlightPath', linewidth=2, colour=(255, 0, 180)) else: path2_obj = None mission = wp.polygon() if len(mission) > 1: mission_obj = mp_slipmap.SlipPolygon('Mission-%s' % filename, wp.polygon(), layer='Mission', linewidth=2, colour=(255, 255, 255)) else: mission_obj = None if opts.imagefile: create_imagefile(opts.imagefile, (lat, lon), ground_width, path_obj, mission_obj, path2_obj=path2_obj) else: global multi_map if opts.multi and multi_map is not None: map = multi_map else: map = mp_slipmap.MPSlipMap(title=filename, service=opts.service, elevation=True, width=600, height=600, ground_width=ground_width, lat=lat, lon=lon, debug=opts.debug) if opts.multi: multi_map = map map.add_object(path_obj) if path2_obj is not None: map.add_object(path2_obj) if mission_obj is not None: map.add_object(mission_obj) for flag in opts.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = map.icon(icon) map.add_object( mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False))
def __init__(self, mpstate): super(MapModule, self).__init__(mpstate, "map", "map display", public=True, multi_instance=True, multi_vehicle=True) cmdname = "map" if self.instance > 1: cmdname += "%u" % self.instance # lat/lon per system ID self.lat_lon = {} self.wp_change_time = 0 self.fence_change_time = 0 self.rally_change_time = 0 self.have_simstate = False self.have_vehicle = {} self.move_wp = -1 self.moving_wp = None self.moving_fencepoint = None self.moving_rally = None self.mission_list = None self.icon_counter = 0 self.circle_counter = 0 self.draw_line = None self.draw_callback = None self.have_global_position = False self.vehicle_type_by_sysid = {} self.vehicle_type_name = 'plane' self.ElevationMap = mp_elevation.ElevationModel() self.last_unload_check_time = time.time() self.unload_check_interval = 0.1 # seconds self.trajectory_layers = set() self.map_settings = mp_settings.MPSettings([ ('showgpspos', int, 1), ('showgps2pos', int, 1), ('showsimpos', int, 0), ('showahrspos', int, 1), ('showahrs2pos', int, 0), ('showahrs3pos', int, 0), ('brightness', float, 1), ('rallycircle', bool, False), ('loitercircle', bool, False), ('showclicktime', int, 2), ('showdirection', bool, False) ]) service = 'MicrosoftHyb' if 'MAP_SERVICE' in os.environ: service = os.environ['MAP_SERVICE'] import platform from MAVProxy.modules.mavproxy_map import mp_slipmap title = "Map" if self.instance > 1: title += str(self.instance) self.map = mp_slipmap.MPSlipMap(service=service, elevation=True, title=title) if self.instance == 1: self.mpstate.map = self.map mpstate.map_functions = {'draw_lines': self.draw_lines} self.map.add_callback(functools.partial(self.map_callback)) self.add_command( cmdname, self.cmd_map, "map control", ['icon', 'set (MAPSETTING)', 'zoom', 'center', 'follow', 'clear']) self.add_completion_function('(MAPSETTING)', self.map_settings.completion) self.default_popup = MPMenuSubMenu('Popup', items=[]) self.add_menu( MPMenuItem('Fly To', 'Fly To', '# guided ', handler=MPMenuCallTextDialog( title='Altitude (m)', default=self.mpstate.settings.guidedalt))) self.add_menu(MPMenuItem('Set Home', 'Set Home', '# map sethomepos ')) self.add_menu( MPMenuItem('Set Home (with height)', 'Set Home', '# map sethome ')) self.add_menu( MPMenuItem('Set Origin', 'Set Origin', '# map setoriginpos ')) self.add_menu( MPMenuItem('Set Origin (with height)', 'Set Origin', '# map setorigin ')) self.add_menu( MPMenuItem('Terrain Check', 'Terrain Check', '# terrain check')) self.add_menu( MPMenuItem('Show Position', 'Show Position', 'showPosition')) self._colour_for_wp_command = { # takeoff commands mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: (255, 0, 0), mavutil.mavlink.MAV_CMD_NAV_TAKEOFF_LOCAL: (255, 0, 0), mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF: (255, 0, 0), # land commands mavutil.mavlink.MAV_CMD_NAV_LAND_LOCAL: (255, 255, 0), mavutil.mavlink.MAV_CMD_NAV_LAND: (255, 255, 0), mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: (255, 255, 0), # waypoint commands mavutil.mavlink.MAV_CMD_NAV_WAYPOINT: (0, 255, 255), mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT: (64, 255, 64), # other commands mavutil.mavlink.MAV_CMD_DO_LAND_START: (255, 127, 0), } self._label_suffix_for_wp_command = { mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: "TOff", mavutil.mavlink.MAV_CMD_DO_LAND_START: "DLS", mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT: "SW", mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: "VL", }
def mavflightview(filename): print("Loading %s ..." % filename) mlog = mavutil.mavlink_connection(filename) wp = mavwp.MAVWPLoader() if opts.mission is not None: wp.load(opts.mission) fen = mavwp.MAVFenceLoader() if opts.fence is not None: fen.load(opts.fence) path = [[]] types = ['MISSION_ITEM', 'CMD'] if opts.rawgps: types.extend(['GPS', 'GPS_RAW_INT']) if opts.rawgps2: types.extend(['GPS2_RAW', 'GPS2']) if opts.dualgps: types.extend(['GPS2_RAW', 'GPS2', 'GPS_RAW_INT', 'GPS']) if opts.ekf: types.extend(['EKF1', 'GPS']) if opts.ahr2: types.extend(['AHR2', 'GPS']) if len(types) == 2: types.extend(['GPS', 'GLOBAL_POSITION_INT']) print("Looking for types %s" % str(types)) while True: try: m = mlog.recv_match(type=types) if m is None: break except Exception: break if m.get_type() == 'MISSION_ITEM': try: while m.seq > wp.count(): print("Adding dummy WP %u" % wp.count()) wp.set(m, wp.count()) wp.set(m, m.seq) except Exception: pass continue if m.get_type() == 'CMD': m = mavutil.mavlink.MAVLink_mission_item_message( 0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng, m.Alt) try: while m.seq > wp.count(): print("Adding dummy WP %u" % wp.count()) wp.set(m, wp.count()) wp.set(m, m.seq) except Exception: pass continue if not mlog.check_condition(opts.condition): continue if opts.mode is not None and mlog.flightmode.lower( ) != opts.mode.lower(): continue if m.get_type() in ['GPS', 'GPS2']: status = getattr(m, 'Status', None) if status is None: status = getattr(m, 'FixType', None) if status is None: print("Can't find status on GPS message") print(m) break if status < 2: continue # flash log lat = m.Lat lng = getattr(m, 'Lng', None) if lng is None: lng = getattr(m, 'Lon', None) if lng is None: print("Can't find longitude on GPS message") print(m) break elif m.get_type() == 'EKF1': pos = mavextra.ekf1_pos(m) if pos is None: continue (lat, lng) = pos elif m.get_type() == 'AHR2': (lat, lng) = (m.Lat, m.Lng) else: lat = m.lat * 1.0e-7 lng = m.lon * 1.0e-7 instance = 0 if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']: instance = 1 if m.get_type() == 'EKF1': if opts.dualgps: instance = 2 else: instance = 1 if m.get_type() == 'AHR2': if opts.dualgps: instance = 2 else: instance = 1 if abs(lat) > 0.01 or abs(lng) > 0.01: if getattr(mlog, 'flightmode', '') in colourmap: colour = colourmap[mlog.flightmode] (r, g, b) = colour (r, g, b) = (r + instance * 50, g + instance * 50, b + instance * 50) if r > 255: r = 205 if g > 255: g = 205 if b > 255: b = 205 colour = (r, g, b) point = (lat, lng, colour) else: point = (lat, lng) while instance >= len(path): path.append([]) path[instance].append(point) if len(path[0]) == 0: print("No points to plot") return bounds = mp_util.polygon_bounds(path[0]) (lat, lon) = (bounds[0] + bounds[2], bounds[1]) (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50) ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2], lon + bounds[3]) while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width - 20 or mp_util.gps_distance( lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20): ground_width += 10 path_objs = [] for i in range(len(path)): if len(path[i]) != 0: path_objs.append( mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, filename), path[i], layer='FlightPath', linewidth=2, colour=(255, 0, 180))) plist = wp.polygon_list() mission_obj = None if len(plist) > 0: mission_obj = [] for i in range(len(plist)): mission_obj.append( mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename, i), plist[i], layer='Mission', linewidth=2, colour=(255, 255, 255))) else: mission_obj = None fence = fen.polygon() if len(fence) > 1: fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename, fen.polygon(), layer='Fence', linewidth=2, colour=(0, 255, 0)) else: fence_obj = None if opts.imagefile: create_imagefile(opts.imagefile, (lat, lon), ground_width, path_objs, mission_obj, fence_obj) else: global multi_map if opts.multi and multi_map is not None: map = multi_map else: map = mp_slipmap.MPSlipMap(title=filename, service=opts.service, elevation=True, width=600, height=600, ground_width=ground_width, lat=lat, lon=lon, debug=opts.debug) if opts.multi: multi_map = map for path_obj in path_objs: map.add_object(path_obj) if mission_obj is not None: display_waypoints(wp, map) if fence_obj is not None: map.add_object(fence_obj) for flag in opts.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = map.icon(icon) map.add_object( mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False))
def mavflightview_show(path, wp, fen, used_flightmodes, mav_type, options, instances, title=None, timelim_pipe=None): if not title: title='MAVFlightView' boundary_path = [] for p in path[0]: boundary_path.append((p[0],p[1])) fence = fen.polygon() if options.fencebounds: for p in fence: boundary_path.append((p[0],p[1])) bounds = mp_util.polygon_bounds(boundary_path) (lat, lon) = (bounds[0]+bounds[2], bounds[1]) (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50) ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3]) while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20): ground_width += 10 path_objs = [] for i in range(len(path)): if len(path[i]) != 0: path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath', linewidth=2, colour=(255,0,180))) plist = wp.polygon_list() mission_obj = None if len(plist) > 0: mission_obj = [] for i in range(len(plist)): mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission', linewidth=2, colour=(255,255,255))) else: mission_obj = None if len(fence) > 1: fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence', linewidth=2, colour=(0,255,0)) else: fence_obj = None kml = getattr(options,'kml',None) if kml is not None: kml_objects = load_kml(kml) else: kml_objects = None if options.imagefile: create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj, kml_objects, used_flightmodes=used_flightmodes, mav_type=mav_type) else: global multi_map if options.multi and multi_map is not None: map = multi_map else: map = mp_slipmap.MPSlipMap(title=title, service=options.service, elevation=True, width=600, height=600, ground_width=ground_width, lat=lat, lon=lon, debug=options.debug, show_flightmode_legend=options.show_flightmode_legend, timelim_pipe=timelim_pipe) if options.multi: multi_map = map for path_obj in path_objs: map.add_object(path_obj) if mission_obj is not None: display_waypoints(wp, map) if fence_obj is not None: map.add_object(fence_obj) if kml_objects is not None: for obj in kml_objects: map.add_object(obj) for flag in options.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = map.icon(icon) map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False)) if options.colour_source == "flightmode": tuples = [ (mode, colour_for_flightmode(mav_type, mode)) for mode in used_flightmodes.keys() ] map.add_object(mp_slipmap.SlipFlightModeLegend("legend", tuples)) elif options.colour_source == "type": tuples = [ (t, map_colours[instances[t]]) for t in instances.keys() ] map.add_object(mp_slipmap.SlipFlightModeLegend("legend", tuples)) else: print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
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(file_list(a, ['jpg', 'pgm', 'png'])) else: if a.find('*') != -1: files.extend(glob.glob(a)) else: files.append(a) files.sort() num_files = len(files) print("num_files=%u" % num_files) region_count = 0 slipmap = mp_slipmap.MPSlipMap(service=opts.service, 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 opts.mission: from pymavlink import mavwp wp = mavwp.MAVWPLoader() wp.load(opts.mission) boundary = wp.polygon() slipmap.add_object( mp_slipmap.SlipPolygon('mission', boundary, layer=1, linewidth=1, colour=(255, 255, 255))) if opts.mavlog: mpos = mav_position.MavInterpolator() mpos.set_logfile(opts.mavlog) else: mpos = None if opts.kmzlog: kmzpos = mav_position.KmlPosition(opts.kmzlog) else: kmzpos = None if opts.triggerlog: triggerpos = mav_position.TriggerPosition(opts.triggerlog) else: triggerpos = None # create a simple lens model using the focal length C_params = cam_params.CameraParams(lens=opts.lens, sensorwidth=opts.sensorwidth) if opts.camera_params: C_params.load(opts.camera_params) mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params) joelog = cuav_joe.JoeLog(None) if opts.view: viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True) scan_parms = { 'MinRegionArea': opts.min_region_area, 'MaxRegionArea': opts.max_region_area, 'MinRegionSize': opts.min_region_size, 'MaxRegionSize': opts.max_region_size, 'MaxRarityPct': opts.max_rarity_pct, 'RegionMergeSize': opts.region_merge } for f in files: if mpos: # get the position by interpolating telemetry data from the MAVLink log file # this assumes that the filename contains the timestamp frame_time = cuav_util.parse_frame_time(f) + opts.time_offset if opts.roll_stabilised: roll = 0 else: roll = None try: pos = mpos.position(frame_time, roll=roll) except Exception: print("No position available for %s" % frame_time) # skip this frame continue elif kmzpos is not None: pos = kmzpos.position(f) elif triggerpos is not None: pos = triggerpos.position(f) else: # get the position using EXIF data pos = mav_position.exif_position(f) pos.time += opts.time_offset # update the plane icon on the map if pos is not None: slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw) # check for any events from the map slipmap.check_events() mosaic.check_events() im_orig = cuav_util.LoadImage(f) (w, h) = cuav_util.image_shape(im_orig) if not opts.camera_params: C_params.set_resolution(w, h) 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 t0 = time.time() if opts.fullres: img_scan = im_full else: img_scan = im_640 if pos is not None: (sw, sh) = cuav_util.image_shape(img_scan) mpp = cuav_util.meters_per_pixel(pos, C=C_params) if mpp is not None: scan_parms['MetersPerPixel'] = mpp * (w / float(sw)) regions = scanner.scan(img_scan, scan_parms) else: regions = scanner.scan(img_scan) regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full)) count += 1 t1 = time.time() frame_time = pos.time regions = cuav_region.filter_regions(im_full, regions, frame_time=frame_time, min_score=opts.minscore, filter_type=opts.filter_type) scan_count += 1 mosaic.add_image(pos.time, f, pos) if pos and len(regions) > 0: joelog.add_regions(frame_time, regions, pos, f, width=w, height=h, altitude=opts.altitude) region_count += len(regions) if 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.view: img_view = img_scan (wview, hview) = cuav_util.image_shape(img_view) mat = cv.fromarray(img_view) for r in regions: r.draw_rectangle(mat, (255, 0, 0)) cv.CvtColor(mat, mat, cv.CV_BGR2RGB) viewer.set_image(mat) viewer.set_title('Image: ' + os.path.basename(f)) total_time += (t1 - t0) if t1 != t0: print('%s scan %.1f fps %u regions [%u/%u]' % (os.path.basename(f), count / total_time, region_count, scan_count, num_files))