Пример #1
0
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
Пример #2
0
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)
Пример #3
0
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)
Пример #4
0
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)
Пример #5
0
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)
Пример #6
0
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)
Пример #7
0
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
Пример #8
0
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)
Пример #9
0
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)
Пример #10
0
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)
Пример #11
0
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
Пример #12
0
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
Пример #13
0
    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()
Пример #14
0
 def rawname(self):
     """return raw filename"""
     return "raw%s.pgm" % cuav_util.frame_time(self.frame_time)
Пример #15
0
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)
Пример #16
0
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
Пример #17
0
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)
Пример #18
0
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)
Пример #19
0
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)
Пример #20
0
    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()
Пример #21
0
 def rawname(self):
   '''return raw filename'''
   return '%s' % cuav_util.frame_time(self.frame_time)
Пример #22
0
    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)
Пример #23
0
 def rawname(self):
     '''return raw filename'''
     return '%s' % cuav_util.frame_time(self.frame_time)