def capture_thread():
'''camera capture thread'''
state = mpstate.camera_state
t1 = time.time()
last_frame_counter = 0
h = None
last_gamma = 0
raw_dir = os.path.join(state.camera_dir, "raw")
cuav_util.mkdir_p(raw_dir)
if mpstate.continue_mode:
mode = 'a'
else:
mode = 'w'
gammalog = open(os.path.join(state.camera_dir, "gamma.log"), mode=mode)
while not mpstate.camera_state.unload.wait(0.02):
if not state.running:
if h is not None:
chameleon.close(h)
h = None
continue
try:
if h is None:
h, base_time, last_frame_time = get_base_time()
# put into continuous mode
chameleon.trigger(h, True)
capture_time = time.time()
if state.settings.depth == 16:
im = numpy.zeros((960, 1280), dtype='uint16')
else:
im = numpy.zeros((960, 1280), dtype='uint8')
if last_gamma != state.settings.gamma:
chameleon.set_gamma(h, state.settings.gamma)
last_gamma = state.settings.gamma
frame_time, frame_counter, shutter = chameleon.capture(h, 1000, im)
if frame_time < last_frame_time:
base_time += 128
if last_frame_counter != 0:
state.frame_loss += frame_counter - (last_frame_counter + 1)
gammalog.write('%f %f %f %s %u %u\n' %
(frame_time, frame_time + base_time, capture_time,
cuav_util.frame_time(frame_time + base_time),
frame_counter, state.settings.gamma))
gammalog.flush()
state.save_queue.put((base_time + frame_time, im))
state.scan_queue.put((base_time + frame_time, im))
state.capture_count += 1
state.fps = 1.0 / (frame_time - last_frame_time)
last_frame_time = frame_time
last_frame_counter = frame_counter
except chameleon.error, msg:
state.error_count += 1
state.error_msg = msg
def process(args):
"""process a set of files"""
count = 0
files = []
for a in args:
if os.path.isdir(a):
files.extend(glob.glob(os.path.join(a, "*.png")))
else:
files.append(a)
files.sort()
num_files = len(files)
print ("num_files=%u" % num_files)
if opts.mavlog:
mpos = mav_position.MavInterpolator(gps_lag=opts.gps_lag)
mpos.set_logfile(opts.mavlog)
else:
print ("You must provide a mavlink log file")
sys.exit(1)
frame_time = 0
if opts.destdir:
cuav_util.mkdir_p(opts.destdir)
for f in files:
frame_time = os.path.getmtime(f)
try:
if opts.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, opts.max_deltat, roll=roll)
except mav_position.MavInterpolatorException as e:
print e
pos = None
im_orig = cv.LoadImage(f)
lat_deg = pos.lat
lng_deg = pos.lon
if opts.inplace:
newfile = f
else:
basefile = f.split(".")[0]
newfile = basefile + ".jpg"
if opts.destdir:
newfile = os.path.join(opts.destdir, os.path.basename(newfile))
cv.SaveImage(newfile, im_orig)
count += 1
print (
"%s %.7f %.7f [%u/%u %.1f%%]"
% (os.path.basename(newfile), lat_deg, lng_deg, count, num_files, (100.0 * count) / num_files)
)
set_gps_location(newfile, lat_deg, lng_deg, pos.altitude, pos.time)
def process(args):
'''process a set of files'''
count = 0
files = []
for a in args:
if os.path.isdir(a):
files.extend(glob.glob(os.path.join(a, '*.png')))
else:
files.append(a)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
if opts.mavlog:
mpos = mav_position.MavInterpolator(gps_lag=opts.gps_lag)
mpos.set_logfile(opts.mavlog)
else:
print("You must provide a mavlink log file")
sys.exit(1)
frame_time = 0
if opts.destdir:
cuav_util.mkdir_p(opts.destdir)
for f in files:
frame_time = os.path.getmtime(f)
try:
if opts.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, opts.max_deltat, roll=roll)
except mav_position.MavInterpolatorException as e:
print e
pos = None
im_orig = cv.LoadImage(f)
lat_deg = pos.lat
lng_deg = pos.lon
if opts.inplace:
newfile = f
else:
basefile = f.split('.')[0]
newfile = basefile + '.jpg'
if opts.destdir:
newfile = os.path.join(opts.destdir, os.path.basename(newfile))
cv.SaveImage(newfile, im_orig)
count += 1
print("%s %.7f %.7f [%u/%u %.1f%%]" %
(os.path.basename(newfile), lat_deg, lng_deg, count, num_files,
(100.0 * count) / num_files))
set_gps_location(newfile, lat_deg, lng_deg, pos.altitude, pos.time)
def process(args):
'''process a set of files'''
count = 0
files = []
if os.path.isdir(args.files):
files.extend(glob.glob(os.path.join(args.files, '*.png')))
else:
files.append(args.files)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
mpos = mav_position.MavInterpolator(gps_lag=args.gps_lag)
mpos.set_logfile(args.mavlog)
frame_time = 0
if args.destdir:
cuav_util.mkdir_p(args.destdir)
for f in files:
frame_time = os.path.getmtime(f)
try:
if args.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, args.max_deltat,roll=roll)
except mav_position.MavInterpolatorException as e:
print e
pos = None
im_orig = cv.LoadImage(f)
lat_deg = pos.lat
lng_deg = pos.lon
if args.inplace:
newfile = f
else:
basefile = f.split('.')[0]
newfile = basefile + '.jpg'
if args.destdir:
newfile = os.path.join(args.destdir, os.path.basename(newfile))
cv.SaveImage(newfile, im_orig)
count += 1
print("%s %.7f %.7f [%u/%u %.1f%%]" % (os.path.basename(newfile),
lat_deg, lng_deg, count, num_files, (100.0*count)/num_files))
set_gps_location(newfile, lat_deg, lng_deg, pos.altitude, pos.time)
def view_threadfunc(self):
'''image viewing thread - this runs on the ground station'''
self.start_gcs_bsend()
self.image_count = 0
self.thumb_count = 0
self.image_total_bytes = 0
#jpeg_total_bytes = 0
self.thumb_total_bytes = 0
self.region_count = 0
self.mosaic = None
self.thumbs_received = set()
# the downloaded thumbs and views are stored in a temp dir
self.view_dir = os.path.join(self.camera_dir, "view")
#self.thumb_dir = os.path.join(self.camera_dir, "thumb")
cuav_util.mkdir_p(self.view_dir)
#cuav_util.mkdir_p(self.thumb_dir)
self.console.set_status('Images', 'Images %u' % self.image_count, row=6)
self.console.set_status('Regions', 'Regions %u' % self.region_count, row=6)
self.console.set_status('JPGSize', 'JPGSize %.0f' % 0.0, row=6)
self.console.set_status('Thumbs', 'Thumbs %u' % self.thumb_count, row=7)
self.console.set_status('ThumbSize', 'ThumbSize %.0f' % 0.0, row=7)
self.console.set_status('ImageSize', 'ImageSize %.0f' % 0.0, row=7)
self.mosaic = cuav_mosaic.Mosaic(slipmap=self.mpstate.map, C=self.c_params,
camera_settings=None,
image_settings=None,
thumb_size=self.camera_settings.mosaic_thumbsize)
while not self.unload_event.wait(0.05):
if self.boundary_polygon is not None:
self.mosaic.set_boundary(self.boundary_polygon)
if self.continue_mode:
self.reload_mosaic(self.mosaic)
# check for keyboard events
self.mosaic.check_events()
self.check_requested_images(self.mosaic)
#check for any new packets
for bsnd in self.bsend:
bsnd.tick(packet_count=1000, max_queue=self.camera_settings.maxqueue)
self.check_commands(bsnd)
self.send_heartbeats()
#ensure the mosiac is closed at end of thread
if self.mosaic.image_mosaic:
self.mosaic.image_mosaic.terminate()
def save_thread():
'''image save thread'''
state = mpstate.camera_state
raw_dir = os.path.join(state.camera_dir, "raw")
cuav_util.mkdir_p(raw_dir)
frame_count = 0
while not state.unload.wait(0.02):
if state.save_queue.empty():
continue
(frame_time,im) = state.save_queue.get()
rawname = "raw%s" % cuav_util.frame_time(frame_time)
frame_count += 1
if state.settings.save_pgm != 0:
if frame_count % state.settings.save_pgm == 0:
chameleon.save_pgm('%s/%s.pgm' % (raw_dir, rawname), im)
def save_thread():
"""image save thread"""
state = mpstate.camera_state
raw_dir = os.path.join(state.camera_dir, "raw")
cuav_util.mkdir_p(raw_dir)
frame_count = 0
while not state.unload.wait(0.02):
if state.save_queue.empty():
continue
(frame_time, im) = state.save_queue.get()
rawname = "raw%s" % cuav_util.frame_time(frame_time)
frame_count += 1
if state.settings.save_pgm != 0:
if frame_count % state.settings.save_pgm == 0:
chameleon.save_pgm("%s/%s.pgm" % (raw_dir, rawname), im)
def capture_thread():
"""camera capture thread"""
state = mpstate.camera_state
t1 = time.time()
last_frame_counter = 0
h = None
last_gamma = 0
raw_dir = os.path.join(state.camera_dir, "raw")
cuav_util.mkdir_p(raw_dir)
if mpstate.continue_mode:
mode = "a"
else:
mode = "w"
gammalog = open(os.path.join(state.camera_dir, "gamma.log"), mode=mode)
while not mpstate.camera_state.unload.wait(0.02):
if not state.running:
if h is not None:
chameleon.close(h)
h = None
continue
try:
if h is None:
h, base_time, last_frame_time = get_base_time()
# put into continuous mode
chameleon.trigger(h, True)
capture_time = time.time()
if state.settings.depth == 16:
im = numpy.zeros((960, 1280), dtype="uint16")
else:
im = numpy.zeros((960, 1280), dtype="uint8")
if last_gamma != state.settings.gamma:
chameleon.set_gamma(h, state.settings.gamma)
last_gamma = state.settings.gamma
frame_time, frame_counter, shutter = chameleon.capture(h, 1000, im)
if frame_time < last_frame_time:
base_time += 128
if last_frame_counter != 0:
state.frame_loss += frame_counter - (last_frame_counter + 1)
gammalog.write(
"%f %f %f %s %u %u\n"
% (
frame_time,
frame_time + base_time,
capture_time,
cuav_util.frame_time(frame_time + base_time),
frame_counter,
state.settings.gamma,
)
)
gammalog.flush()
state.save_queue.put((base_time + frame_time, im))
state.scan_queue.put((base_time + frame_time, im))
state.capture_count += 1
state.fps = 1.0 / (frame_time - last_frame_time)
last_frame_time = frame_time
last_frame_counter = frame_counter
except chameleon.error, msg:
state.error_count += 1
state.error_msg = msg
def __init__(self):
self.running = False
self.unload = threading.Event()
self.unload.clear()
self.capture_thread = None
self.save_thread = None
self.scan_thread1 = None
self.scan_thread2 = None
self.transmit_thread = None
self.view_thread = None
self.settings = mp_settings.MPSettings(
[
("depth", int, 8),
("gcs_address", str, None),
("gcs_view_port", int, 7543),
("bandwidth", int, 40000),
("bandwidth2", int, 2000),
("capture_brightness", int, 150),
("gamma", int, 950),
("brightness", float, 1.0),
("quality", int, 75),
("save_pgm", int, 1),
("transmit", int, 1),
("roll_stabilised", int, 1),
("minscore", int, 75),
("minscore2", int, 500),
("altitude", int, None),
("send1", int, 1),
("send2", int, 1),
("maxqueue1", int, None),
("maxqueue2", int, 30),
("thumbsize", int, 60),
("packet_loss", int, 0),
("gcs_slave", str, None),
("filter_type", str, "simple"),
("fullres", int, 0),
]
)
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.fps = 0
self.scan_fps = 0
self.cam = None
self.save_queue = Queue.Queue()
self.scan_queue = Queue.Queue()
self.transmit_queue = Queue.Queue()
self.viewing = False
self.c_params = CameraParams(lens=4.0)
self.jpeg_size = 0
self.xmit_queue = 0
self.xmit_queue2 = 0
self.efficiency = 1.0
self.last_watch = 0
self.frame_loss = 0
self.boundary = None
self.boundary_polygon = None
self.bandwidth_used = 0
self.rtt_estimate = 0
self.bsocket = None
self.bsend2 = None
self.bsend_slave = None
# setup directory for images
self.camera_dir = os.path.join(os.path.dirname(mpstate.logfile_name), "camera")
cuav_util.mkdir_p(self.camera_dir)
self.mpos = mav_position.MavInterpolator(backlog=5000, gps_lag=0.3)
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir, "joe.log"), append=mpstate.continue_mode)
# load camera params
path = os.path.join(
os.path.dirname(os.path.realpath(__file__)), "..", "..", "..", "cuav", "data", "chameleon1_arecont0.json"
)
self.c_params.load(path)
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 view_thread():
'''image viewing thread - this runs on the ground station'''
from cuav.lib import cuav_mosaic
state = mpstate.camera_state
bsend = block_xmit.BlockSender(state.settings.gcs_view_port, bandwidth=state.settings.bandwidth)
state.bsocket = MavSocket(mpstate.mav_master[0])
state.bsend2 = block_xmit.BlockSender(mss=96, sock=state.bsocket, dest_ip='mavlink', dest_port=0, backlog=5, debug=False)
state.bsend2.set_bandwidth(state.settings.bandwidth2)
view_window = False
image_count = 0
thumb_count = 0
image_total_bytes = 0
jpeg_total_bytes = 0
thumb_total_bytes = 0
region_count = 0
mosaic = None
thumbs_received = set()
view_dir = os.path.join(state.camera_dir, "view")
thumb_dir = os.path.join(state.camera_dir, "thumb")
cuav_util.mkdir_p(view_dir)
cuav_util.mkdir_p(thumb_dir)
img_window = mp_image.MPImage(title='Camera')
mpstate.console.set_status('Images', 'Images %u' % image_count, row=6)
mpstate.console.set_status('Lost', 'Lost %u' % 0, row=6)
mpstate.console.set_status('Regions', 'Regions %u' % region_count, row=6)
mpstate.console.set_status('JPGSize', 'JPGSize %.0f' % 0.0, row=6)
mpstate.console.set_status('XMITQ', 'XMITQ %.0f' % 0.0, row=6)
mpstate.console.set_status('Thumbs', 'Thumbs %u' % thumb_count, row=7)
mpstate.console.set_status('ThumbSize', 'ThumbSize %.0f' % 0.0, row=7)
mpstate.console.set_status('ImageSize', 'ImageSize %.0f' % 0.0, row=7)
ack_time = time.time()
while not state.unload.wait(0.02):
if state.viewing:
tnow = time.time()
if tnow - ack_time > 0.1:
bsend.tick(packet_count=1000, max_queue=state.settings.maxqueue1)
state.bsend2.tick(packet_count=1000, max_queue=state.settings.maxqueue2)
if state.bsend_slave is not None:
state.bsend_slave.tick(packet_count=1000)
ack_time = tnow
if not view_window:
view_window = True
mosaic = cuav_mosaic.Mosaic(slipmap=mpstate.map, C=state.c_params)
if state.boundary_polygon is not None:
mosaic.set_boundary(state.boundary_polygon)
if mpstate.continue_mode:
reload_mosaic(mosaic)
# check for keyboard events
mosaic.check_events()
buf = bsend.recv(0)
if buf is None:
buf = state.bsend2.recv(0)
if buf is None:
continue
try:
obj = cPickle.loads(str(buf))
if obj == None:
continue
except Exception as e:
continue
if state.settings.gcs_slave is not None:
if state.bsend_slave is None:
state.bsend_slave = block_xmit.BlockSender(0, bandwidth=state.settings.bandwidth*10, debug=False)
state.bsend_slave.send(buf,
dest=(state.settings.gcs_slave, state.settings.gcs_view_port),
priority=1)
if isinstance(obj, ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time in thumbs_received:
continue
thumbs_received.add(obj.frame_time)
thumb_total_bytes += len(buf)
# save the thumbnails
thumb_filename = '%s/v%s.jpg' % (thumb_dir, cuav_util.frame_time(obj.frame_time))
chameleon.save_file(thumb_filename, obj.thumb)
composite = cv.LoadImage(thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(obj.regions))
# log the joe positions
filename = '%s/v%s.jpg' % (view_dir, cuav_util.frame_time(obj.frame_time))
pos = obj.pos
log_joe_position(pos, obj.frame_time, obj.regions, filename, thumb_filename)
# update the mosaic and map
mosaic.set_brightness(state.settings.brightness)
mosaic.add_regions(obj.regions, thumbs, filename, pos=pos)
# update console display
region_count += len(obj.regions)
state.frame_loss = obj.frame_loss
state.xmit_queue = obj.xmit_queue
thumb_count += 1
mpstate.console.set_status('Lost', 'Lost %u' % state.frame_loss)
mpstate.console.set_status('Regions', 'Regions %u' % region_count)
mpstate.console.set_status('XMITQ', 'XMITQ %.0f' % state.xmit_queue)
mpstate.console.set_status('Thumbs', 'Thumbs %u' % thumb_count)
mpstate.console.set_status('ThumbSize', 'ThumbSize %.0f' % (thumb_total_bytes/thumb_count))
if isinstance(obj, ImagePacket):
# we have an image from the plane
image_total_bytes += len(buf)
state.xmit_queue = obj.xmit_queue
mpstate.console.set_status('XMITQ', 'XMITQ %.0f' % state.xmit_queue)
# save it to disk
filename = '%s/v%s.jpg' % (view_dir, cuav_util.frame_time(obj.frame_time))
chameleon.save_file(filename, obj.jpeg)
img = cv.LoadImage(filename)
if img.width == 1280:
display_img = cv.CreateImage((640, 480), 8, 3)
cv.Resize(img, display_img)
else:
display_img = img
mosaic.add_image(obj.frame_time, filename, obj.pos)
cv.ConvertScale(display_img, display_img, scale=state.settings.brightness)
img_window.set_image(display_img, bgr=True)
# update console
image_count += 1
jpeg_total_bytes += len(obj.jpeg)
state.jpeg_size = 0.95 * state.jpeg_size + 0.05 * len(obj.jpeg)
mpstate.console.set_status('Images', 'Images %u' % image_count)
mpstate.console.set_status('JPGSize', 'JPG Size %.0f' % (jpeg_total_bytes/image_count))
mpstate.console.set_status('ImageSize', 'ImageSize %.0f' % (image_total_bytes/image_count))
if isinstance(obj, CommandResponse):
print('REMOTE: %s' % obj.response)
else:
if view_window:
view_window = False
h, base_time, last_frame_time = get_base_time(opts.depth, colour=colour, capture_brightness=opts.capbrightness)
print("Found camera: colour=%u GUID=%x" % (colour, chameleon.guid(h)))
if opts.depth == 8:
dtype = 'uint8'
else:
dtype = 'uint16'
im = numpy.zeros((960,1280),dtype=dtype)
cv.NamedWindow('Viewer')
tstart = time.time()
chameleon.trigger(h, True)
chameleon.set_gamma(h, opts.gamma)
cuav_util.mkdir_p(opts.output)
start_thread(save_thread)
i=0
lost = 0
while True:
try:
frame_time, frame_counter, shutter = chameleon.capture(h, 300, im)
except chameleon.error, msg:
lost += 1
continue
if frame_time < last_frame_time:
base_time += 128
save_queue.put((frame_time+base_time, im))
last_frame_time = frame_time
def process(args):
'''process a set of files'''
count = 0
files = []
types = ('*.png', '*.jpeg', '*.jpg')
if os.path.isdir(args.files):
for tp in types:
files.extend(glob.glob(os.path.join(args.files, tp)))
else:
files.append(args.files)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
mpos = mav_position.MavInterpolator(gps_lag=args.gps_lag)
mpos.set_logfile(os.path.join(os.getcwd(), args.mavlog))
frame_time = 0
if args.destdir:
cuav_util.mkdir_p(args.destdir)
for f in files:
#timestamp is in filename
timestamp = (os.path.splitext(os.path.basename(f))[0])
m = re.search("\d", timestamp)
if m:
timestamp = timestamp[m.start():]
frame_time = datetime.datetime.strptime(timestamp, "%Y%m%d%H%M%S%fZ")
frame_time = mav_position.datetime_to_float(frame_time)
try:
if args.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, args.max_deltat, roll=roll)
except mav_position.MavInterpolatorException as e:
print(e)
pos = None
if pos:
lat_deg = pos.lat
lng_deg = pos.lon
if args.inplace:
outfile = f
else:
basefile = os.path.basename(f)
outfile = ""
if args.destdir:
outfile = os.path.join(args.destdir, basefile)
else:
outfile = os.path.join(os.getcwd(), basefile)
shutil.copy2(f, outfile)
count += 1
print(
"%s %.7f %.7f [%u/%u %.1f%%]" %
(os.path.basename(outfile), lat_deg, lng_deg, count, num_files,
(100.0 * count) / num_files))
set_gps_location(outfile, lat_deg, lng_deg, pos.altitude, pos.time)
def process(args):
'''process a set of files'''
global slipmap, mosaic
scan_count = 0
files = scan_image_directory(args.imagedir)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
joes = []
if args.mavlog:
mpos = mav_position.MavInterpolator(gps_lag=args.gps_lag)
mpos.set_logfile(args.mavlog)
else:
mpos = None
if args.boundary:
boundary = cuav_util.polygon_load(args.boundary)
else:
boundary = None
if args.mosaic:
slipmap = mp_slipmap.MPSlipMap(service='GoogleSat', elevation=True, title='Map')
icon = slipmap.icon('redplane.png')
slipmap.add_object(mp_slipmap.SlipIcon('plane', (0,0), icon, layer=3, rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
if args.camera_params:
C_params = cam_params.CameraParams.fromfile(args.camera_params.name)
else:
im_orig = cv2.imread(files[0])
(w,h) = cuav_util.image_shape(im_orig)
C_params = cam_params.CameraParams(lens=args.lens, sensorwidth=args.sensorwidth, xresolution=w, yresolution=h)
mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params)
if boundary is not None:
mosaic.set_boundary(boundary)
if args.joe:
joes = cuav_util.polygon_load(args.joe)
if boundary:
for i in range(len(joes)):
joe = joes[i]
if cuav_util.polygon_outside(joe, boundary):
print("Error: joe outside boundary", joe)
return
icon = slipmap.icon('flag.png')
slipmap.add_object(mp_slipmap.SlipIcon('joe%u' % i, (joe[0],joe[1]), icon, layer=4))
joelog = cuav_joe.JoeLog('joe.log')
if args.view:
viewer = mp_image.MPImage(title='Image')
frame_time = 0
scan_parms = {
'MinRegionArea' : args.min_region_area,
'MaxRegionArea' : args.max_region_area,
'MinRegionSize' : args.min_region_size,
'MaxRegionSize' : args.max_region_size,
'MaxRarityPct' : args.max_rarity_pct,
'RegionMergeSize' : args.region_merge,
'SaveIntermediate' : float(0),
#'SaveIntermediate' : float(args.debug),
'MetersPerPixel' : args.meters_per_pixel100 * args.altitude / 100.0
}
filenum = 0
for f in files:
filenum += 1
if mpos:
frame_time = cuav_util.parse_frame_time(f)
try:
if args.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, args.max_deltat,roll=roll)
slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
except mav_position.MavInterpolatorException as e:
print(e)
pos = None
else:
pos = None
# check for any events from the map
if args.mosaic:
slipmap.check_events()
mosaic.check_events()
im_orig = cv2.imread(f)
(w,h) = cuav_util.image_shape(im_orig)
im_full = im_orig
im_half = cv2.resize(im_orig, (0,0), fx=0.5, fy=0.5)
im_half = numpy.ascontiguousarray(im_half)
im_full = numpy.ascontiguousarray(im_full)
count = 0
total_time = 0
if args.fullres:
img_scan = im_full
else:
img_scan = im_half
t0=time.time()
for i in range(args.repeat):
regions = scanner.scan(img_scan, scan_parms)
regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full))
count += 1
t1=time.time()
if args.filter:
regions = cuav_region.filter_regions(im_full, regions, min_score=args.minscore,
filter_type=args.filter_type)
if len(regions) > 0 and args.debug:
composite = cuav_region.CompositeThumbnail(im_full, regions, thumb_size=args.thumb_size)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
thumb_num = 0
for thumb in thumbs:
print("thumb %u score %f" % (thumb_num, regions[thumb_num].score))
cv2.imwrite('%u_thumb%u.jpg' % (filenum,thumb_num), thumb)
thumb_num += 1
scan_count += 1
# optionally link all the images with joe into a separate directory
# for faster re-running of the test with just joe images
if pos and args.linkjoe and len(regions) > 0:
cuav_util.mkdir_p(args.linkjoe)
if not cuav_util.polygon_outside((pos.lat, pos.lon), boundary):
joepath = os.path.join(args.linkjoe, os.path.basename(f))
if os.path.exists(joepath):
os.unlink(joepath)
os.symlink(f, joepath)
if pos and len(regions) > 0:
joelog.add_regions(frame_time, regions, pos, f, width=w, height=h, altitude=args.altitude, C=C_params)
if boundary:
regions = cuav_region.filter_boundary(regions, boundary, pos)
region_count += len(regions)
if args.mosaic and len(regions) > 0 and pos:
composite = cuav_region.CompositeThumbnail(im_full, regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if args.view:
if args.fullres:
img_view = im_full
else:
img_view = img_scan
#mat = cv.fromarray(img_view)
for r in regions:
r.draw_rectangle(img_view, colour=(255,0,0), linewidth=min(max(w/600,1),3), offset=max(w/200,1))
img_view = cv2.cvtColor(img_view, cv2.COLOR_BGR2RGB)
viewer.set_image(img_view)
total_time += (t1-t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' % (
f, count/total_time, region_count, scan_count, num_files))
def process(args):
'''process a set of files'''
count = 0
files = []
types = ('*.png', '*.jpeg', '*.jpg')
if os.path.isdir(args.files):
for tp in types:
files.extend(glob.glob(os.path.join(args.files, tp)))
else:
files.append(args.files)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
mpos = mav_position.MavInterpolator(gps_lag=args.gps_lag)
mpos.set_logfile(os.path.join(os.getcwd(), args.mavlog))
frame_time = 0
if args.destdir:
cuav_util.mkdir_p(args.destdir)
for f in files:
#timestamp is in filename
timestamp = (os.path.splitext(os.path.basename(f))[0])
m = re.search("\d", timestamp)
if m :
timestamp = timestamp[m.start():]
frame_time = datetime.datetime.strptime(timestamp, "%Y%m%d%H%M%S%fZ")
frame_time = mav_position.datetime_to_float(frame_time)
try:
if args.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, args.max_deltat,roll=roll)
except mav_position.MavInterpolatorException as e:
print(e)
pos = None
if pos:
lat_deg = pos.lat
lng_deg = pos.lon
if args.inplace:
outfile = f
else:
basefile = os.path.basename(f)
outfile = ""
if args.destdir:
outfile = os.path.join(args.destdir, basefile)
else:
outfile = os.path.join(os.getcwd(), basefile)
shutil.copy2(f, outfile)
count += 1
print("%s %.7f %.7f [%u/%u %.1f%%]" % (os.path.basename(outfile),
lat_deg, lng_deg, count, num_files, (100.0*count)/num_files))
set_gps_location(outfile, lat_deg, lng_deg, pos.altitude, pos.time)
def view_thread():
"""image viewing thread - this runs on the ground station"""
from cuav.lib import cuav_mosaic
state = mpstate.camera_state
bsend = block_xmit.BlockSender(state.settings.gcs_view_port, bandwidth=state.settings.bandwidth)
state.bsocket = MavSocket(mpstate.mav_master[0])
state.bsend2 = block_xmit.BlockSender(
mss=96, sock=state.bsocket, dest_ip="mavlink", dest_port=0, backlog=5, debug=False
)
state.bsend2.set_bandwidth(state.settings.bandwidth2)
view_window = False
image_count = 0
thumb_count = 0
image_total_bytes = 0
jpeg_total_bytes = 0
thumb_total_bytes = 0
region_count = 0
mosaic = None
thumbs_received = set()
view_dir = os.path.join(state.camera_dir, "view")
thumb_dir = os.path.join(state.camera_dir, "thumb")
cuav_util.mkdir_p(view_dir)
cuav_util.mkdir_p(thumb_dir)
img_window = mp_image.MPImage(title="Camera")
mpstate.console.set_status("Images", "Images %u" % image_count, row=6)
mpstate.console.set_status("Lost", "Lost %u" % 0, row=6)
mpstate.console.set_status("Regions", "Regions %u" % region_count, row=6)
mpstate.console.set_status("JPGSize", "JPGSize %.0f" % 0.0, row=6)
mpstate.console.set_status("XMITQ", "XMITQ %.0f" % 0.0, row=6)
mpstate.console.set_status("Thumbs", "Thumbs %u" % thumb_count, row=7)
mpstate.console.set_status("ThumbSize", "ThumbSize %.0f" % 0.0, row=7)
mpstate.console.set_status("ImageSize", "ImageSize %.0f" % 0.0, row=7)
ack_time = time.time()
while not state.unload.wait(0.02):
if state.viewing:
tnow = time.time()
if tnow - ack_time > 0.1:
bsend.tick(packet_count=1000, max_queue=state.settings.maxqueue1)
state.bsend2.tick(packet_count=1000, max_queue=state.settings.maxqueue2)
if state.bsend_slave is not None:
state.bsend_slave.tick(packet_count=1000)
ack_time = tnow
if not view_window:
view_window = True
mosaic = cuav_mosaic.Mosaic(slipmap=mpstate.map, C=state.c_params)
if state.boundary_polygon is not None:
mosaic.set_boundary(state.boundary_polygon)
if mpstate.continue_mode:
reload_mosaic(mosaic)
# check for keyboard events
mosaic.check_events()
buf = bsend.recv(0)
if buf is None:
buf = state.bsend2.recv(0)
if buf is None:
continue
try:
obj = cPickle.loads(str(buf))
if obj == None:
continue
except Exception as e:
continue
if state.settings.gcs_slave is not None:
if state.bsend_slave is None:
state.bsend_slave = block_xmit.BlockSender(0, bandwidth=state.settings.bandwidth * 10, debug=False)
state.bsend_slave.send(buf, dest=(state.settings.gcs_slave, state.settings.gcs_view_port), priority=1)
if isinstance(obj, ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time in thumbs_received:
continue
thumbs_received.add(obj.frame_time)
thumb_total_bytes += len(buf)
# save the thumbnails
thumb_filename = "%s/v%s.jpg" % (thumb_dir, cuav_util.frame_time(obj.frame_time))
chameleon.save_file(thumb_filename, obj.thumb)
composite = cv.LoadImage(thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(obj.regions))
# log the joe positions
filename = "%s/v%s.jpg" % (view_dir, cuav_util.frame_time(obj.frame_time))
pos = obj.pos
log_joe_position(pos, obj.frame_time, obj.regions, filename, thumb_filename)
# update the mosaic and map
mosaic.set_brightness(state.settings.brightness)
mosaic.add_regions(obj.regions, thumbs, filename, pos=pos)
# update console display
region_count += len(obj.regions)
state.frame_loss = obj.frame_loss
state.xmit_queue = obj.xmit_queue
thumb_count += 1
mpstate.console.set_status("Lost", "Lost %u" % state.frame_loss)
mpstate.console.set_status("Regions", "Regions %u" % region_count)
mpstate.console.set_status("XMITQ", "XMITQ %.0f" % state.xmit_queue)
mpstate.console.set_status("Thumbs", "Thumbs %u" % thumb_count)
mpstate.console.set_status("ThumbSize", "ThumbSize %.0f" % (thumb_total_bytes / thumb_count))
if isinstance(obj, ImagePacket):
# we have an image from the plane
image_total_bytes += len(buf)
state.xmit_queue = obj.xmit_queue
mpstate.console.set_status("XMITQ", "XMITQ %.0f" % state.xmit_queue)
# save it to disk
filename = "%s/v%s.jpg" % (view_dir, cuav_util.frame_time(obj.frame_time))
chameleon.save_file(filename, obj.jpeg)
img = cv.LoadImage(filename)
if img.width == 1280:
display_img = cv.CreateImage((640, 480), 8, 3)
cv.Resize(img, display_img)
else:
display_img = img
mosaic.add_image(obj.frame_time, filename, obj.pos)
cv.ConvertScale(display_img, display_img, scale=state.settings.brightness)
img_window.set_image(display_img, bgr=True)
# update console
image_count += 1
jpeg_total_bytes += len(obj.jpeg)
state.jpeg_size = 0.95 * state.jpeg_size + 0.05 * len(obj.jpeg)
mpstate.console.set_status("Images", "Images %u" % image_count)
mpstate.console.set_status("JPGSize", "JPG Size %.0f" % (jpeg_total_bytes / image_count))
mpstate.console.set_status("ImageSize", "ImageSize %.0f" % (image_total_bytes / image_count))
if isinstance(obj, CommandResponse):
print ("REMOTE: %s" % obj.response)
else:
if view_window:
view_window = False
def view_threadfunc(self):
'''image viewing thread - this runs on the ground station'''
self.start_gcs_bsend()
self.image_count = 0
self.thumb_count = 0
self.image_total_bytes = 0
#jpeg_total_bytes = 0
self.thumb_total_bytes = 0
self.region_count = 0
self.mosaic = None
self.thumbs_received = set()
# the downloaded thumbs and views are stored in a temp dir
self.view_dir = os.path.join(self.camera_dir, "view")
#self.thumb_dir = os.path.join(self.camera_dir, "thumb")
cuav_util.mkdir_p(self.view_dir)
#cuav_util.mkdir_p(self.thumb_dir)
self.console.set_status('Images',
'Images %u' % self.image_count,
row=6)
self.console.set_status('Regions',
'Regions %u' % self.region_count,
row=6)
self.console.set_status('Thumbs',
'Thumbs %u' % self.thumb_count,
row=7)
self.console.set_status('ThumbSize', 'ThumbSize %.0f' % 0.0, row=7)
self.console.set_status('BX1', 'BX1 --', row=7)
self.console.set_status('BX2', 'BX2 --', row=7)
self.console.set_status('BX3', 'BX3 --', row=7)
# give time for maps to init
time.sleep(3)
map2 = self.module("map2")
map3 = self.module("map3")
if map2:
search_map = map2.map
if map3:
lz_map = map3.map
self.mosaic = cuav_mosaic.Mosaic(
slipmap=self.mpstate.map,
C=self.c_params,
camera_settings=None,
image_settings=None,
thumb_size=self.camera_settings.mosaic_thumbsize,
search_map=search_map,
lz_map=lz_map)
while not self.unload_event.wait(0.05):
if self.boundary_polygon is not None:
self.mosaic.set_boundary(self.boundary_polygon)
if self.continue_mode:
self.reload_mosaic(self.mosaic)
# check for keyboard events
self.mosaic.check_events()
self.check_requested_images(self.mosaic)
#check for any new packets
for bsnd in self.bsend:
bsnd.tick(packet_count=1000,
max_queue=self.camera_settings.maxqueue)
self.check_commands(bsnd)
if self.msend is not None:
self.msend.tick(packet_count=1000,
max_queue=self.camera_settings.maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
#ensure the mosiac is closed at end of thread
if self.mosaic.image_mosaic:
self.mosaic.image_mosaic.terminate()
def __init__(self):
self.running = False
self.unload = threading.Event()
self.unload.clear()
self.capture_thread = None
self.save_thread = None
self.scan_thread1 = None
self.scan_thread2 = None
self.transmit_thread = None
self.view_thread = None
self.settings = mp_settings.MPSettings(
[ ('depth', int, 8),
('gcs_address', str, None),
('gcs_view_port', int, 7543),
('bandwidth', int, 40000),
('bandwidth2', int, 2000),
('capture_brightness', int, 150),
('gamma', int, 950),
('brightness', float, 1.0),
('quality', int, 75),
('save_pgm', int, 1),
('transmit', int, 1),
('roll_stabilised', int, 1),
('minscore', int, 75),
('minscore2', int, 500),
('altitude', int, None),
('send1', int, 1),
('send2', int, 1),
('maxqueue1', int, None),
('maxqueue2', int, 30),
('thumbsize', int, 60),
('packet_loss', int, 0),
('gcs_slave', str, None)
]
)
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.fps = 0
self.scan_fps = 0
self.cam = None
self.save_queue = Queue.Queue()
self.scan_queue = Queue.Queue()
self.transmit_queue = Queue.Queue()
self.viewing = False
self.c_params = CameraParams(lens=4.0)
self.jpeg_size = 0
self.xmit_queue = 0
self.xmit_queue2 = 0
self.efficiency = 1.0
self.last_watch = 0
self.frame_loss = 0
self.boundary = None
self.boundary_polygon = None
self.bandwidth_used = 0
self.rtt_estimate = 0
self.bsocket = None
self.bsend2 = None
self.bsend_slave = None
# setup directory for images
self.camera_dir = os.path.join(os.path.dirname(mpstate.logfile_name),
"camera")
cuav_util.mkdir_p(self.camera_dir)
self.mpos = mav_position.MavInterpolator(backlog=5000, gps_lag=0.3)
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir, 'joe.log'), append=mpstate.continue_mode)
# load camera params
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..', '..',
'cuav', 'data', 'chameleon1_arecont0.json')
self.c_params.load(path)
opts.depth, colour=colour, capture_brightness=opts.capbrightness)
print("Found camera: colour=%u GUID=%x" % (colour, chameleon.guid(h)))
if opts.depth == 8:
dtype = 'uint8'
else:
dtype = 'uint16'
im = numpy.zeros((960, 1280), dtype=dtype)
cv.NamedWindow('Viewer')
tstart = time.time()
chameleon.trigger(h, True)
chameleon.set_gamma(h, opts.gamma)
cuav_util.mkdir_p(opts.output)
start_thread(save_thread)
i = 0
lost = 0
while True:
try:
frame_time, frame_counter, shutter = chameleon.capture(h, 300, im)
except chameleon.error, msg:
lost += 1
continue
if frame_time < last_frame_time:
base_time += 128
save_queue.put((frame_time + base_time, im))
last_frame_time = frame_time