def __init__(self, connection_string, filter_servo_pin=None, filter_count=None, mode=NO_FILTER, capture_time=2, **kwargs): self.vehicle = DronekitConnector(connection_string) self.vehicle.get_mission(update=True) self.camera = PicamImpl() self.capture_time = capture_time self.mode = None self.mission_started = False self.wp = None self.waypoint_capture = False self.close_mission = False self.threads = {} self._init_threads() if mode == self.CONT_FILTER: self.filter = FilterContWheel(filter_servo_pin, filter_count, **kwargs) elif mode == self.POS_FILTER: self.filter = FilterPosWheel(filter_servo_pin) else: self.filter = None
def run_main(self): args = self.get_args() connection = args.connection if connection is None: conn_str = self.n_cfg.udp_ext['picam_1_to_robot'] else: conn_str = connection self.logger.log_info("Starting camera, connecting robot") self.camera = PicamImpl(PicamImpl.RAW_BAYER) self.dconn = DronekitConnector(conn_str) self.logger.log_info("Startup complete") dirname = 'sc_%d' % int(time.time()) self.camera.set_save_directory(dirname) started = False self.logger.log_info("Start mission capture") while not self.exit_all_loops: if self.dconn.get_mode() == 'AUTO': started = True try: self.camera.capture_single() except Exception as e: self.logger.log_error("Could not capture frame: %s" % e) if started and self.dconn.get_mode() != 'AUTO': break
def __init__(self, filter_servo_pin, filter_wheel_count, connection_string): self.vehicle = DronekitConnector(connection_string) self.camera = PicamImpl() self.filter = FilterContWheel(filter_servo_pin, filter_wheel_count) self.filter.detect_c_threshold() self.mode = None self.wp = None self.capture = False self.filter_capture = False self.close_mission = False self.mission_complete = False self.threads = {} self._init_threads()
def __init__(self, filter_servo_pin, connection_string=None, connect_robot=False): self.camera = PicamImpl() self.close_mission = False self.filter = FilterPosWheel(filter_servo_pin) if connect_robot and connection_string is not None: try: self.vehicle = DronekitConnector(connection_string) except: self.vehicle = None else: self.vehicle = None
def run_main(self): args = self.get_args() args_ = {} if args.pin is None: raise Exception("Servo pin required") else: try: args_['servo_pin'] = int(args.pin) except: raise Exception("Valid servo pin required") self.fpw = FilterPosWheel(**args_) camera = PicamImpl() camera.set_save_directory('tests', 'filter_pos') while not self.exit_threads: progress("\ n: rotate to next filter\n\ p: rotate to previous filter\n\ a: capture image\n\ e: exit\n\ filter id, >0 : rotate to filter no.\n\ ") try: c = input() if c == "n": self.logger.log_info("Rotate to next filter...") self.fpw.rotate_to_next() self.logger.log_info("Filter reached: %d" % self.fpw.filter_id) elif c == "p": self.logger.log_info("Rotate to previous filter...") self.fpw.rotate_to_prev() self.logger.log_info("Filter reached: %d" % self.fpw.filter_id) elif c == "e": self.logger.log_info("Exiting...") break elif c == "a": self.logger.log_info("Capturing image...") camera.capture_single() else: try: fid = int(c) self.logger.log_info("Rotate to filter %d..." % fid) self.fpw.rotate_to_filter(fid) self.logger.log_info("Filter reached: %d" % self.fpw.filter_id) except: self.logger.log_info("Got keyboard input %s" % c) except Exception as e: self.logger.log_debug(e)
def run_main(self): args = self.get_args() args_ = {} if args.pin is None: raise Exception("Servo pin required") else: args_['servo_pin'] = args.pin if args.filters is None: raise Exception("Filter count required") else: args_['filter_count'] = args.filters if args.c_thresh: args_['c_threshold'] = args.c_thresh # else: # args_['c_threshold'] = 1000 if args.c_thresh_var: args_['c_threshold_var'] = args.c_thresh_var # else: # args_['c_threshold'] = 50 fcw = FilterContWheel(**args_) if args.speed: speed = args.speed else: speed = 0.5 camera = PicamImpl() camera.set_save_directory('tests', 'filter_cont') while not self.exit_threads: progress("====================================\n\ n: rotate to next filter\n\ r: rotate at current set speed\n\ s: stop rotation\n\ w: increase speed (max 1)\n\ q: decrease speed (min -1)\n\ d: check if notch is detected\n\ c: calibrate c threshold\n\ a: capture image\n\ e: exit\n\ filter id, >0 : rotate to filter no.\n\ ====================================\n\ ") try: c = input() if c == "n": self.logger.log_info("Rotate to next filter...") fcw.rotate_to_next(speed) self.logger.log_info("Filter reached: %d" % fcw.filter_id) elif c == "r": self.logger.log_info("Rotate at speed %f" % speed) fcw.rotate(speed) elif c == "s": fcw.halt() self.logger.log_info("Rotation stopped") elif c == "w": if speed < 1.0: speed += 0.1 self.logger.log_info("Speed set to %f" % speed) elif c == "q": if speed > -1.0: speed -= 0.1 self.logger.log_info("Speed set to %f" % speed) elif c == "d": self.logger.log_info("Check notch detected...") detected = str(fcw.is_notch_detected()) self.logger.log_info("Notch detected: %s" % detected) elif c == "c": self.logger.log_info("Calibrating c threshold...") a = fcw.detect_c_threshold() self.logger.log_info("C threshold found: %d" % fcw.c_threshold) self.logger.log_info("C threshold found: %s" % str(a)) elif c == "a": self.logger.log_info("Capturing image...") camera.capture_single() elif c == "e": self.logger.log_info("Exiting...") break else: try: fid = int(c) except: self.logger.log_info("Got keyboard input %s" % c) else: self.logger.log_info("Rotate to filter %d..." % fid) fcw.rotate_to_filter(fid) self.logger.log_info("Filter reached: %d" % fcw.filter_id) except Exception as e: self.logger.log_debug(e)
class WaypointMissionExec: NO_FILTER = 0 CONT_FILTER = 1 POS_FILTER = 2 def __init__(self, connection_string, filter_servo_pin=None, filter_count=None, mode=NO_FILTER, capture_time=2, **kwargs): self.vehicle = DronekitConnector(connection_string) self.vehicle.get_mission(update=True) self.camera = PicamImpl() self.capture_time = capture_time self.mode = None self.mission_started = False self.wp = None self.waypoint_capture = False self.close_mission = False self.threads = {} self._init_threads() if mode == self.CONT_FILTER: self.filter = FilterContWheel(filter_servo_pin, filter_count, **kwargs) elif mode == self.POS_FILTER: self.filter = FilterPosWheel(filter_servo_pin) else: self.filter = None def _init_threads(self): self.threads['check_state_t'] = threading.Thread( target=self._check_state_thread) self.threads['capture_t'] = threading.Thread( target=self._capture_thread) self.threads['check_state_t'].start() def start(self): self.threads['capture_t'].start() def stop(self): self.close_mission = True time.sleep(2) for t in self.threads: try: t.join() except: pass self.vehicle.disconnect() self.camera.disconnect() def is_mission_complete(self): return self.close_mission def _check_state_thread(self): while not self.close_mission: self.mode = self.vehicle.get_mode() # Camera will capture frames when mission has started and mode is not manual if self.mode == 'AUTO': self.mission_started = True elif self.mission_started: continue # Mission is started but robot is not in AUTO mode new_wp = self.vehicle.mission.next if new_wp != self.wp: # Trigger filter capture at waypoint self.waypoint_capture = True self.wp = new_wp if self.wp == self.vehicle.mission.count and self.mode == 'MANUAL': self.close_mission = True def _capture_thread(self): filename = self.camera.save_dir + 'waypoint_mission.csv' file_exists = os.path.exists(filename) with open(filename, 'a+', newline='') as csvfile: csvwriter = csv.writer(csvfile, delimiter=',') if not file_exists: header = [ 'image_timestamp', 'gps_lat', 'gps_lon', 'gps_fix_type' ] csvwriter.writerow(header) while not self.close_mission: try: if self.waypoint_capture: if self.mode == self.NO_FILTER: images = self._capture_no_filter() else: images = self._process_filters() csvwriter.writerow(images) self.waypoint_capture = False except: pass def _capture_no_filter(self): images = [] start = time.time() while time.time() < start + self.capture_time: filename = self.camera.capture_single() images.append([filename] + self.vehicle.get_gps()) return images def _process_filters(self): images = [] for i in range(self.filter.filter_count): self.filter.rotate_to_next(0.5) filename = self.camera.capture_single("f%d" % i) images.append([filename] + self.vehicle.get_gps()) return images
class FilterCapture: def __init__(self, filter_servo_pin, connection_string=None, connect_robot=False): self.camera = PicamImpl() self.close_mission = False self.filter = FilterPosWheel(filter_servo_pin) if connect_robot and connection_string is not None: try: self.vehicle = DronekitConnector(connection_string) except: self.vehicle = None else: self.vehicle = None def run_once_gps_log(self): filename = self.camera.save_dir + 'waypoint_mission.csv' file_exists = os.path.exists(filename) with open(filename, 'a+', newline='') as csvfile: csvwriter = csv.writer(csvfile, delimiter=',') if not file_exists: header = [ 'image_timestamp', 'gps_lat', 'gps_lon', 'gps_fix_type' ] csvwriter.writerow(header) while not self.close_mission: filters = len(self.filter.filter_angles) try: for _ in range(filters): images = self._process_filters() csvwriter.writerow(images) except: pass def run_once(self): filters = len(self.filter.filter_angles) for i in range(filters): self.camera.capture_single("f%d" % i) self.filter.rotate_to_next() def _process_filters(self): images = [] for i in range(self.filter.filter_count): filename = self.camera.capture_single("f%d" % i) if self.vehicle: gps_data = self.vehicle.get_gps() else: gps_data = [] images.append([filename] + gps_data) self.filter.rotate_to_next() return images def stop(self): self.camera.disconnect() self.filter.stop() self.vehicle.disconnect()
class FilterMissionExec: def __init__(self, filter_servo_pin, filter_wheel_count, connection_string): self.vehicle = DronekitConnector(connection_string) self.camera = PicamImpl() self.filter = FilterContWheel(filter_servo_pin, filter_wheel_count) self.filter.detect_c_threshold() self.mode = None self.wp = None self.capture = False self.filter_capture = False self.close_mission = False self.mission_complete = False self.threads = {} self._init_threads() def _init_threads(self): self.threads['check_state_t'] = threading.Thread( target=self._check_state_thread) self.threads['capture_t'] = threading.Thread( target=self._capture_thread) self.threads['check_state_t'].start() def start(self): self.threads['capture_t'].start() def stop(self): self.close_mission = True time.sleep(2) for t in self.threads: try: t.join() except: pass self.vehicle.disconnect() self.camera.disconnect() def is_mission_complete(self): return self.mission_complete def _check_state_thread(self): while not self.close_mission: self.mode = self.vehicle.get_mode() # Camera will capture frames when mission has started and mode is not manual if self.mode == 'MANUAL': self.capture = False else: self.capture = True new_wp = self.vehicle.mission.next if new_wp != self.wp: # Trigger filter capture at waypoint self.filter_capture = True self.wp = new_wp if self.wp == self.vehicle.mission.count and self.mode == 'MANUAL': self.mission_complete = True def _capture_thread(self): while not self.close_mission: try: if self.filter_capture: self._process_filters() self.filter_capture = False if self.capture: self.camera.capture_single() except: pass def _process_filters(self): for i in range(self.filter.filter_count): self.filter.rotate_to_next(0.5) self.camera.capture_single("f%d" % i)
#!/usr/bin/env python3 import time import sys import signal import threading from picam_lib import PicamImpl #### Setup print("Starting camera, connecting robot") camera = PicamImpl(PicamImpl.RAW_BAYER) subdir = "swc_" % int(time.time()) camera.set_save_directory(subdir) print("Startup complete") #### Functions exit_all_loops = False exit_code = 1 def _sigint_handler(sig, frame): global exit_all_loops exit_all_loops = True def _sigterm_handler(sig, frame): global exit_all_loops, exit_code exit_all_loops = True