def save_thread():
'''thread for saving files'''
try:
os.mkdir('tmp')
except Exception:
pass
last_filename = None
while True:
frame_time, im, is_jpeg = state.save_queue.get()
if is_jpeg:
filename = 'tmp/i%s.jpg' % cuav_util.frame_time(frame_time)
chameleon.save_file(filename, im)
else:
filename = 'tmp/i%s.pgm' % cuav_util.frame_time(frame_time)
chameleon.save_pgm(filename, im)
if opts.make_fake is not None:
try:
os.unlink(opts.make_fake)
except OSError:
pass
os.symlink(filename, opts.make_fake)
if last_filename is not None:
try:
os.unlink(last_filename)
except OSError:
pass
last_filename = filename
def save_thread():
'''thread for saving files'''
try:
os.mkdir('tmp')
except Exception:
pass
while True:
frame_time, im, is_jpeg = state.save_queue.get()
if is_jpeg:
filename = 'tmp/i%s.jpg' % cuav_util.frame_time(frame_time)
chameleon.save_file(filename, im)
else:
filename = 'tmp/i%s.pgm' % cuav_util.frame_time(frame_time)
chameleon.save_pgm(filename, im)
def save_thread():
"""thread for saving files"""
try:
os.mkdir("tmp")
except Exception:
pass
while True:
frame_time, im, is_jpeg = state.save_queue.get()
if is_jpeg:
filename = "tmp/i%s.jpg" % cuav_util.frame_time(frame_time)
chameleon.save_file(filename, im)
else:
filename = "tmp/i%s.pgm" % cuav_util.frame_time(frame_time)
chameleon.save_pgm(filename, im)
def save_thread():
'''thread for saving files'''
try:
os.mkdir('tmp')
except Exception:
pass
while True:
frame_time, im, is_jpeg = state.save_queue.get()
if is_jpeg:
filename = 'tmp/i%s.jpg' % cuav_util.frame_time(frame_time)
chameleon.save_file(filename, im)
else:
filename = 'tmp/i%s.pgm' % cuav_util.frame_time(frame_time)
chameleon.save_pgm(filename, im)
def save_thread():
global save_queue
while True:
(frame_time, im) = save_queue.get()
filename = '%s/%s.pgm' % (opts.output,
cuav_util.frame_time(frame_time))
chameleon.save_pgm(filename, im)
def run_capture():
"""the main capture loop"""
print("Getting base frame time")
h, base_time, last_frame_time = get_base_time()
if not opts.trigger:
print("Starting continuous trigger")
chameleon.trigger(h, True)
frame_loss = 0
num_captured = 0
last_frame_counter = 0
print("Starting main capture loop")
while True:
im = numpy.zeros((960, 1280), dtype="uint8" if opts.depth == 8 else "uint16")
try:
if opts.trigger:
chameleon.trigger(h, False)
frame_time, frame_counter, shutter = chameleon.capture(h, 1000, im)
except chameleon.error:
print("failed to capture")
continue
if frame_time < last_frame_time:
base_time += 128
if last_frame_counter != 0:
frame_loss += frame_counter - (last_frame_counter + 1)
if opts.compress or opts.scan:
state.bayer_queue.put((base_time + frame_time, im))
if opts.save and not opts.compress:
state.save_queue.put((base_time + frame_time, im, False))
print(
"Captured %s shutter=%f tdelta=%f ft=%f loss=%u qsave=%u qbayer=%u qcompress=%u scan=%u"
% (
cuav_util.frame_time(base_time + frame_time),
shutter,
frame_time - last_frame_time,
frame_time,
frame_loss,
state.save_queue.qsize(),
state.bayer_queue.qsize(),
state.compress_queue.qsize(),
state.scan_queue.qsize(),
)
)
last_frame_time = frame_time
last_frame_counter = frame_counter
num_captured += 1
if num_captured == opts.num_frames:
break
print("Closing camera")
time.sleep(2)
chameleon.close(h)
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 run_capture():
'''the main capture loop'''
print("Getting base frame time")
h, base_time, last_frame_time = get_base_time()
if not opts.trigger:
print('Starting continuous trigger')
chameleon.trigger(h, True)
frame_loss = 0
num_captured = 0
last_frame_counter = 0
print('Starting main capture loop')
while True:
im = numpy.zeros((960,1280),dtype='uint8' if opts.depth==8 else 'uint16')
try:
if opts.trigger:
chameleon.trigger(h, False)
frame_time, frame_counter, shutter = chameleon.capture(h, 1000, im)
except chameleon.error:
print('failed to capture')
continue
if frame_time < last_frame_time:
base_time += 128
if last_frame_counter != 0:
frame_loss += frame_counter - (last_frame_counter+1)
if opts.compress or opts.scan:
state.bayer_queue.put((base_time+frame_time, im))
if opts.save and not opts.compress:
state.save_queue.put((base_time+frame_time, im, False))
print("Captured %s shutter=%f tdelta=%f ft=%f loss=%u qsave=%u qbayer=%u qcompress=%u scan=%u" % (
cuav_util.frame_time(base_time+frame_time),
shutter,
frame_time - last_frame_time,
frame_time,
frame_loss,
state.save_queue.qsize(),
state.bayer_queue.qsize(),
state.compress_queue.qsize(),
state.scan_queue.qsize()))
last_frame_time = frame_time
last_frame_counter = frame_counter
num_captured += 1
if num_captured == opts.num_frames:
break
print('Closing camera')
time.sleep(2)
chameleon.close(h)
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 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 check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = cPickle.loads(str(buf))
if obj is None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
bidx = None
for i in range(len(self.bsend)):
if bsend == self.bsend[i]:
bidx = i + 1
if bidx is None and bsend == self.msend:
bidx = 0
if bidx is not None:
now = time.time()
self.mcount[bidx] += 1
self.last_msg_stamp[bidx] = now
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.HeartBeat):
self.image_count = obj.icount
self.console.set_status('Images',
'Images %u' % self.image_count,
row=6)
if isinstance(obj, cuav_command.ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time not in self.thumbs_received:
self.thumbs_received.add(obj.frame_time)
self.thumb_total_bytes += len(buf)
# add the thumbnails to the mosaic
thumbdec = cv2.imdecode(obj.thumb, 1)
if thumbdec is None:
pass
thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
# log the joe positions
# note the filename is where the fullsize image would be downloaded
# to, if requested
filename = os.path.join(
self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
self.log_joe_position(obj.pos, obj.frame_time, obj.regions,
filename, None)
# update the mosaic and map
self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos)
# update console display
self.region_count += len(obj.regions)
self.thumb_count += 1
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' % (self.thumb_total_bytes / self.thumb_count),
row=7)
if isinstance(obj, cuav_command.ImagePacket):
# we have an image from the plane
self.image_total_bytes += len(buf)
#save to file
imagedec = cv2.imdecode(obj.jpeg, 1)
ff = os.path.join(self.view_dir,
cuav_util.frame_time(obj.frame_time)) + ".jpg"
write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99]
cv2.imwrite(ff, imagedec, write_param)
self.mosaic.tag_image(obj.frame_time)
if obj.pos is not None:
self.mosaic.add_image(obj.frame_time, ff, obj.pos)
# update console
self.image_count += 1
color = 'black'
self.console.set_status('Images',
'Images %u' % self.image_count,
row=6,
fg=color)
if isinstance(obj, cuav_command.CommandPacket):
# ground doesn't accept command packets from air
pass
if isinstance(obj, cuav_command.CommandResponse):
# reply to CommandPacket
print('REMOTE: %s' % obj.response)
if isinstance(obj, cuav_command.CameraMessage):
print('CUAV AIR REMOTE: %s' % obj.msg)
if isinstance(obj, cuav_landingregion.LandingZoneDisplay):
lzresult = obj
# display on all maps
for m in self.module_matching('map?'):
m.map.add_object(
mp_slipmap.SlipCircle('LZ',
'LZ',
lzresult.latlon,
lzresult.maxrange,
linewidth=3,
color=(0, 255, 0)))
m.map.add_object(
mp_slipmap.SlipCircle('LZMid',
'LZMid',
lzresult.latlon,
2.0,
linewidth=3,
color=(0, 255, 0)))
lztext = 'LZ: %.6f %.6f E:%.1f AS:%.0f N:%u' % (
lzresult.latlon[0], lzresult.latlon[1], lzresult.maxrange,
lzresult.avgscore, lzresult.numregions)
m.map.add_object(mp_slipmap.SlipInfoText(
'landingzone', lztext))
# assume map2 is the search map
map2 = self.module('map2')
if map2 is not None:
#map2.map.set_zoom(250)
map2.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map2.map.set_follow(0)
# assume map3 is the lz map
map3 = self.module('map3')
if map3 is not None:
map3.map.set_zoom(max(50, 2 * lzresult.maxrange))
map3.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map3.map.set_follow(0)
try:
cuav = self.module('CUAV')
cuav.show_JoeZone()
except Exception as ex:
print("err: ", ex)
return
if isinstance(obj, cuav_command.FilePacket):
print("got file %s" % obj.filename)
try:
open(obj.filename, "w").write(obj.contents)
except Exception as ex:
print("file save failed", ex)
return
if obj.filename == "newwp.txt":
try:
wpmod = self.module('wp')
wpmod.wploader.load(obj.filename)
except Exception as ex:
print("wp load failed", ex)
if isinstance(obj, cuav_command.PreviewPacket):
# we have a preview image from the plane
img = cv2.imdecode(obj.jpeg, 1)
if self.preview_window is None or not self.preview_window.is_alive(
):
self.preview_window = mp_image.MPImage(title='Preview',
width=410,
height=308,
auto_size=True)
if self.preview_window is not None:
self.preview_window.set_image(img, bgr=True)
self.preview_window.poll()
def rawname(self):
"""return raw filename"""
return "raw%s.pgm" % cuav_util.frame_time(self.frame_time)
def save_thread():
global save_queue
while True:
(frame_time, im) = save_queue.get()
filename = '%s/%s.pgm' % (opts.output, cuav_util.frame_time(frame_time))
chameleon.save_pgm(filename, im)
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 playback(mavcon, images, targets, target_lat, target_lon, C_params):
'''create synthetic images matching target position'''
print("Connecting to %s" % mavcon)
mlog = mavutil.mavlink_connection(mavcon)
# get first message
msg = mlog.recv_match(type='ATTITUDE', blocking=True)
mavtime = msg.time_boot_ms * 0.001
last_print = time.time()
last_image = mavtime
mpos = mav_position.MavInterpolator()
Xres = 1640
Yres = 1232
tmpimages = "tmpimages"
print("Using %s directory" % tmpimages)
try:
os.mkdir(tmpimages)
except Exception:
pass
while True:
msg = mlog.recv_match(blocking=False)
if not msg:
time.sleep(0.1)
continue
mpos.add_msg(msg)
mtype = msg.get_type()
if mtype == "ATTITUDE":
mavtime = msg.time_boot_ms * 0.001
if mavtime - last_image > 0.8:
last_image = mavtime
if not 'GLOBAL_POSITION_INT' in mlog.messages:
continue
frame_time = time.time() - 2.0
try:
pos = mpos.position(frame_time)
except Exception:
continue
imgfile = random.choice(images)
img = cv2.imread(imgfile)
filename = os.path.join(
tmpimages,
cuav_util.frame_time(frame_time) + ".jpg")
# see if the target should be in the image
xy = find_xy_in_image(target_lat, target_lon, pos, Xres, Yres,
C_params)
if xy is None:
# when we are not modifying the image use a hardlink to save space
os.link(imgfile, filename)
else:
# overlay a random target on the image
(x, y) = xy
tgtfile = random.choice(targets)
# targets come from an 80m reference height
tgt_scale = 80.0 / max(pos.altitude, 1)
tgt = cv2.imread(tgtfile)
tgt = cv2.resize(tgt, (0, 0), fx=tgt_scale, fy=tgt_scale)
try:
img[y:y + tgt.shape[0], x:x + tgt.shape[1]] = tgt
cv2.imwrite(filename, img)
except Exception:
os.link(imgfile, filename)
# create symlink
try:
os.unlink("capture.jpg")
except Exception:
pass
os.symlink(filename, "capture.jpg")
print(filename, xy)
def playback(mavcon, images, targets, target_lat, target_lon, C_params):
'''create synthetic images matching target position'''
print("Connecting to %s" % mavcon)
mlog = mavutil.mavlink_connection(mavcon)
# get first message
msg = mlog.recv_match(type='ATTITUDE', blocking=True)
mavtime = msg.time_boot_ms * 0.001
last_print = time.time()
last_image = mavtime
mpos = mav_position.MavInterpolator()
Xres = 1640
Yres = 1232
tmpimages = "tmpimages"
print("Using %s directory" % tmpimages)
try:
os.mkdir(tmpimages)
except Exception:
pass
while True:
msg = mlog.recv_match(blocking=False)
if not msg:
time.sleep(0.1)
continue
mpos.add_msg(msg)
mtype = msg.get_type()
if mtype == "ATTITUDE":
mavtime = msg.time_boot_ms*0.001
if mavtime - last_image > 0.8:
last_image = mavtime
if not 'GLOBAL_POSITION_INT' in mlog.messages:
continue
frame_time = time.time() - 2.0
try:
pos = mpos.position(frame_time)
except Exception:
continue
imgfile = random.choice(images)
img = cv2.imread(imgfile)
filename = os.path.join(tmpimages, cuav_util.frame_time(frame_time) + ".jpg")
# see if the target should be in the image
xy = find_xy_in_image(target_lat, target_lon, pos, Xres, Yres, C_params)
if xy is None:
# when we are not modifying the image use a hardlink to save space
os.link(imgfile, filename)
else:
# overlay a random target on the image
(x,y) = xy
tgtfile = random.choice(targets)
# targets come from an 80m reference height
tgt_scale = 80.0 / max(pos.altitude,1)
tgt = cv2.imread(tgtfile)
tgt = cv2.resize(tgt, (0,0), fx=tgt_scale, fy=tgt_scale)
try:
img[y:y+tgt.shape[0], x:x+tgt.shape[1]] = tgt
cv2.imwrite(filename, img)
except Exception:
os.link(imgfile, filename)
# create symlink
try:
os.unlink("capture.jpg")
except Exception:
pass
os.symlink(filename, "capture.jpg")
print(filename, xy)
def run_capture():
'''the main capture loop'''
print("Getting base frame time")
h, base_time, last_frame_time = get_base_time()
if not opts.trigger:
print('Starting continuous trigger')
chameleon.trigger(h, True)
frame_loss = 0
num_captured = 0
last_frame_counter = 0
error_count = 0
print('Starting main capture loop')
while True:
im = numpy.zeros((960,1280),dtype='uint8' if opts.depth==8 else 'uint16')
try:
if opts.trigger:
chameleon.trigger(h, False)
frame_time, frame_counter, shutter = chameleon.capture(h, 1000, im)
except chameleon.error:
print('failed to capture')
error_count += 1
if error_count > 3:
error_count = 0
print('re-opening camera')
chameleon.close(h)
h = chameleon.open(not opts.mono, opts.depth, opts.brightness)
if opts.framerate != 0:
chameleon.set_framerate(h, opts.framerate)
if not opts.trigger:
print('Starting continuous trigger')
chameleon.trigger(h, True)
continue
if frame_time < last_frame_time:
base_time += 128
if last_frame_counter != 0:
frame_loss += frame_counter - (last_frame_counter+1)
if opts.compress or opts.scan:
state.bayer_queue.put((base_time+frame_time, im))
if opts.save and not opts.compress:
if opts.reduction == 0 or num_captured % opts.reduction == 0:
state.save_queue.put((base_time+frame_time, im, False))
print("Captured %s shutter=%f tdelta=%f(%.2f) ft=%f loss=%u qsave=%u qbayer=%u qcompress=%u scan=%u" % (
cuav_util.frame_time(base_time+frame_time),
shutter,
frame_time - last_frame_time,
1.0/(frame_time - last_frame_time),
frame_time,
frame_loss,
state.save_queue.qsize(),
state.bayer_queue.qsize(),
state.compress_queue.qsize(),
state.scan_queue.qsize()))
error_count = 0
last_frame_time = frame_time
last_frame_counter = frame_counter
num_captured += 1
if num_captured == opts.num_frames:
break
print('Closing camera')
time.sleep(2)
chameleon.close(h)
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = pickle.loads(buf)
if obj is None:
return
except Exception as e:
print(e)
return
if isinstance(obj, cuav_command.StampedCommand):
bidx = None
for i in range(len(self.bsend)):
if bsend == self.bsend[i]:
bidx = i+1
if bidx is None and bsend == self.msend:
bidx = 0
if bidx is not None:
now = time.time()
self.mcount[bidx] += 1
self.last_msg_stamp[bidx] = now
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.HeartBeat):
self.image_count = obj.icount
self.console.set_status('Images', 'Images %u' % self.image_count, row=6)
if isinstance(obj, cuav_command.ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time not in self.thumbs_received:
self.thumbs_received.add(obj.frame_time)
self.thumb_total_bytes += len(buf)
# add the thumbnails to the mosaic
thumbdec = cv2.imdecode(obj.thumb, 1)
if thumbdec is None:
pass
thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
# log the joe positions
# note the filename is where the fullsize image would be downloaded
# to, if requested
filename = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
self.log_joe_position(obj.pos, obj.frame_time, obj.regions, filename, None)
# update the mosaic and map
self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos)
# update console display
self.region_count += len(obj.regions)
self.thumb_count += 1
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' %
(self.thumb_total_bytes/self.thumb_count), row=7)
if isinstance(obj, cuav_command.ImagePacket):
# we have an image from the plane
self.image_total_bytes += len(buf)
#save to file
imagedec = cv2.imdecode(obj.jpeg, 1)
ff = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99]
cv2.imwrite(ff, imagedec, write_param)
self.mosaic.tag_image(obj.frame_time)
if obj.pos is not None:
self.mosaic.add_image(obj.frame_time, ff, obj.pos)
# update console
self.image_count += 1
color = 'black'
self.console.set_status('Images', 'Images %u' % self.image_count, row=6, fg=color)
if isinstance(obj, cuav_command.CommandPacket):
# ground doesn't accept command packets from air
pass
if isinstance(obj, cuav_command.CommandResponse):
# reply to CommandPacket
print('REMOTE: %s' % obj.response)
if isinstance(obj, cuav_command.CameraMessage):
print('CUAV AIR REMOTE: %s' % obj.msg)
if isinstance(obj, cuav_landingregion.LandingZoneDisplay):
lzresult = obj
# display on all maps
for m in self.module_matching('map?'):
m.map.add_object(mp_slipmap.SlipCircle('LZ', 'LZ', lzresult.latlon, lzresult.maxrange,
linewidth=3, color=(0,255,0)))
m.map.add_object(mp_slipmap.SlipCircle('LZMid', 'LZMid', lzresult.latlon, 2.0,
linewidth=3, color=(0,255,0)))
lztext = 'LZ: %.6f %.6f E:%.1f AS:%.0f N:%u' % (
lzresult.latlon[0], lzresult.latlon[1], lzresult.maxrange, lzresult.avgscore, lzresult.numregions)
m.map.add_object(mp_slipmap.SlipInfoText('landingzone', lztext))
# assume map2 is the search map
map2 = self.module('map2')
if map2 is not None:
#map2.map.set_zoom(250)
map2.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map2.map.set_follow(0)
# assume map3 is the lz map
map3 = self.module('map3')
if map3 is not None:
map3.map.set_zoom(max(50, 2*lzresult.maxrange))
map3.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map3.map.set_follow(0)
try:
cuav = self.module('CUAV')
cuav.show_JoeZone()
except Exception as ex:
print("err: ", ex)
return
if isinstance(obj, cuav_command.FilePacket):
print("got file %s" % obj.filename)
try:
open(obj.filename,"w").write(obj.contents)
except Exception as ex:
print("file save failed", ex)
return
if obj.filename == "newwp.txt":
try:
wpmod = self.module('wp')
wpmod.wploader.load(obj.filename)
except Exception as ex:
print("wp load failed", ex)
if isinstance(obj, cuav_command.PreviewPacket):
# we have a preview image from the plane
img = cv2.imdecode(obj.jpeg, 1)
if self.preview_window is None or not self.preview_window.is_alive():
self.preview_window = mp_image.MPImage(title='Preview', width=410, height=308, auto_size=True)
if self.preview_window is not None:
self.preview_window.set_image(img, bgr=True)
self.preview_window.poll()
def rawname(self): '''return raw filename''' return '%s' % cuav_util.frame_time(self.frame_time)
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = cPickle.loads(str(buf))
if obj is None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time not in self.thumbs_received:
self.thumbs_received.add(obj.frame_time)
self.thumb_total_bytes += len(buf)
# add the thumbnails to the mosaic
thumbdec = cv2.imdecode(obj.thumb, 1)
if thumbdec is None:
pass
thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
# log the joe positions
# note the filename is where the fullsize image would be downloaded
# to, if requested
filename = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
self.log_joe_position(obj.pos, obj.frame_time, obj.regions, filename, None)
# update the mosaic and map
self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos)
# update console display
self.region_count += len(obj.regions)
self.thumb_count += 1
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' %
(self.thumb_total_bytes/self.thumb_count), row=7)
if isinstance(obj, cuav_command.ImagePacket):
# we have an image from the plane
self.image_total_bytes += len(buf)
#save to file
imagedec = cv2.imdecode(obj.jpeg, 1)
ff = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99]
cv2.imwrite(ff, imagedec, write_param)
self.mosaic.tag_image(obj.frame_time)
if obj.pos is not None:
self.mosaic.add_image(obj.frame_time, ff, obj.pos)
# update console
self.image_count += 1
color = 'black'
self.console.set_status('Images', 'Images %u' % self.image_count, row=6, fg=color)
self.console.set_status('ImageSize', 'ImageSize %.0f' %
(self.image_total_bytes/self.image_count), row=7)
if isinstance(obj, cuav_command.CommandPacket):
# ground doesn't accept command packets from air
pass
if isinstance(obj, cuav_command.CommandResponse):
# reply to CommandPacket
print('REMOTE: %s' % obj.response)
if isinstance(obj, cuav_command.CameraMessage):
print('CUAV AIR REMOTE: %s' % obj.msg)
def rawname(self):
'''return raw filename'''
return '%s' % cuav_util.frame_time(self.frame_time)