def reload_mosaic(mosaic):
'''reload state into mosaic'''
state = mpstate.camera_state
regions = []
last_thumbfile = None
last_joe = None
joes = cuav_joe.JoeIterator(state.joelog.filename)
for joe in joes:
print joe
if joe.thumb_filename == last_thumbfile or last_thumbfile is None:
regions.append(joe.r)
last_joe = joe
last_thumbfile = joe.thumb_filename
else:
try:
composite = cv.LoadImage(last_joe.thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.set_brightness(state.settings.brightness)
mosaic.add_regions(regions, thumbs, last_joe.image_filename,
last_joe.pos)
except Exception:
pass
regions = []
last_joe = None
last_thumbfile = None
if last_joe:
try:
composite = cv.LoadImage(last_joe.thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.set_brightness(state.settings.brightness)
mosaic.add_regions(regions, thumbs, last_joe.image_filename,
last_joe.pos)
except Exception:
pass
def view_thread():
'''image viewing thread - this runs on the ground station'''
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_thread():
'''image viewing thread - this runs on the ground station'''
import cuav_mosaic
state = mpstate.camera_state
bsend = block_xmit.BlockSender(state.gcs_view_port, state.bandwidth)
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
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)
while not state.unload.wait(0.02):
if state.viewing:
bsend.tick(packet_count=1000)
cv.WaitKey(1)
if not view_window:
view_window = True
key = cv.WaitKey(1)
mosaic = cuav_mosaic.Mosaic(slipmap=mpstate.map,
lens=state.lens)
if state.boundary is not None:
boundary = cuav_util.polygon_load(state.boundary)
mosaic.set_boundary(boundary)
buf = bsend.recv(0)
if buf is None:
continue
try:
obj = cPickle.loads(str(buf))
if obj == None:
continue
except Exception as e:
continue
if isinstance(obj, ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
thumb_total_bytes += len(buf)
# save the thumbnails
filename = '%s/v%s.jpg' % (
thumb_dir, cuav_util.frame_time(obj.frame_time))
chameleon.save_file(filename, obj.thumb)
composite = cv.LoadImage(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 = get_plane_position(obj.frame_time)
log_joe_position(pos, obj.frame_time, obj.regions, filename)
# update the mosaic and map
mosaic.add_regions(obj.regions,
thumbs,
obj.latlon_list,
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)
# 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)
# work out where we were at the time
try:
pos = state.mpos.position(obj.frame_time, 0)
except mav_position.MavInterpolatorException, msg:
print msg
pos = None
if pos:
mosaic.add_image(filename, img, pos)
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
cv.ConvertScale(display_img,
display_img,
scale=state.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))
else:
if view_window:
view_window = False