class CameraAirModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraAirModule, self).__init__(mpstate, "camera_air", "cuav camera control (air)", public = True)
self.running = False
self.unload_event = threading.Event()
self.unload_event.clear()
self.capture_thread = None
self.scan_thread = None
self.transmit_thread = None
self.airstart_triggered = False
self.terrain_alt = None
self.handled_timestamps = {}
self.imagefilenamemapping = {}
self.posmapping = {}
self.is_armed = True
self.lz = cuav_landingregion.LandingZone()
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.camera_settings = MPSettings(
[ MPSetting('roll_stabilised', bool, False, 'Roll Stabilised'),
MPSetting('roll_limit', float, 0, 'Roll stabilisation limit'),
MPSetting('minspeed', int, 20, 'For airstart, minimum speed for capture to start'),
MPSetting('minalt', int, 30, 'MinAltitude of images', range=(0,10000), increment=1),
MPSetting('rotate180', bool, False, 'rotate images by 180', tab='Capture2'),
MPSetting('ignoretimestamps', bool, False, 'Ignore image timestamps', tab='Capture2'),
MPSetting('camparms', str, None, 'camera parameters file (json) in cuav package', tab='Imaging'),
MPSetting('imagefile', str, None, 'latest captured image', tab='Imaging'),
MPSetting('filter_type', str, 'simple', 'Filter Type',
choice=['simple'], tab='Imaging'),
MPSetting('blue_emphasis', bool, False, 'BlueEmphasis', tab='Imaging'),
MPSetting('use_capture_time', bool, True, 'Use Capture Time (false for sim)', tab='Simulation'),
MPSetting('target_latitude', float, 0, 'filter detected images to latitude', tab='Filter to Location'),
MPSetting('target_longitude', float, 0, 'filter detected images to longitude', tab='Filter to Location'),
MPSetting('target_radius', float, 0, 'filter detected images to radius', tab='Filter to Location'),
MPSetting('gcs_address', str, "", 'GCS Addresses in RemIP:RemPort:LocalPort:Bandwidth format (127.0.0.1:1440:1234:45, ...)', tab='GCS'),
MPSetting('qualitysend', int, 90, 'Compression Quality for send', range=(1,100), increment=1, tab='GCS'),
MPSetting('transmit', bool, True, 'Transmit Enable for thumbnails', tab='GCS'),
MPSetting('maxqueue', int, 50, 'Maximum images queue', tab='GCS'),
MPSetting('thumbsize', int, 60, 'Thumbnail Size', range=(10, 200), increment=1),
MPSetting('minscore', int, 1000, 'Min Score to pass detection', range=(0,100000), increment=1, tab='Imaging'),
MPSetting('clock_sync', bool, False, 'GPS Clock Sync'),
MPSetting('m_minscore', int, 20000, 'Min Score to pass detection on mavlink', range=(0,100000), increment=1, tab='Imaging'),
MPSetting('m_bandwidth', int, 500, 'max bandwidth on mavlink', increment=1, tab='GCS'),
MPSetting('m_maxqueue', int, 5, 'Maximum images queue for mavlink', tab='GCS'),
MPSetting('preview', bool, True, 'enable camera preview window', tab='Imaging'),
MPSetting('previewquality', int, 40, 'Compression Quality for preview', range=(1,100), increment=1, tab='Imaging'),
MPSetting('previewscale', int, 5, 'preview downscaling', range=(1,10), increment=1, tab='Imaging'),
MPSetting('previewfreq', int, 4, 'preview image frequency', range=(1,10), increment=1, tab='Imaging'),
],
title='Camera Settings'
)
self.image_settings = MPSettings(
[ MPSetting('MinRegionArea', float, 0.15, range=(0,100), increment=0.05, digits=2, tab='Image Processing'),
MPSetting('MaxRegionArea', float, 1.0, range=(0,100), increment=0.1, digits=1, tab='Image Processing'),
MPSetting('MinRegionSize', float, 0.2, range=(0,100), increment=0.05, digits=2, tab='Image Processing'),
MPSetting('MaxRegionSize', float, 1.0, range=(0,100), increment=0.1, digits=1, tab='Image Processing'),
MPSetting('MaxRarityPct', float, 0.02, range=(0,100), increment=0.01, digits=2, tab='Image Processing'),
MPSetting('RegionMergeSize', float, 1.0, range=(0,100), increment=0.1, digits=1, tab='Image Processing'),
],
title='Image Settings')
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.scan_fps = 0
self.scan_queue = multiproc.Queue()
self.transmit_queue = multiproc.Queue()
self.have_set_gps_time = False
self.c_params = None
self.jpeg_size = 0
self.xmit_queue = []
self.efficiency = []
self.last_watch = 0
self.boundary = None
self.boundary_polygon = None
self.bandwidth_used = []
self.rtt_estimate = []
self.bsend = [] #note this is an array of bsends
# msend is a BlockSender over MAVLink
self.msocket = None
self.msend = None
self.last_heartbeat = time.time()
self.mpos = mav_position.MavInterpolator(backlog=500, gps_lag=0.0)
self.joelog = None #cuav_joe.JoeLog(os.path.join(self.settings.imagefile, '..', 'joe.log'), append=self.continue_mode)
self.add_command('camera', self.cmd_camera,
'camera control',
['<start|stop|status|boundary|airstart>',
'set (CAMERASETTING)'])
self.add_completion_function('(CAMERASETTING)', self.camera_settings.completion)
# prevent loopback of messages
for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
self.module('link').no_fwd_types.add(mtype)
print("camera initialised")
def get_bsend(self, bsnd):
'''get a bsend object, given a tag name'''
if bsnd is None:
return None
if bsnd == 'msend':
return self.msend
return self.bsend[bsnd]
def get_bsend_index(self, bsnd):
'''get a bsend index from a bsend object. This avoids pickling a block xmit object'''
if bsnd is None or isinstance(bsnd, int) or bsnd == 'msend':
return bsnd
if bsnd == self.msend:
return 'msend'
return self.bsend.index(bsnd)
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <start|airstart|stop|status|queue|set>"
if len(args) == 0:
print(usage)
return
if args[0] == "start":
self.capture_count = 0
self.error_count = 0
self.error_msg = None
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params " + str(self.camera_settings.camparms))
return
if self.running == False:
self.running = True
self.joelog = cuav_joe.JoeLog(os.path.join(os.path.dirname(self.camera_settings.imagefile), 'joe_air.log'), append=self.continue_mode)
self.capture_thread = self.start_thread(self.capture_threadfunc)
self.scan_thread = self.start_thread(self.scan_threadfunc)
self.transmit_thread = self.start_thread(self.transmit_threadfunc)
time.sleep(0.1)
self.send_message("Started cuav running")
print("Started cuav running")
else:
self.send_message("cuav already running")
print("cuav already running")
elif args[0] == "stop":
self.running = False
self.airstart_triggered = False
print("Stopped cuav")
self.send_message("Stopped cuav")
elif args[0] == "status":
ret = "Cap imgs:%u err:%u scan:%u regions:%u jsize:%.0f xmitq:%s sq:%u eff:%s" % (
self.capture_count, self.error_count, self.scan_count,
self.region_count,
self.jpeg_size,
self.xmit_queue, self.scan_queue.qsize(),
self.efficiency)
print(ret)
self.send_message(ret)
elif args[0] == "queue":
ret = "scan %u transmit %u eff %s bw %s rtt %s" % (
self.scan_queue.qsize(),
self.transmit_queue.qsize(),
self.efficiency,
self.bandwidth_used,
self.rtt_estimate)
print(ret)
elif args[0] == "set":
self.camera_settings.command(args[1:])
elif args[0] == "airstart":
#just keep the block xmit going for now
self.capture_count = 0
self.error_count = 0
self.error_msg = None
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params " + str(self.camera_settings.camparms))
return
if self.airstart_triggered == False:
self.airstart_triggered = True
self.joelog = cuav_joe.JoeLog(os.path.join(os.path.dirname(self.camera_settings.imagefile), 'joe_air.log'), append=self.continue_mode)
self.transmit_thread = self.start_thread(self.transmit_threadfunc)
time.sleep(0.1)
self.send_message("cuav airstart ready")
print("cuav airstart ready")
else:
self.send_message("cuav airstart already running")
print("cuav airstart already running")
else:
print(usage)
def check_camera_parms(self):
'''check for change in camera parameters'''
#dir is rel to this python file:
if self.camera_settings.camparms is None:
return False
camfiletxt = pkg_resources.resource_string("cuav", self.camera_settings.camparms)
if sys.version_info.major >= 3:
camfiletxt = camfiletxt.decode('utf-8')
try:
self.c_params = CameraParams.fromstring(camfiletxt)
return True
except:
return False
def capture_threadfunc(self):
'''image capture thread, via monitoring the
link for changed linked filenames'''
prev_image = None
self.scan_queue = multiproc.Queue()
while not self.unload_event.wait(0.05):
try:
filename = os.path.realpath(self.camera_settings.imagefile)
if not self.camera_settings.ignoretimestamps:
filetime = cuav_util.parse_frame_time(filename)
else:
filetime = float(time.time())
except Exception:
filename = None
pass
#ensure all items are valid and the queue isn't overfilled > 100
if filename != None and prev_image != filename and filetime != None and self.scan_queue.qsize() < 100:
self.scan_queue.put((filetime, filename))
self.imagefilenamemapping[str(filetime)] = filename
self.capture_count += 1
prev_image = filename
if self.is_armed:
stopfile = self.camera_settings.imagefile + ".stop"
if os.path.exists(stopfile):
print("Removing stopfile")
os.unlink(self.camera_settings.imagefile + ".stop")
def scan_threadfunc(self):
'''image scanning thread'''
while not self.unload_event.wait(0.05):
if self.scan_queue.empty():
continue
(frame_time,im) = self.scan_queue.get()
scan_parms = {}
for name in self.image_settings.list():
scan_parms[name] = self.image_settings.get(name)
scan_parms['BlueEmphasis'] = float(self.camera_settings.blue_emphasis)
if self.terrain_alt is not None:
altitude = self.terrain_alt
if altitude < self.camera_settings.minalt:
altitude = self.camera_settings.minalt
scan_parms['MetersPerPixel'] = cuav_util.pixel_width(altitude,
self.c_params.xresolution,
self.c_params.lens,
self.c_params.sensorwidth)
t1 = time.time()
try:
img_scan = cv2.imread(im, -1)
except Exception:
continue
if img_scan is None:
continue
(h, w) = img_scan.shape[:2]
if self.camera_settings.rotate180:
M = cv2.getRotationMatrix2D(center, angle180, scale)
img_scan = cv2.warpAffine(img_scan, M, (w, h))
im_numpy = numpy.ascontiguousarray(img_scan)
regions = scanner.scan(im_numpy, scan_parms)
regions = cuav_region.RegionsConvert(regions,
cuav_util.image_shape(img_scan),
cuav_util.image_shape(img_scan))
t2 = time.time()
self.scan_fps = 1.0 / (t2-t1)
self.scan_count += 1
regions = cuav_region.filter_regions(img_scan, regions,
min_score=self.camera_settings.minscore,
filter_type=self.camera_settings.filter_type)
self.region_count += len(regions)
# possibly send a preview image
self.send_preview(img_scan)
if self.camera_settings.roll_stabilised:
roll=0
else:
roll=None
pos = self.get_plane_position(frame_time, roll=roll)
if pos is not None:
self.posmapping[str(frame_time)] = pos
# this adds the latlon field to the regions (georeferencing)
for r in regions:
r.latlon = cuav_util.gps_position_from_image_region(r, pos, w, h, altitude=None, C=self.c_params)
if self.joelog:
self.log_joe_position(pos, frame_time, regions)
# filter out any regions outside the target radius
if self.camera_settings.target_radius > 0 and pos is not None:
regions = cuav_region.filter_radius(regions,
(self.camera_settings.target_latitude,
self.camera_settings.target_longitude),
self.camera_settings.target_radius)
# filter out any regions outside the boundary
if self.boundary_polygon:
regions = cuav_region.filter_boundary(regions, self.boundary_polygon, pos)
#filter by minscore
regions = cuav_region.filter_regions(img_scan, regions, min_score=self.camera_settings.minscore,
filter_type=self.camera_settings.filter_type)
high_score = 1
for r in regions:
if r.score > high_score:
high_score = r.score
if len(regions) > 0 and pos is not None:
for r in regions:
self.lz.checkaddregion(r, pos)
try:
lzresult = self.lz.calclandingzone()
except Exception as ex:
print("calclandingzone failed: ", ex)
continue
if lzresult:
self.transmit_queue.put((lzresult, 100000, None))
if self.msend:
self.transmit_queue.put((lzresult, 100000, 'msend'))
if len(regions) > 0 and self.camera_settings.transmit:
# send a region message with thumbnails to the ground station
thumb_img = cuav_region.CompositeThumbnail(img_scan, regions,
thumb_size=self.camera_settings.thumbsize)
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90]
(result, thumb) = cv2.imencode('.jpg', thumb_img, encode_param)
pkt = cuav_command.ThumbPacket(frame_time, regions, thumb, pos)
if self.transmit_queue.qsize() < 100:
self.transmit_queue.put((pkt, None, None))
if self.msend is not None and high_score >= self.camera_settings.m_minscore:
self.transmit_queue.put((pkt, None, 'msend'))
else:
self.send_message("Warning: image Tx queue too long")
print("Warning: image Tx queue too long")
def get_plane_position(self, frame_time,roll=None):
'''get a MavPosition object for the planes position if possible'''
try:
pos = self.mpos.position(frame_time, 0, roll=roll, maxroll=self.camera_settings.roll_limit)
return pos
except mav_position.MavInterpolatorException as e:
print(str(e))
return None
def log_joe_position(self, pos, frame_time, regions, filename=None, thumb_filename=None):
'''add to joe.log if possible, returning a list of (lat,lon) tuples
for the positions of the identified image regions'''
return self.joelog.add_regions(frame_time, regions, pos, filename, thumb_filename)
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def transmit_threadfunc(self):
'''thread for image and message transmit to camera_ground
in addition to reading commands from the camera_ground'''
self.start_aircraft_bsend()
self.spacewarning = False
while (not self.unload_event.wait(0.05)) or self.airstart_triggered:
for bsnd in self.bsend:
bsnd.tick(packet_count=1000, max_queue=self.camera_settings.maxqueue)
try:
self.check_commands(bsnd)
except Exception as ex:
print("Failed command", ex)
if self.msend is not None:
self.msend.tick(packet_count=1000, max_queue=self.camera_settings.m_maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
#check remaining disk space and warn user if required
try:
stat = os.statvfs(os.path.dirname(self.camera_settings.imagefile))
if not self.spacewarning and stat.f_bfree*stat.f_bsize < 20971520:
self.send_message("Warning: <200Mb disk space left on cuav_air")
self.spacewarning = True
except OSError:
pass
while not self.transmit_queue.empty():
(pkt, priority, linktosend) = self.transmit_queue.get()
linktosend = self.get_bsend(linktosend)
self.send_object(pkt, priority, linktosend)
#update the stats
self.xmit_queue = []
self.efficiency = []
self.bandwidth_used = []
self.rtt_estimate = []
for bsnd in self.bsend:
self.xmit_queue.append(bsnd.sendq_size())
self.efficiency.append(bsnd.get_efficiency())
self.bandwidth_used.append(bsnd.get_bandwidth_used())
self.rtt_estimate.append(bsnd.get_rtt_estimate())
if self.msend is not None:
self.xmit_queue.append(self.msend.sendq_size())
self.efficiency.append(self.msend.get_efficiency())
self.bandwidth_used.append(self.msend.get_bandwidth_used())
self.rtt_estimate.append(self.msend.get_rtt_estimate())
def send_image(self, img, frame_time, priority, pos, linktosend):
'''send an image object to the GCS'''
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), self.camera_settings.qualitysend]
(result, jpeg) = cv2.imencode('.jpg', img, encode_param)
# keep filtered image size
self.jpeg_size = 0.95 * self.jpeg_size + 0.05 * len(jpeg)
pkt = cuav_command.ImagePacket(frame_time, jpeg, pos, priority)
self.transmit_queue.put((pkt, priority, self.get_bsend_index(linktosend)))
def send_preview(self, img):
'''send a preview image object to the GCS'''
if not self.camera_settings.preview or self.transmit_queue.qsize() > 3:
# only send when link is nearly idle
return
if self.scan_count % self.camera_settings.previewfreq != 0:
return
scale = 1.0 / self.camera_settings.previewscale
small_img = cv2.resize(img, (0,0), fx=scale, fy=scale)
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), self.camera_settings.previewquality]
(result, jpeg) = cv2.imencode('.jpg', small_img, encode_param)
pkt = cuav_command.PreviewPacket(jpeg)
self.transmit_queue.put((pkt, 1, None))
def send_file(self, filename):
'''send a file over all links'''
try:
contents = open(filename).read()
filename = os.path.basename(filename)
except Exception:
return
pkt = cuav_command.FilePacket(filename, contents)
# send over all links
self.transmit_queue.put((pkt, 20000, None))
if self.msend:
self.transmit_queue.put((pkt, 20000, 'msend'))
def start_aircraft_bsend(self):
'''start bsend for aircraft side'''
if len(self.bsend) == 0:
for lnk in self.camera_settings.gcs_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 GCS endpoint (must be remIP:remport:localport:bw): " + str(lnk))
pass
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 start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.running = False
self.unload_event.set()
if self.capture_thread is not None:
self.capture_thread.join(1.0)
self.scan_thread.join(1.0)
self.transmit_thread.join(1.0)
print('camera unload OK')
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 == 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.ImageRequest):
self.handle_image_request(obj, bsend)
if isinstance(obj, cuav_command.ChangeCameraSetting):
self.camera_settings.set(obj.name, obj.value)
self.camera_settings_callback(obj)
if isinstance(obj, cuav_command.ChangeImageSetting):
self.image_settings.set(obj.name, obj.value)
self.image_settings_callback(obj)
if isinstance(obj, cuav_command.CommandPacket):
self.handle_command_packet(obj, bsend)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if self.mpstate.status.watch in ["camera","queue"] and time.time() > self.last_watch+1:
self.last_watch = time.time()
self.cmd_camera(["status" if self.mpstate.status.watch == "camera" else "queue"])
# update position interpolator
self.mpos.add_msg(m)
if m.get_type() == 'SYSTEM_TIME' and self.camera_settings.clock_sync and self.capture_thread is not None:
# optionally sync system clock on the capture side
self.sync_gps_clock(m.time_unix_usec)
if m.get_type() == 'VFR_HUD' and self.airstart_triggered and not self.running:
#if the airstart is triggered and we're flying, then start capture
if m.airspeed > self.camera_settings.minspeed or m.groundspeed > self.camera_settings.minspeed:
self.running = True
self.joelog = cuav_joe.JoeLog(os.path.join(os.path.dirname(self.camera_settings.imagefile), 'joe_air.log'), append=self.continue_mode)
self.capture_thread = self.start_thread(self.capture_threadfunc)
self.scan_thread = self.start_thread(self.scan_threadfunc)
self.send_message("Started cuav running")
print("Started cuav running")
if m.get_type() == "TERRAIN_REPORT":
self.terrain_alt = m.current_height
if m.get_type() == "HEARTBEAT" and m.type != mavutil.mavlink.MAV_TYPE_GCS:
was_armed = self.is_armed
self.is_armed = (m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
if not self.is_armed and was_armed:
stopfile = self.camera_settings.imagefile + ".stop"
if not os.path.exists(stopfile):
print("Creating stop file")
open(stopfile,"w").write("")
if m.get_type() in [ 'DATA16', 'DATA32', 'DATA64', 'DATA96' ]:
if self.msocket is not None:
self.msocket.incoming.append(m)
def sync_gps_clock(self, time_usec):
'''sync system clock with GPS time'''
if time_usec == 0:
# no GPS lock
return
if os.geteuid() != 0:
# can only do this as root
return
time_seconds = time_usec*1.0e-6
if self.have_set_gps_time and abs(time_seconds - time.time()) < 10:
# only change a 2nd time if time is off by 10 seconds
return
t1 = time.time()
cuav_util.set_system_clock(time_seconds)
t2 = time.time()
print("Changed system time by %.2f seconds" % (t2-t1))
self.have_set_gps_time = True
def handle_image_request(self, obj, bsend):
'''handle ImageRequest from GCS. Only sends to the requesting GCS'''
strname = str(obj.frame_time)
if not strname in self.imagefilenamemapping:
print("Unknown image %s" % strname)
return
filename = self.imagefilenamemapping[strname]
if not os.path.exists(filename):
print("No file: %s" % filename)
return
try:
img = cv2.imread(filename, -1)
except Exception:
return
if img is None:
print("Bad image: %s" % filename)
return
if not obj.fullres:
im_small = cv2.resize(img, (0,0), fx=0.5, fy=0.5)
img = im_small
print("Sending image %s" % filename)
pos = self.posmapping.get(str(obj.frame_time), None)
self.send_image(img, obj.frame_time, 10000, pos, bsend)
def camera_settings_callback(self, setting):
'''called on a changed camera setting'''
pkt = cuav_command.ChangeCameraSetting(setting.name, setting.value)
self.transmit_queue.put((pkt, None, None))
def image_settings_callback(self, setting):
'''called on a changed image setting'''
pkt = cuav_command.ChangeImageSetting(setting.name, setting.value)
self.transmit_queue.put((pkt, None, None))
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat(self.capture_count)
for bidx in range(len(self.bsend)):
self.transmit_queue.put((pkt, None, bidx))
if self.msend is not None:
self.transmit_queue.put((pkt, None, 'msend'))
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
self.transmit_queue.put((pkt, 100, None))
if self.msend is not None:
self.transmit_queue.put((pkt, 100, 'msend'))
def send_object_complete(self, obj, bsend):
'''called on complete of an send_object, cancelling send on other links'''
if obj.blockid is not None:
for bsnd in self.bsend:
if bsend != bsnd:
bsnd.cancel(obj.blockid)
if self.msend is not None:
self.msend.cancel(obj.blockid)
def send_object(self, obj, priority=None, linktosend=None):
'''send an object to all links if linktosend is none
otherwise just send to the specified link'''
try:
buf = pickle.dumps(obj, pickle.HIGHEST_PROTOCOL)
except Exception as ex:
print("dump failed: ", ex)
return
if priority is None:
priority = 10000
#only send if the queue is not clogged
if not linktosend:
for bsnd in self.bsend:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf, priority=priority, callback=functools.partial(self.send_object_complete, obj, bsnd))
else:
if linktosend == self.msend:
qsize = self.camera_settings.m_maxqueue
else:
qsize = self.camera_settings.maxqueue
if linktosend.sendq_size() < qsize:
obj.blockid = linktosend.send(buf, priority=priority, callback=functools.partial(self.send_object_complete, obj, linktosend))
def handle_command_packet(self, obj, bsend):
'''handle CommandPacket from other end'''
stdout_saved = sys.stdout
buf = StringIO()
sys.stdout = buf
self.mpstate.functions.process_stdin(obj.command, immediate=True)
sys.stdout = stdout_saved
if str(buf.getvalue().strip()):
pkt = cuav_command.CommandResponse(str(buf.getvalue()).strip())
self.transmit_queue.put((pkt, None, self.get_bsend_index(bsend)))
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(file_list(a, ["jpg", "pgm", "png"]))
else:
if a.find("*") != -1:
files.extend(glob.glob(a))
else:
files.append(a)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
slipmap = mp_slipmap.MPSlipMap(service=opts.service, elevation=True, title="Map")
icon = slipmap.icon("redplane.png")
slipmap.add_object(
mp_slipmap.SlipIcon("plane", (0, 0), icon, layer=3, rotation=0, follow=True, trail=mp_slipmap.SlipTrail())
)
for flag in opts.flag:
a = flag.split(",")
lat = a[0]
lon = a[1]
icon = "flag.png"
if len(a) > 2:
icon = a[2] + ".png"
icon = slipmap.icon(icon)
slipmap.add_object(
mp_slipmap.SlipIcon(
"icon - %s" % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False
)
)
if opts.mission:
from pymavlink import mavwp
wp = mavwp.MAVWPLoader()
wp.load(opts.mission)
plist = wp.polygon_list()
if len(plist) > 0:
for i in range(len(plist)):
slipmap.add_object(
mp_slipmap.SlipPolygon(
"Mission-%s-%u" % (opts.mission, i),
plist[i],
layer="Mission",
linewidth=2,
colour=(255, 255, 255),
)
)
if opts.mavlog:
mpos = mav_position.MavInterpolator()
mpos.set_logfile(opts.mavlog)
else:
mpos = None
if opts.gammalog is not None:
gamma = parse_gamma_log(opts.gammalog)
else:
gamma = None
if opts.kmzlog:
kmzpos = mav_position.KmlPosition(opts.kmzlog)
else:
kmzpos = None
if opts.triggerlog:
triggerpos = mav_position.TriggerPosition(opts.triggerlog)
else:
triggerpos = None
# create a simple lens model using the focal length
C_params = cam_params.CameraParams(lens=opts.lens, sensorwidth=opts.sensorwidth)
if opts.camera_params:
C_params.load(opts.camera_params)
if opts.target:
target = opts.target.split(",")
else:
target = [0, 0, 0]
camera_settings = MPSettings(
[
MPSetting("roll_stabilised", bool, opts.roll_stabilised, "Roll Stabilised"),
MPSetting("altitude", int, opts.altitude, "Altitude", range=(0, 10000), increment=1),
MPSetting("minalt", int, 30, "MinAltitude", range=(0, 10000), increment=1),
MPSetting("mpp100", float, 0.0977, "MPPat100m", range=(0, 10000), increment=0.001),
MPSetting("rotate180", bool, opts.rotate_180, "rotate180"),
MPSetting("filter_type", str, "compactness", "Filter Type", choice=["simple", "compactness"]),
MPSetting("target_lattitude", float, float(target[0]), "target latitude", increment=1.0e-7),
MPSetting("target_longitude", float, float(target[1]), "target longitude", increment=1.0e-7),
MPSetting("target_radius", float, float(target[2]), "target radius", increment=1),
MPSetting("quality", int, 75, "Compression Quality", range=(1, 100), increment=1),
MPSetting("thumbsize", int, opts.thumbsize, "Thumbnail Size", range=(10, 200), increment=1),
MPSetting("minscore", int, opts.minscore, "Min Score", range=(0, 1000), increment=1, tab="Scoring"),
MPSetting(
"brightness", float, 1.0, "Display Brightness", range=(0.1, 10), increment=0.1, digits=2, tab="Display"
),
],
title="Camera Settings",
)
image_settings = MPSettings(
[
MPSetting("MinRegionArea", float, 0.05, range=(0, 100), increment=0.05, digits=2, tab="Image Processing"),
MPSetting("MaxRegionArea", float, 4.0, range=(0, 100), increment=0.1, digits=1),
MPSetting("MinRegionSize", float, 0.02, range=(0, 100), increment=0.05, digits=2),
MPSetting("MaxRegionSize", float, 3.0, range=(0, 100), increment=0.1, digits=1),
MPSetting("MaxRarityPct", float, 0.02, range=(0, 100), increment=0.01, digits=2),
MPSetting("RegionMergeSize", float, 1.0, range=(0, 100), increment=0.1, digits=1),
MPSetting("BlueEmphasis", bool, opts.blue_emphasis),
MPSetting("SaveIntermediate", bool, opts.debug),
],
title="Image Settings",
)
mosaic = cuav_mosaic.Mosaic(
slipmap,
C=C_params,
camera_settings=camera_settings,
image_settings=image_settings,
start_menu=True,
classify=opts.categories,
thumb_size=opts.mosaic_thumbsize,
)
joelog = cuav_joe.JoeLog(None)
if opts.view:
viewer = mp_image.MPImage(title="Image", can_zoom=True, can_drag=True)
for f in files:
if not mosaic.started():
print("Waiting for startup")
while not mosaic.started():
mosaic.check_events()
time.sleep(0.01)
if mpos:
# get the position by interpolating telemetry data from the MAVLink log file
# this assumes that the filename contains the timestamp
if gamma is not None:
frame_time = parse_gamma_time(f, gamma)
else:
frame_time = cuav_util.parse_frame_time(f)
frame_time += opts.time_offset
if camera_settings.roll_stabilised:
roll = 0
else:
roll = None
try:
pos = mpos.position(frame_time, roll=roll)
except Exception:
print("No position available for %s" % frame_time)
# skip this frame
continue
elif kmzpos is not None:
pos = kmzpos.position(f)
elif triggerpos is not None:
pos = triggerpos.position(f)
else:
# get the position using EXIF data
pos = mav_position.exif_position(f)
pos.time += opts.time_offset
# update the plane icon on the map
if pos is not None:
slipmap.set_position("plane", (pos.lat, pos.lon), rotation=pos.yaw)
if camera_settings.altitude > 0:
pos.altitude = camera_settings.altitude
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180)
if im_orig is None:
continue
(w, h) = cuav_util.image_shape(im_orig)
if not opts.camera_params:
C_params.set_resolution(w, h)
im_full = im_orig
im_640 = cv.CreateImage((640, 480), 8, 3)
cv.Resize(im_full, im_640, cv.CV_INTER_NN)
im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
im_full = numpy.ascontiguousarray(cv.GetMat(im_full))
count = 0
total_time = 0
t0 = time.time()
img_scan = im_full
scan_parms = {}
for name in image_settings.list():
scan_parms[name] = image_settings.get(name)
scan_parms["SaveIntermediate"] = float(scan_parms["SaveIntermediate"])
scan_parms["BlueEmphasis"] = float(scan_parms["BlueEmphasis"])
if pos is not None:
(sw, sh) = cuav_util.image_shape(img_scan)
altitude = pos.altitude
if altitude < camera_settings.minalt:
altitude = camera_settings.minalt
scan_parms["MetersPerPixel"] = camera_settings.mpp100 * altitude / 100.0
regions = scanner.scan(img_scan, scan_parms)
else:
regions = scanner.scan(img_scan)
regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full))
count += 1
t1 = time.time()
frame_time = pos.time
if pos:
for r in regions:
r.latlon = cuav_util.gps_position_from_image_region(r, pos, w, h, altitude=altitude)
if camera_settings.target_radius > 0 and pos is not None:
regions = cuav_region.filter_radius(
regions,
(camera_settings.target_lattitude, camera_settings.target_longitude),
camera_settings.target_radius,
)
regions = cuav_region.filter_regions(
im_full,
regions,
frame_time=frame_time,
min_score=camera_settings.minscore,
filter_type=camera_settings.filter_type,
)
scan_count += 1
if pos and len(regions) > 0:
altitude = camera_settings.altitude
if altitude <= 0:
altitude = None
joelog.add_regions(frame_time, regions, pos, f, width=w, height=h, altitude=altitude)
mosaic.add_image(pos.time, f, pos)
region_count += len(regions)
if len(regions) > 0:
composite = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)), regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if opts.view:
img_view = img_scan
(wview, hview) = cuav_util.image_shape(img_view)
mat = cv.fromarray(img_view)
for r in regions:
r.draw_rectangle(mat, (255, 0, 0))
cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
viewer.set_image(mat)
viewer.set_title("Image: " + os.path.basename(f))
if opts.saveview:
cv.CvtColor(mat, mat, cv.CV_RGB2BGR)
cv.SaveImage("view-" + os.path.basename(f), mat)
total_time += t1 - t0
if t1 != t0:
print(
"%s scan %.1f fps %u regions [%u/%u]"
% (os.path.basename(f), count / total_time, region_count, scan_count, num_files)
)
# raw_input("hit ENTER when ready")
print("All images processed")
while True:
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
time.sleep(0.2)
def process(args):
'''process a set of files'''
global slipmap, mosaic
scan_count = 0
files = []
if os.path.isdir(args.directory):
files.extend(file_list(args.directory, ['jpg', 'pgm', 'png']))
else:
if args.directory.find('*') != -1:
files.extend(glob.glob(args.directory))
else:
files.append(args.directory)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
slipmap = mp_slipmap.MPSlipMap(service=args.service,
elevation=True,
title='Map')
icon = slipmap.icon('redplane.png')
slipmap.add_object(
mp_slipmap.SlipIcon('plane', (0, 0),
icon,
layer=3,
rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
for flag in args.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = slipmap.icon(icon)
slipmap.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
if args.mission:
from pymavlink import mavwp
wp = mavwp.MAVWPLoader()
wp.load(args.mission.name)
plist = wp.polygon_list()
if len(plist) > 0:
for i in range(len(plist)):
slipmap.add_object(
mp_slipmap.SlipPolygon('Mission-%s-%u' %
(args.mission.name, i),
plist[i],
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
if args.mavlog:
mpos = mav_position.MavInterpolator()
mpos.set_logfile(args.mavlog.name)
else:
mpos = None
if args.gammalog is not None:
gamma = parse_gamma_log(args.gammalog)
else:
gamma = None
if args.kmzlog:
kmzpos = mav_position.KmlPosition(args.kmzlog.name)
else:
kmzpos = None
if args.triggerlog:
triggerpos = mav_position.TriggerPosition(args.triggerlog.name)
else:
triggerpos = None
# create a simple lens model using the focal length
if args.camera_params:
C_params = cam_params.CameraParams.fromfile(args.camera_params.name)
else:
C_params = cam_params.CameraParams(lens=args.lens,
sensorwidth=args.sensorwidth,
xresolution=args.xresolution,
yresolution=args.yresolution)
if args.target:
target = args.target.split(',')
else:
target = [0, 0, 0]
camera_settings = MPSettings([
MPSetting('roll_stabilised', bool, args.roll_stabilised,
'Roll Stabilised'),
MPSetting('altitude',
int,
args.altitude,
'Altitude',
range=(0, 10000),
increment=1),
MPSetting(
'minalt', int, 30, 'MinAltitude', range=(0, 10000), increment=1),
MPSetting('mpp100',
float,
0.0977,
'MPPat100m',
range=(0, 10000),
increment=0.001),
MPSetting('rotate180', bool, args.rotate_180, 'rotate180'),
MPSetting('filter_type',
str,
'compactness',
'Filter Type',
choice=['simple', 'compactness']),
MPSetting('target_lattitude',
float,
float(target[0]),
'target latitude',
increment=1.0e-7),
MPSetting('target_longitude',
float,
float(target[1]),
'target longitude',
increment=1.0e-7),
MPSetting('target_radius',
float,
float(target[2]),
'target radius',
increment=1),
MPSetting('quality',
int,
75,
'Compression Quality',
range=(1, 100),
increment=1),
MPSetting('thumbsize',
int,
args.thumbsize,
'Thumbnail Size',
range=(10, 200),
increment=1),
MPSetting('minscore',
int,
args.minscore,
'Min Score',
range=(0, 1000),
increment=1,
tab='Scoring'),
MPSetting('brightness',
float,
1.0,
'Display Brightness',
range=(0.1, 10),
increment=0.1,
digits=2,
tab='Display'),
],
title='Camera Settings')
image_settings = MPSettings([
MPSetting('MinRegionArea',
float,
0.05,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionArea',
float,
4.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('MinRegionSize',
float,
0.02,
range=(0, 100),
increment=0.05,
digits=2),
MPSetting('MaxRegionSize',
float,
3.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('MaxRarityPct',
float,
0.02,
range=(0, 100),
increment=0.01,
digits=2),
MPSetting('RegionMergeSize',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('BlueEmphasis', bool, args.blue_emphasis),
MPSetting('SaveIntermediate', bool, args.debug)
],
title='Image Settings')
mosaic = cuav_mosaic.Mosaic(slipmap,
C=C_params,
camera_settings=camera_settings,
image_settings=image_settings,
start_menu=True,
classify=args.categories,
thumb_size=args.mosaic_thumbsize)
joelog = cuav_joe.JoeLog(None)
if args.view:
viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)
start_time = time.time()
for f in files:
if not mosaic.started():
print("Waiting for startup")
if args.start:
mosaic.has_started = True
while not mosaic.started():
mosaic.check_events()
time.sleep(0.01)
if mpos:
# get the position by interpolating telemetry data from the MAVLink log file
# this assumes that the filename contains the timestamp
if gamma is not None:
frame_time = parse_gamma_time(f, gamma)
else:
frame_time = cuav_util.parse_frame_time(f)
frame_time += args.time_offset
if camera_settings.roll_stabilised:
roll = 0
else:
roll = None
try:
pos = mpos.position(frame_time, roll=roll)
except Exception:
print("No position available for %s" % frame_time)
# skip this frame
continue
elif kmzpos is not None:
pos = kmzpos.position(f)
elif triggerpos is not None:
pos = triggerpos.position(f)
else:
# get the position using EXIF data
pos = mav_position.exif_position(f)
pos.time += args.time_offset
# update the plane icon on the map
if pos is not None:
slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
if camera_settings.altitude > 0:
pos.altitude = camera_settings.altitude
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180)
if im_orig is None:
continue
(w, h) = cuav_util.image_shape(im_orig)
if False:
im_640 = cv.CreateImage((640, 480), 8, 3)
cv.Resize(im_full, im_640, cv.CV_INTER_NN)
im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
im_full = im_orig
im_full = numpy.ascontiguousarray(cv.GetMat(im_full))
count = 0
total_time = 0
t0 = time.time()
img_scan = im_full
scan_parms = {}
for name in image_settings.list():
scan_parms[name] = image_settings.get(name)
scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])
scan_parms['BlueEmphasis'] = float(scan_parms['BlueEmphasis'])
if pos is not None:
(sw, sh) = cuav_util.image_shape(img_scan)
altitude = pos.altitude
if altitude < camera_settings.minalt:
altitude = camera_settings.minalt
scan_parms[
'MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0
regions = scanner.scan(img_scan, scan_parms)
else:
regions = scanner.scan(img_scan)
regions = cuav_region.RegionsConvert(regions,
cuav_util.image_shape(img_scan),
cuav_util.image_shape(im_full))
count += 1
t1 = time.time()
frame_time = pos.time
if pos:
for r in regions:
r.latlon = cuav_util.gps_position_from_image_region(
r, pos, w, h, altitude=altitude, C=C_params)
if camera_settings.target_radius > 0 and pos is not None:
regions = cuav_region.filter_radius(
regions, (camera_settings.target_lattitude,
camera_settings.target_longitude),
camera_settings.target_radius)
regions = cuav_region.filter_regions(
im_full,
regions,
frame_time=frame_time,
min_score=camera_settings.minscore,
filter_type=camera_settings.filter_type)
scan_count += 1
if pos and len(regions) > 0:
altitude = camera_settings.altitude
if altitude <= 0:
altitude = None
joelog.add_regions(frame_time,
regions,
pos,
f,
width=w,
height=h,
altitude=altitude)
mosaic.add_image(pos.time, f, pos)
region_count += len(regions)
if len(regions) > 0:
composite = cuav_mosaic.CompositeThumbnail(
cv.GetImage(cv.fromarray(im_full)), regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if args.view:
img_view = img_scan
(wview, hview) = cuav_util.image_shape(img_view)
mat = cv.fromarray(img_view)
for r in regions:
r.draw_rectangle(mat, (255, 0, 0))
cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
viewer.set_image(mat)
viewer.set_title('Image: ' + os.path.basename(f))
if args.saveview:
cv.CvtColor(mat, mat, cv.CV_RGB2BGR)
cv.SaveImage('view-' + os.path.basename(f), mat)
total_time += (t1 - t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' %
(os.path.basename(f), count / total_time, region_count,
scan_count, num_files))
#raw_input("hit ENTER when ready")
print("All images processed (%u seconds)" % (time.time() - start_time))
while True:
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
time.sleep(0.2)
class CameraAirModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraAirModule, self).__init__(mpstate,
"camera_air",
"cuav camera control (air)",
public=True)
self.running = False
self.unload_event = threading.Event()
self.unload_event.clear()
self.capture_thread = None
self.scan_thread = None
self.transmit_thread = None
self.airstart_triggered = False
self.terrain_alt = None
self.handled_timestamps = {}
self.imagefilenamemapping = {}
self.posmapping = {}
self.is_armed = True
self.lz = cuav_landingregion.LandingZone()
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.camera_settings = MPSettings([
MPSetting('roll_stabilised', bool, False, 'Roll Stabilised'),
MPSetting('roll_limit', float, 0, 'Roll stabilisation limit'),
MPSetting('minspeed', int, 20,
'For airstart, minimum speed for capture to start'),
MPSetting('minalt',
int,
30,
'MinAltitude of images',
range=(0, 10000),
increment=1),
MPSetting('rotate180',
bool,
False,
'rotate images by 180',
tab='Capture2'),
MPSetting('ignoretimestamps',
bool,
False,
'Ignore image timestamps',
tab='Capture2'),
MPSetting('camparms',
str,
None,
'camera parameters file (json) in cuav package',
tab='Imaging'),
MPSetting(
'imagefile', str, None, 'latest captured image',
tab='Imaging'),
MPSetting('filter_type',
str,
'simple',
'Filter Type',
choice=['simple'],
tab='Imaging'),
MPSetting(
'blue_emphasis', bool, False, 'BlueEmphasis', tab='Imaging'),
MPSetting('use_capture_time',
bool,
True,
'Use Capture Time (false for sim)',
tab='Simulation'),
MPSetting('target_latitude',
float,
0,
'filter detected images to latitude',
tab='Filter to Location'),
MPSetting('target_longitude',
float,
0,
'filter detected images to longitude',
tab='Filter to Location'),
MPSetting('target_radius',
float,
0,
'filter detected images to radius',
tab='Filter to Location'),
MPSetting(
'gcs_address',
str,
"",
'GCS Addresses in RemIP:RemPort:LocalPort:Bandwidth format (127.0.0.1:1440:1234:45, ...)',
tab='GCS'),
MPSetting('qualitysend',
int,
90,
'Compression Quality for send',
range=(1, 100),
increment=1,
tab='GCS'),
MPSetting('transmit',
bool,
True,
'Transmit Enable for thumbnails',
tab='GCS'),
MPSetting('maxqueue', int, 50, 'Maximum images queue', tab='GCS'),
MPSetting('thumbsize',
int,
60,
'Thumbnail Size',
range=(10, 200),
increment=1),
MPSetting('minscore',
int,
1000,
'Min Score to pass detection',
range=(0, 100000),
increment=1,
tab='Imaging'),
MPSetting('clock_sync', bool, False, 'GPS Clock Sync'),
MPSetting('m_minscore',
int,
20000,
'Min Score to pass detection on mavlink',
range=(0, 100000),
increment=1,
tab='Imaging'),
MPSetting('m_bandwidth',
int,
500,
'max bandwidth on mavlink',
increment=1,
tab='GCS'),
MPSetting('m_maxqueue',
int,
5,
'Maximum images queue for mavlink',
tab='GCS'),
MPSetting('preview',
bool,
True,
'enable camera preview window',
tab='Imaging'),
MPSetting('previewquality',
int,
40,
'Compression Quality for preview',
range=(1, 100),
increment=1,
tab='Imaging'),
MPSetting('previewscale',
int,
5,
'preview downscaling',
range=(1, 10),
increment=1,
tab='Imaging'),
MPSetting('previewfreq',
int,
4,
'preview image frequency',
range=(1, 10),
increment=1,
tab='Imaging'),
],
title='Camera Settings')
self.image_settings = MPSettings([
MPSetting('MinRegionArea',
float,
0.15,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionArea',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1,
tab='Image Processing'),
MPSetting('MinRegionSize',
float,
0.2,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionSize',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1,
tab='Image Processing'),
MPSetting('MaxRarityPct',
float,
0.02,
range=(0, 100),
increment=0.01,
digits=2,
tab='Image Processing'),
MPSetting('RegionMergeSize',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1,
tab='Image Processing'),
],
title='Image Settings')
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.scan_fps = 0
self.scan_queue = multiproc.Queue()
self.transmit_queue = multiproc.Queue()
self.have_set_gps_time = False
self.c_params = None
self.jpeg_size = 0
self.xmit_queue = []
self.efficiency = []
self.last_watch = 0
self.boundary = None
self.boundary_polygon = None
self.bandwidth_used = []
self.rtt_estimate = []
self.bsend = [] #note this is an array of bsends
# msend is a BlockSender over MAVLink
self.msocket = None
self.msend = None
self.last_heartbeat = time.time()
self.mpos = mav_position.MavInterpolator(backlog=500, gps_lag=0.0)
self.joelog = None #cuav_joe.JoeLog(os.path.join(self.settings.imagefile, '..', 'joe.log'), append=self.continue_mode)
self.add_command(
'camera', self.cmd_camera, 'camera control',
['<start|stop|status|boundary|airstart>', 'set (CAMERASETTING)'])
self.add_completion_function('(CAMERASETTING)',
self.camera_settings.completion)
# prevent loopback of messages
for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
self.module('link').no_fwd_types.add(mtype)
print("camera initialised")
def get_bsend(self, bsnd):
'''get a bsend object, given a tag name'''
if bsnd is None:
return None
if bsnd == 'msend':
return self.msend
return self.bsend[bsnd]
def get_bsend_index(self, bsnd):
'''get a bsend index from a bsend object. This avoids pickling a block xmit object'''
if bsnd is None or isinstance(bsnd, int) or bsnd == 'msend':
return bsnd
if bsnd == self.msend:
return 'msend'
return self.bsend.index(bsnd)
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <start|airstart|stop|status|queue|set>"
if len(args) == 0:
print(usage)
return
if args[0] == "start":
self.capture_count = 0
self.error_count = 0
self.error_msg = None
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params " +
str(self.camera_settings.camparms))
return
if self.running == False:
self.running = True
self.joelog = cuav_joe.JoeLog(os.path.join(
os.path.dirname(self.camera_settings.imagefile),
'joe_air.log'),
append=self.continue_mode)
self.capture_thread = self.start_thread(
self.capture_threadfunc)
self.scan_thread = self.start_thread(self.scan_threadfunc)
self.transmit_thread = self.start_thread(
self.transmit_threadfunc)
time.sleep(0.1)
self.send_message("Started cuav running")
print("Started cuav running")
else:
self.send_message("cuav already running")
print("cuav already running")
elif args[0] == "stop":
self.running = False
self.airstart_triggered = False
print("Stopped cuav")
self.send_message("Stopped cuav")
elif args[0] == "status":
ret = "Cap imgs:%u err:%u scan:%u regions:%u jsize:%.0f xmitq:%s sq:%u eff:%s" % (
self.capture_count, self.error_count, self.scan_count,
self.region_count, self.jpeg_size, self.xmit_queue,
self.scan_queue.qsize(), self.efficiency)
print(ret)
self.send_message(ret)
elif args[0] == "queue":
ret = "scan %u transmit %u eff %s bw %s rtt %s" % (
self.scan_queue.qsize(), self.transmit_queue.qsize(),
self.efficiency, self.bandwidth_used, self.rtt_estimate)
print(ret)
elif args[0] == "set":
self.camera_settings.command(args[1:])
elif args[0] == "airstart":
#just keep the block xmit going for now
self.capture_count = 0
self.error_count = 0
self.error_msg = None
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params " +
str(self.camera_settings.camparms))
return
if self.airstart_triggered == False:
self.airstart_triggered = True
self.joelog = cuav_joe.JoeLog(os.path.join(
os.path.dirname(self.camera_settings.imagefile),
'joe_air.log'),
append=self.continue_mode)
self.transmit_thread = self.start_thread(
self.transmit_threadfunc)
time.sleep(0.1)
self.send_message("cuav airstart ready")
print("cuav airstart ready")
else:
self.send_message("cuav airstart already running")
print("cuav airstart already running")
else:
print(usage)
def check_camera_parms(self):
'''check for change in camera parameters'''
#dir is rel to this python file:
if self.camera_settings.camparms is None:
return False
camfiletxt = pkg_resources.resource_string(
"cuav", self.camera_settings.camparms)
if sys.version_info.major >= 3:
camfiletxt = camfiletxt.decode('utf-8')
try:
self.c_params = CameraParams.fromstring(camfiletxt)
return True
except:
return False
def capture_threadfunc(self):
'''image capture thread, via monitoring the
link for changed linked filenames'''
prev_image = None
self.scan_queue = multiproc.Queue()
while not self.unload_event.wait(0.05):
try:
filename = os.path.realpath(self.camera_settings.imagefile)
if not self.camera_settings.ignoretimestamps:
filetime = cuav_util.parse_frame_time(filename)
else:
filetime = float(time.time())
except Exception:
filename = None
pass
#ensure all items are valid and the queue isn't overfilled > 100
if filename != None and prev_image != filename and filetime != None and self.scan_queue.qsize(
) < 100:
self.scan_queue.put((filetime, filename))
self.imagefilenamemapping[str(filetime)] = filename
self.capture_count += 1
prev_image = filename
if self.is_armed:
stopfile = self.camera_settings.imagefile + ".stop"
if os.path.exists(stopfile):
print("Removing stopfile")
os.unlink(self.camera_settings.imagefile + ".stop")
def scan_threadfunc(self):
'''image scanning thread'''
while not self.unload_event.wait(0.05):
if self.scan_queue.empty():
continue
(frame_time, im) = self.scan_queue.get()
scan_parms = {}
for name in self.image_settings.list():
scan_parms[name] = self.image_settings.get(name)
scan_parms['BlueEmphasis'] = float(
self.camera_settings.blue_emphasis)
if self.terrain_alt is not None:
altitude = self.terrain_alt
if altitude < self.camera_settings.minalt:
altitude = self.camera_settings.minalt
scan_parms['MetersPerPixel'] = cuav_util.pixel_width(
altitude, self.c_params.xresolution, self.c_params.lens,
self.c_params.sensorwidth)
t1 = time.time()
try:
img_scan = cv2.imread(im, -1)
except Exception:
continue
if img_scan is None:
continue
(h, w) = img_scan.shape[:2]
if self.camera_settings.rotate180:
M = cv2.getRotationMatrix2D(center, angle180, scale)
img_scan = cv2.warpAffine(img_scan, M, (w, h))
im_numpy = numpy.ascontiguousarray(img_scan)
regions = scanner.scan(im_numpy, scan_parms)
regions = cuav_region.RegionsConvert(
regions, cuav_util.image_shape(img_scan),
cuav_util.image_shape(img_scan))
t2 = time.time()
self.scan_fps = 1.0 / (t2 - t1)
self.scan_count += 1
regions = cuav_region.filter_regions(
img_scan,
regions,
min_score=self.camera_settings.minscore,
filter_type=self.camera_settings.filter_type)
self.region_count += len(regions)
# possibly send a preview image
self.send_preview(img_scan)
if self.camera_settings.roll_stabilised:
roll = 0
else:
roll = None
pos = self.get_plane_position(frame_time, roll=roll)
if pos is not None:
self.posmapping[str(frame_time)] = pos
# this adds the latlon field to the regions (georeferencing)
for r in regions:
r.latlon = cuav_util.gps_position_from_image_region(
r, pos, w, h, altitude=None, C=self.c_params)
if self.joelog:
self.log_joe_position(pos, frame_time, regions)
# filter out any regions outside the target radius
if self.camera_settings.target_radius > 0 and pos is not None:
regions = cuav_region.filter_radius(
regions, (self.camera_settings.target_latitude,
self.camera_settings.target_longitude),
self.camera_settings.target_radius)
# filter out any regions outside the boundary
if self.boundary_polygon:
regions = cuav_region.filter_boundary(regions,
self.boundary_polygon,
pos)
#filter by minscore
regions = cuav_region.filter_regions(
img_scan,
regions,
min_score=self.camera_settings.minscore,
filter_type=self.camera_settings.filter_type)
high_score = 1
for r in regions:
if r.score > high_score:
high_score = r.score
if len(regions) > 0 and pos is not None:
for r in regions:
self.lz.checkaddregion(r, pos)
try:
lzresult = self.lz.calclandingzone()
except Exception as ex:
print("calclandingzone failed: ", ex)
continue
if lzresult:
self.transmit_queue.put((lzresult, 100000, None))
if self.msend:
self.transmit_queue.put((lzresult, 100000, 'msend'))
if len(regions) > 0 and self.camera_settings.transmit:
# send a region message with thumbnails to the ground station
thumb_img = cuav_region.CompositeThumbnail(
img_scan,
regions,
thumb_size=self.camera_settings.thumbsize)
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90]
(result, thumb) = cv2.imencode('.jpg', thumb_img, encode_param)
pkt = cuav_command.ThumbPacket(frame_time, regions, thumb, pos)
if self.transmit_queue.qsize() < 100:
self.transmit_queue.put((pkt, None, None))
if self.msend is not None and high_score >= self.camera_settings.m_minscore:
self.transmit_queue.put((pkt, None, 'msend'))
else:
self.send_message("Warning: image Tx queue too long")
print("Warning: image Tx queue too long")
def get_plane_position(self, frame_time, roll=None):
'''get a MavPosition object for the planes position if possible'''
try:
pos = self.mpos.position(frame_time,
0,
roll=roll,
maxroll=self.camera_settings.roll_limit)
return pos
except mav_position.MavInterpolatorException as e:
print(str(e))
return None
def log_joe_position(self,
pos,
frame_time,
regions,
filename=None,
thumb_filename=None):
'''add to joe.log if possible, returning a list of (lat,lon) tuples
for the positions of the identified image regions'''
return self.joelog.add_regions(frame_time, regions, pos, filename,
thumb_filename)
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def transmit_threadfunc(self):
'''thread for image and message transmit to camera_ground
in addition to reading commands from the camera_ground'''
self.start_aircraft_bsend()
self.spacewarning = False
while (not self.unload_event.wait(0.05)) or self.airstart_triggered:
for bsnd in self.bsend:
bsnd.tick(packet_count=1000,
max_queue=self.camera_settings.maxqueue)
try:
self.check_commands(bsnd)
except Exception as ex:
print("Failed command", ex)
if self.msend is not None:
self.msend.tick(packet_count=1000,
max_queue=self.camera_settings.m_maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
#check remaining disk space and warn user if required
try:
stat = os.statvfs(
os.path.dirname(self.camera_settings.imagefile))
if not self.spacewarning and stat.f_bfree * stat.f_bsize < 20971520:
self.send_message(
"Warning: <200Mb disk space left on cuav_air")
self.spacewarning = True
except OSError:
pass
while not self.transmit_queue.empty():
(pkt, priority, linktosend) = self.transmit_queue.get()
linktosend = self.get_bsend(linktosend)
self.send_object(pkt, priority, linktosend)
#update the stats
self.xmit_queue = []
self.efficiency = []
self.bandwidth_used = []
self.rtt_estimate = []
for bsnd in self.bsend:
self.xmit_queue.append(bsnd.sendq_size())
self.efficiency.append(bsnd.get_efficiency())
self.bandwidth_used.append(bsnd.get_bandwidth_used())
self.rtt_estimate.append(bsnd.get_rtt_estimate())
if self.msend is not None:
self.xmit_queue.append(self.msend.sendq_size())
self.efficiency.append(self.msend.get_efficiency())
self.bandwidth_used.append(self.msend.get_bandwidth_used())
self.rtt_estimate.append(self.msend.get_rtt_estimate())
def send_image(self, img, frame_time, priority, pos, linktosend):
'''send an image object to the GCS'''
encode_param = [
int(cv2.IMWRITE_JPEG_QUALITY), self.camera_settings.qualitysend
]
(result, jpeg) = cv2.imencode('.jpg', img, encode_param)
# keep filtered image size
self.jpeg_size = 0.95 * self.jpeg_size + 0.05 * len(jpeg)
pkt = cuav_command.ImagePacket(frame_time, jpeg, pos, priority)
self.transmit_queue.put(
(pkt, priority, self.get_bsend_index(linktosend)))
def send_preview(self, img):
'''send a preview image object to the GCS'''
if not self.camera_settings.preview or self.transmit_queue.qsize() > 3:
# only send when link is nearly idle
return
if self.scan_count % self.camera_settings.previewfreq != 0:
return
scale = 1.0 / self.camera_settings.previewscale
small_img = cv2.resize(img, (0, 0), fx=scale, fy=scale)
encode_param = [
int(cv2.IMWRITE_JPEG_QUALITY), self.camera_settings.previewquality
]
(result, jpeg) = cv2.imencode('.jpg', small_img, encode_param)
pkt = cuav_command.PreviewPacket(jpeg)
self.transmit_queue.put((pkt, 1, None))
def send_file(self, filename):
'''send a file over all links'''
try:
contents = open(filename).read()
filename = os.path.basename(filename)
except Exception:
return
pkt = cuav_command.FilePacket(filename, contents)
# send over all links
self.transmit_queue.put((pkt, 20000, None))
if self.msend:
self.transmit_queue.put((pkt, 20000, 'msend'))
def start_aircraft_bsend(self):
'''start bsend for aircraft side'''
if len(self.bsend) == 0:
for lnk in self.camera_settings.gcs_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 GCS endpoint (must be remIP:remport:localport:bw): "
+ str(lnk))
pass
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 start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.running = False
self.unload_event.set()
if self.capture_thread is not None:
self.capture_thread.join(1.0)
self.scan_thread.join(1.0)
self.transmit_thread.join(1.0)
print('camera unload OK')
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 == 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.ImageRequest):
self.handle_image_request(obj, bsend)
if isinstance(obj, cuav_command.ChangeCameraSetting):
self.camera_settings.set(obj.name, obj.value)
self.camera_settings_callback(obj)
if isinstance(obj, cuav_command.ChangeImageSetting):
self.image_settings.set(obj.name, obj.value)
self.image_settings_callback(obj)
if isinstance(obj, cuav_command.CommandPacket):
self.handle_command_packet(obj, bsend)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if self.mpstate.status.watch in [
"camera", "queue"
] and time.time() > self.last_watch + 1:
self.last_watch = time.time()
self.cmd_camera([
"status" if self.mpstate.status.watch == "camera" else "queue"
])
# update position interpolator
self.mpos.add_msg(m)
if m.get_type(
) == 'SYSTEM_TIME' and self.camera_settings.clock_sync and self.capture_thread is not None:
# optionally sync system clock on the capture side
self.sync_gps_clock(m.time_unix_usec)
if m.get_type(
) == 'VFR_HUD' and self.airstart_triggered and not self.running:
#if the airstart is triggered and we're flying, then start capture
if m.airspeed > self.camera_settings.minspeed or m.groundspeed > self.camera_settings.minspeed:
self.running = True
self.joelog = cuav_joe.JoeLog(os.path.join(
os.path.dirname(self.camera_settings.imagefile),
'joe_air.log'),
append=self.continue_mode)
self.capture_thread = self.start_thread(
self.capture_threadfunc)
self.scan_thread = self.start_thread(self.scan_threadfunc)
self.send_message("Started cuav running")
print("Started cuav running")
if m.get_type() == "TERRAIN_REPORT":
self.terrain_alt = m.current_height
if m.get_type(
) == "HEARTBEAT" and m.type != mavutil.mavlink.MAV_TYPE_GCS:
was_armed = self.is_armed
self.is_armed = (m.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
if not self.is_armed and was_armed:
stopfile = self.camera_settings.imagefile + ".stop"
if not os.path.exists(stopfile):
print("Creating stop file")
open(stopfile, "w").write("")
if m.get_type() in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
if self.msocket is not None:
self.msocket.incoming.append(m)
def sync_gps_clock(self, time_usec):
'''sync system clock with GPS time'''
if time_usec == 0:
# no GPS lock
return
if os.geteuid() != 0:
# can only do this as root
return
time_seconds = time_usec * 1.0e-6
if self.have_set_gps_time and abs(time_seconds - time.time()) < 10:
# only change a 2nd time if time is off by 10 seconds
return
t1 = time.time()
cuav_util.set_system_clock(time_seconds)
t2 = time.time()
print("Changed system time by %.2f seconds" % (t2 - t1))
self.have_set_gps_time = True
def handle_image_request(self, obj, bsend):
'''handle ImageRequest from GCS. Only sends to the requesting GCS'''
strname = str(obj.frame_time)
if not strname in self.imagefilenamemapping:
print("Unknown image %s" % strname)
return
filename = self.imagefilenamemapping[strname]
if not os.path.exists(filename):
print("No file: %s" % filename)
return
try:
img = cv2.imread(filename, -1)
except Exception:
return
if img is None:
print("Bad image: %s" % filename)
return
if not obj.fullres:
im_small = cv2.resize(img, (0, 0), fx=0.5, fy=0.5)
img = im_small
print("Sending image %s" % filename)
pos = self.posmapping.get(str(obj.frame_time), None)
self.send_image(img, obj.frame_time, 10000, pos, bsend)
def camera_settings_callback(self, setting):
'''called on a changed camera setting'''
pkt = cuav_command.ChangeCameraSetting(setting.name, setting.value)
self.transmit_queue.put((pkt, None, None))
def image_settings_callback(self, setting):
'''called on a changed image setting'''
pkt = cuav_command.ChangeImageSetting(setting.name, setting.value)
self.transmit_queue.put((pkt, None, None))
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat(self.capture_count)
for bidx in range(len(self.bsend)):
self.transmit_queue.put((pkt, None, bidx))
if self.msend is not None:
self.transmit_queue.put((pkt, None, 'msend'))
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
self.transmit_queue.put((pkt, 100, None))
if self.msend is not None:
self.transmit_queue.put((pkt, 100, 'msend'))
def send_object_complete(self, obj, bsend):
'''called on complete of an send_object, cancelling send on other links'''
if obj.blockid is not None:
for bsnd in self.bsend:
if bsend != bsnd:
bsnd.cancel(obj.blockid)
if self.msend is not None:
self.msend.cancel(obj.blockid)
def send_object(self, obj, priority=None, linktosend=None):
'''send an object to all links if linktosend is none
otherwise just send to the specified link'''
try:
buf = pickle.dumps(obj, pickle.HIGHEST_PROTOCOL)
except Exception as ex:
print("dump failed: ", ex)
return
if priority is None:
priority = 10000
#only send if the queue is not clogged
if not linktosend:
for bsnd in self.bsend:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf,
priority=priority,
callback=functools.partial(
self.send_object_complete, obj,
bsnd))
else:
if linktosend == self.msend:
qsize = self.camera_settings.m_maxqueue
else:
qsize = self.camera_settings.maxqueue
if linktosend.sendq_size() < qsize:
obj.blockid = linktosend.send(buf,
priority=priority,
callback=functools.partial(
self.send_object_complete,
obj, linktosend))
def handle_command_packet(self, obj, bsend):
'''handle CommandPacket from other end'''
stdout_saved = sys.stdout
buf = StringIO()
sys.stdout = buf
self.mpstate.functions.process_stdin(obj.command, immediate=True)
sys.stdout = stdout_saved
if str(buf.getvalue().strip()):
pkt = cuav_command.CommandResponse(str(buf.getvalue()).strip())
self.transmit_queue.put((pkt, None, self.get_bsend_index(bsend)))
def process(args):
'''process a set of files'''
global slipmap, mosaic
scan_count = 0
files = []
if os.path.isdir(args.directory):
files.extend(file_list(args.directory, ['jpg', 'jpeg', 'png']))
else:
if args.directory.find('*') != -1:
files.extend(glob.glob(args.directory))
else:
files.append(args.directory)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
slipmap = mp_slipmap.MPSlipMap(service=args.service, elevation=True, title='Map')
if args.vehicle_type == "Copter":
icon = slipmap.icon('redcopter.png')
else:
icon = slipmap.icon('redplane.png')
slipmap.add_object(mp_slipmap.SlipIcon('plane', (0,0), icon, layer=3, rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
for flag in args.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = slipmap.icon(icon)
slipmap.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
if args.mission:
from pymavlink import mavwp
wp = mavwp.MAVWPLoader()
wp.load(args.mission.name)
plist = wp.polygon_list()
if len(plist) > 0:
for i in range(len(plist)):
slipmap.add_object(mp_slipmap.SlipPolygon('Mission-%s-%u' % (args.mission.name,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
if args.mavlog:
mpos = mav_position.MavInterpolator()
mpos.set_logfile(args.mavlog.name)
else:
mpos = None
if args.gammalog is not None:
gamma = parse_gamma_log(args.gammalog)
else:
gamma = None
if args.kmzlog:
kmzpos = mav_position.KmlPosition(args.kmzlog.name)
else:
kmzpos = None
if args.triggerlog:
triggerpos = mav_position.TriggerPosition(args.triggerlog.name)
else:
triggerpos = None
# create a simple lens model using the focal length
if args.camera_params:
C_params = cam_params.CameraParams.fromfile(args.camera_params.name)
else:
im_orig = cv2.imread(files[0])
(w,h) = cuav_util.image_shape(im_orig)
C_params = cam_params.CameraParams(lens=args.lens, sensorwidth=args.sensorwidth, xresolution=w, yresolution=h)
if args.target:
target = args.target.split(',')
else:
target = [0,0,0]
camera_settings = MPSettings(
[ MPSetting('roll_stabilised', bool, args.roll_stabilised, 'Roll Stabilised'),
MPSetting('pitch_stabilised', bool, args.pitch_stabilised, 'Pitch Stabilised'),
MPSetting('roll_offset', float, args.roll_offset, 'Roll Offset'),
MPSetting('pitch_offset', float, args.pitch_offset, 'Pitch Offset'),
MPSetting('altitude', int, args.altitude, 'Altitude', range=(0,10000), increment=1),
MPSetting('minalt', int, 30, 'MinAltitude', range=(0,10000), increment=1),
MPSetting('mpp100', float, 0.0977, 'MPPat100m', range=(0,10000), increment=0.001),
MPSetting('rotate180', bool, False, 'rotate180'),
MPSetting('filter_type', str, 'simple', 'Filter Type',
choice=['simple']),
MPSetting('target_lattitude', float, float(target[0]), 'target latitude', increment=1.0e-7),
MPSetting('target_longitude', float, float(target[1]), 'target longitude', increment=1.0e-7),
MPSetting('target_radius', float, float(target[2]), 'target radius', increment=1),
MPSetting('quality', int, 75, 'Compression Quality', range=(1,100), increment=1),
MPSetting('thumbsize', int, args.thumbsize, 'Thumbnail Size', range=(10, 200), increment=1),
MPSetting('RegionHue', int, ags.targethue, 'Target Hue (0 to disable)', range=(0,180), increment=1, tab='Imaging'),
MPSetting('map_thumbsize', int, args.map_thumbsize, 'Map Thumbnail Size', range=(10, 200), increment=1),
MPSetting('minscore', int, args.minscore, 'Min Score', range=(0,1000), increment=1, tab='Scoring'),
MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1,
digits=2, tab='Display'),
],
title='Camera Settings'
)
image_settings = MPSettings(
[ MPSetting('MinRegionArea', float, 0.15, range=(0,100), increment=0.05, digits=2, tab='Image Processing'),
MPSetting('MaxRegionArea', float, 1.0, range=(0,100), increment=0.1, digits=1),
MPSetting('MinRegionSize', float, 0.2, range=(0,100), increment=0.05, digits=2),
MPSetting('MaxRegionSize', float, 1.0, range=(0,100), increment=0.1, digits=1),
MPSetting('MaxRarityPct', float, 0.02, range=(0,100), increment=0.01, digits=2),
MPSetting('RegionMergeSize', float, 1.0, range=(0,100), increment=0.1, digits=1),
MPSetting('SaveIntermediate', bool, args.debug)
],
title='Image Settings')
mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params,
camera_settings=camera_settings,
image_settings=image_settings,
start_menu=True,
classify=args.categories,
thumb_size=args.mosaic_thumbsize,
map_thumb_size=args.map_thumbsize)
joelog = cuav_joe.JoeLog(None)
if args.view:
viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)
start_time = time.time()
for f in files:
if not mosaic.started():
print("Waiting for startup")
if args.start:
mosaic.has_started = True
while not mosaic.started():
mosaic.check_events()
time.sleep(0.01)
if mpos:
# get the position by interpolating telemetry data from the MAVLink log file
# this assumes that the filename contains the timestamp
if gamma is not None:
frame_time = parse_gamma_time(f, gamma)
else:
frame_time = cuav_util.parse_frame_time(f)
frame_time += args.time_offset
if camera_settings.roll_stabilised:
roll = 0
else:
roll = None
if camera_settings.pitch_stabilised:
pitch = 0
else:
pitch = None
try:
pos = mpos.position(frame_time, roll=roll, pitch=pitch,
roll_offset=camera_settings.roll_offset,
pitch_offset=camera_settings.pitch_offset)
except Exception:
print("No position available for %s" % frame_time)
# skip this frame
continue
elif kmzpos is not None:
pos = kmzpos.position(f)
elif triggerpos is not None:
pos = triggerpos.position(f)
else:
# get the position using EXIF data
pos = mav_position.exif_position(f)
pos.time += args.time_offset
# update the vehicle icon on the map
if pos is not None:
slipmap.set_position("plane", (pos.lat, pos.lon), rotation=pos.yaw)
if camera_settings.altitude > 0:
pos.altitude = camera_settings.altitude
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
#im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180)
im_orig = cv2.imread(f)
if im_orig is None:
continue
(w,h) = cuav_util.image_shape(im_orig)
if args.downsample:
im_full = cv2.resize(im_orig, (0,0), fx=0.5, fy=0.5)
else:
im_full = im_orig
count = 0
total_time = 0
t0=time.time()
img_scan = im_full
scan_parms = {}
for name in image_settings.list():
scan_parms[name] = image_settings.get(name)
scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])
if pos is not None:
(sw,sh) = cuav_util.image_shape(img_scan)
altitude = pos.altitude
if altitude < camera_settings.minalt:
altitude = camera_settings.minalt
scan_parms['MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0
regions = scanner.scan(img_scan, scan_parms)
else:
regions = scanner.scan(img_scan)
regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full))
count += 1
t1=time.time()
frame_time = pos.time
regions = cuav_region.filter_regions(im_full, regions, min_score=camera_settings.minscore,
filter_type=camera_settings.filter_type,
target_hue=camera_settings.RegionHue)
if pos:
for r in regions:
r.latlon = cuav_util.gps_position_from_image_region(r, pos, w, h, altitude=altitude, C=C_params)
if camera_settings.target_radius > 0 and pos is not None:
regions = cuav_region.filter_radius(regions, (camera_settings.target_lattitude,
camera_settings.target_longitude),
camera_settings.target_radius)
scan_count += 1
if pos and len(regions) > 0:
joelog.add_regions(frame_time, regions, pos, f)
mosaic.add_image(pos.time, f, pos)
region_count += len(regions)
if len(regions) > 0:
composite = cuav_region.CompositeThumbnail(im_full, regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
mosaic.add_regions(regions, thumbsRGB, f, pos)
if args.view:
img_view = img_scan
(wview,hview) = cuav_util.image_shape(img_view)
for r in regions:
r.draw_rectangle(img_view, (255,0,0))
#cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
img_view = cv2.cvtColor(img_view, cv2.COLOR_BGR2RGB)
viewer.set_image(img_view)
viewer.set_title('Image: ' + os.path.basename(f))
total_time += (t1-t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' % (
os.path.basename(f), count/total_time, region_count, scan_count, num_files))
#raw_input("hit ENTER when ready")
print("All images processed (%u seconds)" % (time.time() - start_time))
while True:
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
time.sleep(0.2)
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(file_list(a, ['jpg', 'pgm', 'png']))
else:
if a.find('*') != -1:
files.extend(glob.glob(a))
else:
files.append(a)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
slipmap = mp_slipmap.MPSlipMap(service=opts.service,
elevation=True,
title='Map')
icon = slipmap.icon('redplane.png')
slipmap.add_object(
mp_slipmap.SlipIcon('plane', (0, 0),
icon,
layer=3,
rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
if opts.mission:
from pymavlink import mavwp
wp = mavwp.MAVWPLoader()
wp.load(opts.mission)
boundary = wp.polygon()
slipmap.add_object(
mp_slipmap.SlipPolygon('mission',
boundary,
layer=1,
linewidth=1,
colour=(255, 255, 255)))
if opts.mavlog:
mpos = mav_position.MavInterpolator()
mpos.set_logfile(opts.mavlog)
else:
mpos = None
if opts.kmzlog:
kmzpos = mav_position.KmlPosition(opts.kmzlog)
else:
kmzpos = None
if opts.triggerlog:
triggerpos = mav_position.TriggerPosition(opts.triggerlog)
else:
triggerpos = None
# create a simple lens model using the focal length
C_params = cam_params.CameraParams(lens=opts.lens,
sensorwidth=opts.sensorwidth)
if opts.camera_params:
C_params.load(opts.camera_params)
camera_settings = MPSettings([
MPSetting('roll_stabilised', bool, True, 'Roll Stabilised'),
MPSetting(
'altitude', int, 0, 'Altitude', range=(0, 10000), increment=1),
MPSetting('filter_type',
str,
'simple',
'Filter Type',
choice=['simple', 'compactness']),
MPSetting('fullres', bool, False, 'Full Resolution'),
MPSetting('quality',
int,
75,
'Compression Quality',
range=(1, 100),
increment=1),
MPSetting('thumbsize',
int,
60,
'Thumbnail Size',
range=(10, 200),
increment=1),
MPSetting('minscore',
int,
75,
'Min Score',
range=(0, 1000),
increment=1,
tab='Scoring'),
MPSetting('brightness',
float,
1.0,
'Display Brightness',
range=(0.1, 10),
increment=0.1,
digits=2,
tab='Display')
],
title='Camera Settings')
image_settings = MPSettings([
MPSetting('MinRegionArea',
float,
0.15,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionArea',
float,
2.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('MinRegionSize',
float,
0.1,
range=(0, 100),
increment=0.05,
digits=2),
MPSetting(
'MaxRegionSize', float, 2, range=(0, 100), increment=0.1,
digits=1),
MPSetting('MaxRarityPct',
float,
0.02,
range=(0, 100),
increment=0.01,
digits=2),
MPSetting('RegionMergeSize',
float,
3.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('SaveIntermediate', bool, False)
],
title='Image Settings')
mosaic = cuav_mosaic.Mosaic(slipmap,
C=C_params,
camera_settings=camera_settings,
image_settings=image_settings,
start_menu=True)
joelog = cuav_joe.JoeLog(None)
if opts.view:
viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)
if camera_settings.filter_type == 'compactness':
calculate_compactness = True
print("Using compactness filter")
else:
calculate_compactness = False
for f in files:
if not mosaic.started():
print("Waiting for startup")
while not mosaic.started():
mosaic.check_events()
time.sleep(0.01)
if mpos:
# get the position by interpolating telemetry data from the MAVLink log file
# this assumes that the filename contains the timestamp
frame_time = cuav_util.parse_frame_time(f) + opts.time_offset
if camera_settings.roll_stabilised:
roll = 0
else:
roll = None
try:
pos = mpos.position(frame_time, roll=roll)
except Exception:
print("No position available for %s" % frame_time)
# skip this frame
continue
elif kmzpos is not None:
pos = kmzpos.position(f)
elif triggerpos is not None:
pos = triggerpos.position(f)
else:
# get the position using EXIF data
pos = mav_position.exif_position(f)
pos.time += opts.time_offset
# update the plane icon on the map
if pos is not None:
slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
if camera_settings.altitude > 0:
pos.altitude = camera_settings.altitude
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
im_orig = cuav_util.LoadImage(f)
(w, h) = cuav_util.image_shape(im_orig)
if not opts.camera_params:
C_params.set_resolution(w, h)
im_full = im_orig
im_640 = cv.CreateImage((640, 480), 8, 3)
cv.Resize(im_full, im_640, cv.CV_INTER_NN)
im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
im_full = numpy.ascontiguousarray(cv.GetMat(im_full))
count = 0
total_time = 0
t0 = time.time()
if camera_settings.fullres:
img_scan = im_full
else:
img_scan = im_640
scan_parms = {}
for name in image_settings.list():
scan_parms[name] = image_settings.get(name)
scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])
if pos is not None:
(sw, sh) = cuav_util.image_shape(img_scan)
mpp = cuav_util.meters_per_pixel(pos, C=C_params)
if mpp is not None:
scan_parms['MetersPerPixel'] = mpp * (w / float(sw))
regions = scanner.scan(img_scan, scan_parms)
else:
regions = scanner.scan(img_scan)
regions = cuav_region.RegionsConvert(regions,
cuav_util.image_shape(img_scan),
cuav_util.image_shape(im_full),
calculate_compactness)
count += 1
t1 = time.time()
frame_time = pos.time
regions = cuav_region.filter_regions(
im_full,
regions,
frame_time=frame_time,
min_score=camera_settings.minscore,
filter_type=camera_settings.filter_type)
scan_count += 1
mosaic.add_image(pos.time, f, pos)
if pos and len(regions) > 0:
altitude = camera_settings.altitude
if altitude <= 0:
altitude = None
joelog.add_regions(frame_time,
regions,
pos,
f,
width=w,
height=h,
altitude=altitude)
region_count += len(regions)
if len(regions) > 0:
composite = cuav_mosaic.CompositeThumbnail(
cv.GetImage(cv.fromarray(im_full)), regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if opts.view:
img_view = img_scan
(wview, hview) = cuav_util.image_shape(img_view)
mat = cv.fromarray(img_view)
for r in regions:
r.draw_rectangle(mat, (255, 0, 0))
cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
viewer.set_image(mat)
viewer.set_title('Image: ' + os.path.basename(f))
total_time += (t1 - t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' %
(os.path.basename(f), count / total_time, region_count,
scan_count, num_files))
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(file_list(a, ['jpg', 'pgm', 'png']))
else:
if a.find('*') != -1:
files.extend(glob.glob(a))
else:
files.append(a)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
slipmap = mp_slipmap.MPSlipMap(service=opts.service, elevation=True, title='Map')
icon = slipmap.icon('redplane.png')
slipmap.add_object(mp_slipmap.SlipIcon('plane', (0,0), icon, layer=3, rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = slipmap.icon(icon)
slipmap.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
if opts.mission:
from pymavlink import mavwp
wp = mavwp.MAVWPLoader()
wp.load(opts.mission)
boundary = wp.polygon()
slipmap.add_object(mp_slipmap.SlipPolygon('mission', boundary, layer=1,
linewidth=1, colour=(255,255,255)))
if opts.mavlog:
mpos = mav_position.MavInterpolator()
mpos.set_logfile(opts.mavlog)
else:
mpos = None
if opts.gammalog is not None:
gamma = parse_gamma_log(opts.gammalog)
else:
gamma = None
if opts.kmzlog:
kmzpos = mav_position.KmlPosition(opts.kmzlog)
else:
kmzpos = None
if opts.triggerlog:
triggerpos = mav_position.TriggerPosition(opts.triggerlog)
else:
triggerpos = None
# create a simple lens model using the focal length
C_params = cam_params.CameraParams(lens=opts.lens, sensorwidth=opts.sensorwidth)
if opts.camera_params:
C_params.load(opts.camera_params)
camera_settings = MPSettings(
[ MPSetting('roll_stabilised', bool, opts.roll_stabilised, 'Roll Stabilised'),
MPSetting('altitude', int, opts.altitude, 'Altitude', range=(0,10000), increment=1),
MPSetting('minalt', int, 30, 'MinAltitude', range=(0,10000), increment=1),
MPSetting('mpp100', float, 0.0977, 'MPPat100m', range=(0,10000), increment=0.001),
MPSetting('filter_type', str, 'simple', 'Filter Type',
choice=['simple', 'compactness']),
MPSetting('quality', int, 75, 'Compression Quality', range=(1,100), increment=1),
MPSetting('thumbsize', int, opts.thumbsize, 'Thumbnail Size', range=(10, 200), increment=1),
MPSetting('minscore', int, opts.minscore, 'Min Score', range=(0,1000), increment=1, tab='Scoring'),
MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1,
digits=2, tab='Display')
],
title='Camera Settings'
)
image_settings = MPSettings(
[ MPSetting('MinRegionArea', float, 0.15, range=(0,100), increment=0.05, digits=2, tab='Image Processing'),
MPSetting('MaxRegionArea', float, 2.0, range=(0,100), increment=0.1, digits=1),
MPSetting('MinRegionSize', float, 0.1, range=(0,100), increment=0.05, digits=2),
MPSetting('MaxRegionSize', float, 2, range=(0,100), increment=0.1, digits=1),
MPSetting('MaxRarityPct', float, 0.02, range=(0,100), increment=0.01, digits=2),
MPSetting('RegionMergeSize', float, 3.0, range=(0,100), increment=0.1, digits=1),
MPSetting('SaveIntermediate', bool, opts.debug)
],
title='Image Settings')
mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params,
camera_settings=camera_settings,
image_settings=image_settings,
start_menu=True,
classify=opts.categories,
thumb_size=opts.mosaic_thumbsize)
joelog = cuav_joe.JoeLog(None)
if opts.view:
viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)
if camera_settings.filter_type == 'compactness':
calculate_compactness = True
print("Using compactness filter")
else:
calculate_compactness = False
for f in files:
if not mosaic.started():
print("Waiting for startup")
while not mosaic.started():
mosaic.check_events()
time.sleep(0.01)
if mpos:
# get the position by interpolating telemetry data from the MAVLink log file
# this assumes that the filename contains the timestamp
if gamma is not None:
frame_time = parse_gamma_time(f, gamma)
else:
frame_time = cuav_util.parse_frame_time(f)
frame_time += opts.time_offset
if camera_settings.roll_stabilised:
roll = 0
else:
roll = None
try:
pos = mpos.position(frame_time, roll=roll)
except Exception:
print("No position available for %s" % frame_time)
# skip this frame
continue
elif kmzpos is not None:
pos = kmzpos.position(f)
elif triggerpos is not None:
pos = triggerpos.position(f)
else:
# get the position using EXIF data
pos = mav_position.exif_position(f)
pos.time += opts.time_offset
# update the plane icon on the map
if pos is not None:
slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
if camera_settings.altitude > 0:
pos.altitude = camera_settings.altitude
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
im_orig = cuav_util.LoadImage(f)
(w,h) = cuav_util.image_shape(im_orig)
if not opts.camera_params:
C_params.set_resolution(w, h)
im_full = im_orig
im_640 = cv.CreateImage((640, 480), 8, 3)
cv.Resize(im_full, im_640, cv.CV_INTER_NN)
im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
im_full = numpy.ascontiguousarray(cv.GetMat(im_full))
count = 0
total_time = 0
t0=time.time()
img_scan = im_full
scan_parms = {}
for name in image_settings.list():
scan_parms[name] = image_settings.get(name)
scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])
if pos is not None:
(sw,sh) = cuav_util.image_shape(img_scan)
altitude = pos.altitude
if altitude < camera_settings.minalt:
altitude = camera_settings.minalt
scan_parms['MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0
regions = scanner.scan(img_scan, scan_parms)
else:
regions = scanner.scan(img_scan)
regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full), calculate_compactness)
count += 1
t1=time.time()
frame_time = pos.time
regions = cuav_region.filter_regions(im_full, regions, frame_time=frame_time,
min_score=camera_settings.minscore,
filter_type=camera_settings.filter_type)
scan_count += 1
if pos and len(regions) > 0:
altitude = camera_settings.altitude
if altitude <= 0:
altitude = None
joelog.add_regions(frame_time, regions, pos, f, width=w, height=h,
altitude=altitude)
mosaic.add_image(pos.time, f, pos)
region_count += len(regions)
if len(regions) > 0:
composite = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)), regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if opts.view:
img_view = img_scan
(wview,hview) = cuav_util.image_shape(img_view)
mat = cv.fromarray(img_view)
for r in regions:
r.draw_rectangle(mat, (255,0,0))
cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
viewer.set_image(mat)
viewer.set_title('Image: ' + os.path.basename(f))
total_time += (t1-t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' % (
os.path.basename(f), count/total_time, region_count, scan_count, num_files))
