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 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) gammalog = open(os.path.join(state.camera_dir, "gamma.log"), "w") 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.depth == 16: im = numpy.zeros((960,1280),dtype='uint16') else: im = numpy.zeros((960,1280),dtype='uint8') if last_gamma != state.gamma: chameleon.set_gamma(h, state.gamma) last_gamma = state.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.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.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.depth = 8 self.gcs_address = None self.gcs_view_port = 7543 self.bandwidth = 120000 self.capture_brightness = 150 self.gamma = 950 self.lens = 5.0 self.brightness = 1.0 self.quality = 75 self.jpeg_size = 0 self.xmit_queue = 0 self.efficiency = 1.0 self.last_watch = 0 self.frame_loss = 0 self.colour = 1 self.boundary = None self.packet_loss = 0 self.save_pgm = True self.bandwidth_used = 0 self.rtt_estimate = 0 self.transmit = True # 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) self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir, 'joe.log'))
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) 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) if state.save_pgm: 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) 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) if state.save_pgm: chameleon.save_pgm('%s/%s.pgm' % (raw_dir, rawname), im)
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
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 __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)
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, 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) 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 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.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 __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) ] ) 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 # 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)
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""" 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") for f in files: frame_time = cuav_util.parse_frame_time(f) if mpos: 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_full(im_8bit, im_full) im_640 = numpy.zeros((480, 640, 3), dtype="uint8") scanner.downsample(im_full, im_640) else: im_full = cv.LoadImage(f) im_640 = cv.CreateImage((640, 480), 8, 3) cv.Resize(im_full, im_640) 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_full(im_full) else: regions = scanner.scan(img_scan) count += 1 regions = cuav_region.RegionsConvert(regions) t1 = time.time() if opts.filter: regions = cuav_region.filter_regions(im_full, regions, frame_time=frame_time, min_score=opts.minscore) 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, quality=opts.quality ) chameleon.save_file("composite.jpg", composite) thumbs = cuav_mosaic.ExtractThumbs(cv.LoadImage("composite.jpg"), 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() if opts.fullres: x1 *= 2 y1 *= 2 x2 *= 2 y2 *= 2 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 print ("%s scan %.1f fps %u regions [%u/%u]" % (f, count / total_time, region_count, scan_count, num_files))
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.depth == 16: im = numpy.zeros((600,800,3),dtype='uint16') else: im = numpy.zeros((600,800,3),dtype='uint8') #im = numpy.zeros((600,800),dtype='uint16') if last_gamma != state.gamma: chameleon.set_gamma(h, state.gamma) last_gamma = state.gamma frame_time, frame_counter, shutter = chameleon.capture(h, 1000, im) #cv.SaveImage("/home/root/opencv/blob/captures/downsampled.jpg",cv.fromarray(im)) #cv.cvSaveImage("/tmp/test.jpg",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.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: except Exception, 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.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.depth = 8 self.gcs_address = None self.gcs_view_port = 7543 self.bandwidth = 40000 self.bandwidth2 = 800 self.capture_brightness = 150 self.gamma = 950 self.c_params = CameraParams(lens=4.0) self.brightness = 1.0 self.quality = 75 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.colour = 1 self.boundary = None self.boundary_polygon = None self.packet_loss = 0 self.save_pgm = True self.bandwidth_used = 0 self.rtt_estimate = 0 self.transmit = True self.roll_stabilised = True self.minscore = 3 self.altitude = None self.bsocket = None self.bsend2 = None self.send2 = False self.thumbsize = 60 # 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 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