def start_gcs_bsend(self): '''start up block senders for GCS side''' if self.msend is None: self.msocket = cuav_command.MavSocket(self.mpstate.mav_master[0]) self.msend = block_xmit.BlockSender(mss=96, sock=self.msocket, dest_ip='mavlink', dest_port=0, backlog=5, debug=False) self.msend.set_bandwidth(500) if len(self.bsend) == 0: for lnk in self.camera_settings.air_address.split(','): try: [remoteip, remoteport, localport, bw] = lnk.split(':') newbsnd = block_xmit.BlockSender(bandwidth=int(bw), debug=False, dest_ip=remoteip, dest_port=int(remoteport), port=int(localport)) self.bsend.append(newbsnd) except: print( "Bad Air endpoint (must be remIP:remport:localport:bw): " + str(lnk)) pass
def test_camera_start(mpstate, image_file): '''put a few images through the module and check they come out via the block xmit''' loadedModule = camera_air.init(mpstate) parms = "/data/ChameleonArecort/params.json" loadedModule.cmd_camera(["set", "camparms", parms]) loadedModule.cmd_camera(["set", "imagefile", image_file]) loadedModule.cmd_camera(["set", "minscore", "0"]) loadedModule.cmd_camera(["set", "gcs_address", "127.0.0.1:14550:14560:9000, 127.0.0.1:14650:14660:15000"]) #Set up the ground side recievers b1 = block_xmit.BlockSender(dest_ip='127.0.0.1', port = 14550, dest_port = 14560) b2 = block_xmit.BlockSender(dest_ip='127.0.0.1', port = 14650, dest_port = 14660) blk1 = None blk2 = None b2.tick() b1.tick() capture_thread = sim_camera() time.sleep(0.05) loadedModule.cmd_camera(["start"]) time.sleep(0.8) loadedModule.cmd_camera(["status"]) loadedModule.cmd_camera(["queue"]) #get the sent data b2.tick() b1.tick() blk1 = cPickle.loads(str(b1.recv(0.1))) blk2 = cPickle.loads(str(b2.recv(0.1))) time.sleep(0.05) loadedModule.cmd_camera(["status"]) loadedModule.cmd_camera(["stop"]) loadedModule.unload() capture_thread.join(1.0) assert loadedModule.capture_count == 3 assert loadedModule.scan_count == 3 assert loadedModule.region_count > 0 assert isinstance(blk1, cuav_command.StampedCommand) assert isinstance(blk2, cuav_command.StampedCommand) assert abs(blk1.timestamp - blk2.timestamp) < 0.01
def start_aircraft_bsend(self): '''start bsend for aircraft side''' if self.msend is None: self.msocket = cuav_command.MavSocket(self.mpstate.mav_master[0]) self.msend = block_xmit.BlockSender(mss=96, sock=self.msocket, dest_ip='mavlink', dest_port=0, backlog=5, debug=False) self.msend.set_bandwidth(self.camera_settings.m_bandwidth)
def test_camera_image_request(mpstate, image_file): '''image request via the block_xmit''' loadedModule = camera_air.init(mpstate) parms = "/data/ChameleonArecort/params.json" loadedModule.cmd_camera(["set", "camparms", parms]) loadedModule.cmd_camera(["set", "imagefile", image_file]) loadedModule.cmd_camera(["set", "minscore", "0"]) loadedModule.cmd_camera(["set", "gcs_address", "127.0.0.1:14550:14560:2000000"]) filename = os.path.join(os.getcwd(), 'tests', 'testdata', 'raw2016111223465160Z.png') pkt = cuav_command.ImageRequest(cuav_util.parse_frame_time(filename), True) buf = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL) capture_thread = sim_camera() time.sleep(0.05) loadedModule.cmd_camera(["start"]) time.sleep(0.8) loadedModule.cmd_camera(["status"]) #don't load the block xmit until afterwards b1 = block_xmit.BlockSender(dest_ip='127.0.0.1', port = 14550, dest_port = 14560) b1.tick() b1.send(buf) #b1.tick() time.sleep(0.1) blkret = [] while True: try: b1.tick() blk = cPickle.loads(str(b1.recv(0.01, True))) #only want paricular packets - discard all the heartbeats, etc if isinstance(blk, cuav_command.ImagePacket): blkret.append(blk) break time.sleep(0.05) except cPickle.UnpicklingError: continue loadedModule.cmd_camera(["stop"]) loadedModule.unload() capture_thread.join(1.0) assert len(blkret) == 1 #not sure if the last or 2nd last packed will contain the image - depends on the exact thread timing assert isinstance(blkret[0], cuav_command.ImagePacket) assert blkret[0].jpeg is not None
def test_camera_airstart(mpstate, image_file): '''test the airstart - that cauv auto starts after vehicle has taken off''' loadedModule = camera_air.init(mpstate) parms = "/data/ChameleonArecort/params.json" loadedModule.cmd_camera(["set", "camparms", parms]) loadedModule.cmd_camera(["set", "imagefile", image_file]) loadedModule.cmd_camera(["set", "minspeed", "20"]) loadedModule.cmd_camera(["set", "gcs_address", "127.0.0.1:14550:14560:2000000"]) #load the other side of the xmit link b1 = block_xmit.BlockSender(dest_ip='127.0.0.1', port = 14550, dest_port = 14560) blkret = [] b1.tick() loadedModule.cmd_camera(["airstart"]) #send a mavlink packet of VFR_HUD that the airspeed is above the threshold #See "common.py" in pymavlink dialects for message classes msg = common.MAVLink_vfr_hud_message(airspeed=21, groundspeed=15, heading=90, throttle=80, alt=30, climb=3.2) msg._timestamp = time.time() loadedModule.mavlink_packet(msg) #get the packets time.sleep(0.1) while True: try: b1.tick() blk = cPickle.loads(str(b1.recv(0.01, True))) if isinstance(blk, cuav_command.CommandResponse) or isinstance(blk, cuav_command.CameraMessage): blkret.append(blk) time.sleep(0.05) except cPickle.UnpicklingError: break loadedModule.cmd_camera(["stop"]) loadedModule.unload() assert len(blkret) == 2 assert blkret[0].msg == "cuav airstart ready" assert blkret[1].msg == "Started cuav running"
def test_camera_command(mpstate, image_file): '''send some commands via the block_xmit''' loadedModule = camera_air.init(mpstate) parms = "/data/ChameleonArecort/params.json" loadedModule.cmd_camera(["set", "camparms", parms]) loadedModule.cmd_camera(["set", "imagefile", image_file]) loadedModule.cmd_camera(["set", "minscore", "0"]) loadedModule.cmd_camera(["set", "gcs_address", "127.0.0.1:14550:14560:9000"]) pkt = cuav_command.ChangeCameraSetting("minscore", 50) b1 = block_xmit.BlockSender(dest_ip='127.0.0.1', port = 14550, dest_port = 14560) buf = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL) loadedModule.cmd_camera(["start"]) time.sleep(0.1) b1.tick() b1.send(buf) b1.tick() time.sleep(0.1) loadedModule.cmd_camera(["stop"]) loadedModule.unload() assert loadedModule.camera_settings.minscore == 50
def view_thread(): '''image viewing thread - this runs on the ground station''' from cuav.lib import cuav_mosaic state = mpstate.camera_state bsend = block_xmit.BlockSender(state.settings.gcs_view_port, bandwidth=state.settings.bandwidth) state.bsocket = MavSocket(mpstate.mav_master[0]) state.bsend2 = block_xmit.BlockSender(mss=96, sock=state.bsocket, dest_ip='mavlink', dest_port=0, backlog=5, debug=False) state.bsend2.set_bandwidth(state.settings.bandwidth2) view_window = False image_count = 0 thumb_count = 0 image_total_bytes = 0 jpeg_total_bytes = 0 thumb_total_bytes = 0 region_count = 0 mosaic = None thumbs_received = set() view_dir = os.path.join(state.camera_dir, "view") thumb_dir = os.path.join(state.camera_dir, "thumb") cuav_util.mkdir_p(view_dir) cuav_util.mkdir_p(thumb_dir) img_window = mp_image.MPImage(title='Camera') mpstate.console.set_status('Images', 'Images %u' % image_count, row=6) mpstate.console.set_status('Lost', 'Lost %u' % 0, row=6) mpstate.console.set_status('Regions', 'Regions %u' % region_count, row=6) mpstate.console.set_status('JPGSize', 'JPGSize %.0f' % 0.0, row=6) mpstate.console.set_status('XMITQ', 'XMITQ %.0f' % 0.0, row=6) mpstate.console.set_status('Thumbs', 'Thumbs %u' % thumb_count, row=7) mpstate.console.set_status('ThumbSize', 'ThumbSize %.0f' % 0.0, row=7) mpstate.console.set_status('ImageSize', 'ImageSize %.0f' % 0.0, row=7) ack_time = time.time() while not state.unload.wait(0.02): if state.viewing: tnow = time.time() if tnow - ack_time > 0.1: bsend.tick(packet_count=1000, max_queue=state.settings.maxqueue1) state.bsend2.tick(packet_count=1000, max_queue=state.settings.maxqueue2) if state.bsend_slave is not None: state.bsend_slave.tick(packet_count=1000) ack_time = tnow if not view_window: view_window = True mosaic = cuav_mosaic.Mosaic(slipmap=mpstate.map, C=state.c_params) if state.boundary_polygon is not None: mosaic.set_boundary(state.boundary_polygon) if mpstate.continue_mode: reload_mosaic(mosaic) # check for keyboard events mosaic.check_events() buf = bsend.recv(0) if buf is None: buf = state.bsend2.recv(0) if buf is None: continue try: obj = cPickle.loads(str(buf)) if obj == None: continue except Exception as e: continue if state.settings.gcs_slave is not None: if state.bsend_slave is None: state.bsend_slave = block_xmit.BlockSender(0, bandwidth=state.settings.bandwidth*10, debug=False) state.bsend_slave.send(buf, dest=(state.settings.gcs_slave, state.settings.gcs_view_port), priority=1) if isinstance(obj, ThumbPacket): # we've received a set of thumbnails from the plane for a positive hit if obj.frame_time in thumbs_received: continue thumbs_received.add(obj.frame_time) thumb_total_bytes += len(buf) # save the thumbnails thumb_filename = '%s/v%s.jpg' % (thumb_dir, cuav_util.frame_time(obj.frame_time)) chameleon.save_file(thumb_filename, obj.thumb) composite = cv.LoadImage(thumb_filename) thumbs = cuav_mosaic.ExtractThumbs(composite, len(obj.regions)) # log the joe positions filename = '%s/v%s.jpg' % (view_dir, cuav_util.frame_time(obj.frame_time)) pos = obj.pos log_joe_position(pos, obj.frame_time, obj.regions, filename, thumb_filename) # update the mosaic and map mosaic.set_brightness(state.settings.brightness) mosaic.add_regions(obj.regions, thumbs, filename, pos=pos) # update console display region_count += len(obj.regions) state.frame_loss = obj.frame_loss state.xmit_queue = obj.xmit_queue thumb_count += 1 mpstate.console.set_status('Lost', 'Lost %u' % state.frame_loss) mpstate.console.set_status('Regions', 'Regions %u' % region_count) mpstate.console.set_status('XMITQ', 'XMITQ %.0f' % state.xmit_queue) mpstate.console.set_status('Thumbs', 'Thumbs %u' % thumb_count) mpstate.console.set_status('ThumbSize', 'ThumbSize %.0f' % (thumb_total_bytes/thumb_count)) if isinstance(obj, ImagePacket): # we have an image from the plane image_total_bytes += len(buf) state.xmit_queue = obj.xmit_queue mpstate.console.set_status('XMITQ', 'XMITQ %.0f' % state.xmit_queue) # save it to disk filename = '%s/v%s.jpg' % (view_dir, cuav_util.frame_time(obj.frame_time)) chameleon.save_file(filename, obj.jpeg) img = cv.LoadImage(filename) if img.width == 1280: display_img = cv.CreateImage((640, 480), 8, 3) cv.Resize(img, display_img) else: display_img = img mosaic.add_image(obj.frame_time, filename, obj.pos) cv.ConvertScale(display_img, display_img, scale=state.settings.brightness) img_window.set_image(display_img, bgr=True) # update console image_count += 1 jpeg_total_bytes += len(obj.jpeg) state.jpeg_size = 0.95 * state.jpeg_size + 0.05 * len(obj.jpeg) mpstate.console.set_status('Images', 'Images %u' % image_count) mpstate.console.set_status('JPGSize', 'JPG Size %.0f' % (jpeg_total_bytes/image_count)) mpstate.console.set_status('ImageSize', 'ImageSize %.0f' % (image_total_bytes/image_count)) if isinstance(obj, CommandResponse): print('REMOTE: %s' % obj.response) else: if view_window: view_window = False
def transmit_thread(): '''thread for image transmit to GCS''' state = mpstate.camera_state tx_count = 0 skip_count = 0 bsend = block_xmit.BlockSender(0, bandwidth=state.settings.bandwidth, debug=False) 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) while not state.unload.wait(0.02): bsend.tick(packet_count=1000, max_queue=state.settings.maxqueue1) state.bsend2.tick(packet_count=1000, max_queue=state.settings.maxqueue2) check_commands() if state.transmit_queue.empty(): continue (frame_time, regions, im_full, im_640) = state.transmit_queue.get() if state.settings.roll_stabilised: roll=0 else: roll=None pos = get_plane_position(frame_time, roll=roll) # this adds the latlon field to the regions log_joe_position(pos, frame_time, regions) # filter out any regions outside the boundary if state.boundary_polygon: regions = cuav_region.filter_boundary(regions, state.boundary_polygon, pos) regions = cuav_region.filter_regions(im_full, regions, min_score=state.settings.minscore) state.xmit_queue = bsend.sendq_size() state.xmit_queue2 = state.bsend2.sendq_size() state.efficiency = bsend.get_efficiency() state.bandwidth_used = bsend.get_bandwidth_used() state.rtt_estimate = bsend.get_rtt_estimate() jpeg = None if len(regions) > 0: lowscore = 0 highscore = 0 for r in regions: lowscore = min(lowscore, r.score) highscore = max(highscore, r.score) if state.settings.transmit: # send a region message with thumbnails to the ground station thumb = None if state.settings.send1: thumb_img = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)), regions, thumb_size=state.settings.thumbsize) thumb = scanner.jpeg_compress(numpy.ascontiguousarray(cv.GetMat(thumb_img)), state.settings.quality) pkt = ThumbPacket(frame_time, regions, thumb, state.frame_loss, state.xmit_queue, pos) buf = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL) bsend.set_bandwidth(state.settings.bandwidth) bsend.set_packet_loss(state.settings.packet_loss) bsend.send(buf, dest=(state.settings.gcs_address, state.settings.gcs_view_port), priority=1) # also send thumbnails via 900MHz telemetry if state.settings.send2 and highscore >= state.settings.minscore2: if thumb is None or lowscore < state.settings.minscore2: # remove some of the regions regions = cuav_region.filter_regions(im_full, regions, min_score=state.settings.minscore2) thumb_img = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)), regions, thumb_size=state.settings.thumbsize) thumb = scanner.jpeg_compress(numpy.ascontiguousarray(cv.GetMat(thumb_img)), state.settings.quality) pkt = ThumbPacket(frame_time, regions, thumb, state.frame_loss, state.xmit_queue, pos) buf = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL) state.bsend2.set_bandwidth(state.settings.bandwidth2) state.bsend2.send(buf, priority=highscore) # Base how many images we send on the send queue size send_frequency = state.xmit_queue // 3 if send_frequency == 0 or (tx_count+skip_count) % send_frequency == 0: jpeg = scanner.jpeg_compress(im_640, state.settings.quality) if jpeg is None: skip_count += 1 continue # keep filtered image size state.jpeg_size = 0.95 * state.jpeg_size + 0.05 * len(jpeg) tx_count += 1 if state.settings.gcs_address is None: continue bsend.set_packet_loss(state.settings.packet_loss) bsend.set_bandwidth(state.settings.bandwidth) pkt = ImagePacket(frame_time, jpeg, state.xmit_queue, pos) str = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL) bsend.send(str, dest=(state.settings.gcs_address, state.settings.gcs_view_port))
action='store_true', default=False, help="verbose debug") parser.add_option("--mss", type='int', default=0, help="maximum segment size") parser.add_option("--count", type='int', default=1, help="number of blocks to send") (opts, args) = parser.parse_args() import hashlib bs = block_xmit.BlockSender(opts.port, dest_ip=opts.dest_ip, listen_ip=opts.listen_ip, bandwidth=opts.bandwidth, chunk_size=opts.chunk_size, backlog=opts.backlog, mss=opts.mss, debug=opts.debug) if opts.loss: print("set packet loss of %.1f%%" % (100.0 * opts.loss)) bs.set_packet_loss(opts.loss) if opts.receive: while True: data = bs.recv(0.01) if data is not None: print("received %u bytes sum %s" % (len(data), hashlib.md5(data).hexdigest())) bs.tick()
def test_block_xmit(): debug = False bandwidth = 100000 ordered = False num_blocks = 10 packet_loss = 0 average_block_size = 5000 # setup a send/recv pair b1 = block_xmit.BlockSender(dest_ip='127.0.0.1', debug=debug, bandwidth=bandwidth, ordered=ordered) b2 = block_xmit.BlockSender(dest_ip='127.0.0.1', debug=debug, bandwidth=bandwidth, ordered=ordered) # setup for some packet loss if packet_loss: b1.set_packet_loss(packet_loss) b2.set_packet_loss(packet_loss) # tell b1 the port b2 got allocated b1.set_dest_port(b2.get_port()) b2.set_dest_port(b1.get_port()) # generate some data blocks to work with blocks = [ bytes(os.urandom(random.randint(1, average_block_size))) for i in range(num_blocks) ] total_size = sum([len(blk) for blk in blocks]) t0 = time.time() # send them from b1 to b2 and from b2 to b1 for blk in blocks: b1.send(blk) b2.send(blk) received1 = [] received2 = [] # wait till they have all been received and acked while (b1.sendq_size() > 0 or b2.sendq_size() > 0 or len(received1) != num_blocks or len(received2) != num_blocks): b1.tick() b2.tick() blk = b2.recv(0.01) if blk is not None: received2.append(blk) blk = b1.recv(0.01) if blk is not None: received1.append(blk) if int(time.time()) - int(t0) > 2: #timeout assert False t1 = time.time() if not ordered: blocks.sort() received1.sort() received2.sort() assert blocks == received1 assert blocks == received2