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