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