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))