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