def process_video(file_name, n=1):
"""
iterate over frames of videofile
:param file_name: filename
:param n:process one out of every n frames-> default 1: take all frames
:return:list of finish frames
"""
fvs = FileVideoStream(file_name).start() #load video
cam = cv2.VideoCapture(file_name)
fps = cam.get(cv2.CAP_PROP_FPS) #get original fps of video
counter = 1
frame_list = []
teller = 0
while fvs.running():
if fvs.more():
teller += 1
frame = fvs.read()
if frame is not None:
frame_list.append(
cv2.resize(
frame,
(int(frame.shape[1] / 2), int(frame.shape[0] / 2)))
) #append frame to list and resize it: height/2, width/2
counter += 1
else:
time.sleep(2)
return (frame_list, fps)
def extract(input_path, output_folder, rate):
STR_FRAME_PATH = "{}\\frame{}.jpg"
video_args = {}
video_args["video"] = input_path
fvs = FileVideoStream(video_args["video"], queue_size=300).start()
time.sleep(1.0)
framecount = 0
fps = FPS().start()
while fvs.running():
frame = fvs.read()
img_raw = None
framecount = framecount + 1
if framecount % int(rate) == 0:
print("Extrating frame {}".format(framecount), end="\r")
cv2.imwrite(STR_FRAME_PATH.format(output_folder, str(framecount)),
frame)
else:
continue
fps.update()
fps.stop()
fvs.stop()
print("[INFO] elasped time: {:.2f}".format(fps.elapsed()))
print("[INFO] approx. FPS: {:.2f}".format(fps.fps()))
class VideoSource (object):
def __init__ (self, video_file, log, use_pi_camera = True, resolution=(320, 200), framerate = 30, night = False):
self.filename = video_file
self.log = log
self.use_pi_camera = use_pi_camera
self.resolution = resolution
self.framerate = framerate
self.night = night
self.fvs = None
self.stream = None
self._done = False
def start (self):
if self.filename is not None:
self.log.debug('Video file: %s', self.filename)
self.fvs = FileVideoStream(self.filename).start()
self.stream = self.fvs.stream
time.sleep(1.0)
else:
if self.use_pi_camera:
self.log.debug('Pi Camera (%d %d)', self.resolution[0], self.resolution[1])
self.stream = VideoStream(src=0,
usePiCamera=True,
resolution=self.resolution,
framerate=self.framerate,
sensor_mode=5
).start()
else:
self.log.debug('Web Camera')
self.stream = cv2.VideoCapture(0)
self.stream.set(cv2.CAP_PROP_BUFFERSIZE, 2)
self.resolution = (
self.stream.get(cv2.CAP_PROP_FRAME_WIDTH),
self.stream.get(cv2.CAP_PROP_FRAME_HEIGHT)
)
self.framerate = self.stream.get(cv2.CAP_PROP_FPS)
return self.resolution, self.framerate
def read(self):
if self.filename:
frame = self.fvs.read()
else:
frame = self.stream.read()
return frame
def stop (self):
if self.filename:
self.fvs.stop()
else:
self.stream.stop()
self._done = True
def done(self):
# check to see if video is still running
running = (self.filename and self.fvs.running()) or True
self._done = not running
return self._done
def extract(input_path, output_folder, rate):
STR_FRAME_PATH = "{}\\frame{}.jpg"
video_args = {}
video_args["video"] = input_path
fvs = FileVideoStream(video_args["video"], queue_size=300).start()
time.sleep(1.0)
framecount = 0
fps = FPS().start()
net = cv2.dnn.readNetFromCaffe(args.face_prototxt, args.face_model)
while fvs.running():
frame = fvs.read()
img_raw = None
framecount = framecount + 1
if framecount % int(rate) == 0:
print("Extrating frame {}".format(framecount), end="\r")
path = "{}\\{}{}.jpg".format(
Path(args.input).parent,
Path(args.input).stem, framecount)
faces = extract_faces_from_image(frame, path, args.confidence, net)
faces_to_save = []
for face in faces:
gender = get_gender(face, args.gender_model, args.gender_proto)
if gender["gender"] == "Female" and gender["conf"] > 0.98:
faces_to_save.append(face)
save_faces_from_image(faces_to_save, path)
else:
continue
fps.update()
fps.stop()
fvs.stop()
print("[INFO] elasped time: {:.2f}".format(fps.elapsed()))
print("[INFO] approx. FPS: {:.2f}".format(fps.fps()))
def cli(ctx, sink, fp_in, opt_size_type):
"""Add mappings data to chain"""
# -------------------------------------------------------------
# imports
import os
import logging
import cv2 as cv
from tqdm import tqdm
import imutils
from vframe.settings.paths import Paths
from vframe.utils import file_utils, logger_utils
from vframe.models.chair_item import ChairItem, PhotoChairItem, VideoKeyframeChairItem
from vframe.utils import logger_utils, im_utils
# ---------------------------------------------------------------
# init
log = logger_utils.Logger.getLogger()
log.info('fp_in: {}'.format(fp_in))
# assume video
ext = file_utils.get_ext(fp_in)
media_format = file_utils.ext_media_format(ext) # enum type
# check file existence
if not Path(fp_in).exists():
log.error('file not found: {}'.format(fp_in))
return
if media_format == types.MediaFormat.PHOTO:
log.debug('photo type')
sink.send(PhotoChairItem(ctx, fp_in))
elif media_format == types.MediaFormat.VIDEO:
log.debug('video type')
import time
from imutils.video import FileVideoStream
opt_size = cfg.IMAGE_SIZES[opt_size_type]
# TODO this should be in "add_images" command
def frame_transform(frame):
return im_utils.resize(frame, width=opt_size)
fvs = FileVideoStream(fp_in, transform=frame_transform).start()
time.sleep(1.0) # recommended delay
stream = fvs.stream
ctx.opts['last_display_ms'] = time.time()
ctx.opts['fps'] = stream.get(cv.CAP_PROP_FPS)
ctx.opts['mspf'] = int(1 / ctx.opts['fps'] *
1000) # milliseconds per frame
# loop over frames from the video file stream
frame_idx = 0
while fvs.running():
frame = fvs.read()
sink.send(VideoKeyframeChairItem(ctx, frame, frame_idx))
frame_idx += 1
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video", required=True,
help="path to input video file")
args = vars(ap.parse_args())
# start the file video stream thread and allow the buffer to
# start to fill
print("[INFO] starting video file thread...")
fvs = FileVideoStream(args["video"], transform=filterFrame).start()
time.sleep(1.0)
# start the FPS timer
fps = FPS().start()
# loop over frames from the video file stream
while fvs.running():
# grab the frame from the threaded video file stream, resize
# it, and convert it to grayscale (while still retaining 3
# channels)
frame = fvs.read()
# Relocated filtering into producer thread with transform=filterFrame
# Python 2.7: FPS 92.11 -> 131.36
# Python 3.7: FPS 41.44 -> 50.11
#frame = filterFrame(frame)
# display the size of the queue on the frame
cv2.putText(frame, "Queue Size: {}".format(fvs.Q.qsize()),
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
# show the frame and update the FPS counter
class movement_direction:
def __init__(self, video_source_info, user_roi):
self.source_info = video_source_info
self.source = None
if self.source_info == '0' or self.source_info == '1':
self.source = 'Camera'
elif isinstance(self.source_info, str) and Path(
self.source_info).exists() and Path(sys.argv[1]).is_file():
self.source = self.source_info
self._is_valid = not (self.source is None)
self.frame_count = 0
self.prev_frame = None
self.prev_angle = None
self.current_angle = None
self.angle_estimate_available = False
self.angle_posts = dict(Right=75, Left=120)
self.angle_diff_threshold = 7
self.current_state = State.eUnknown
self.prev_state = self.current_state
self.logger = get_logger()
self.angle_states = {}
self.angle_states[State.eRight] = 'Right'
self.angle_states[State.eLeft] = 'Left'
self.angle_states[State.eSame] = 'Straight'
self.angle_states[State.eUnknown] = 'Unknown'
'''
Initialize Capture and set up region of interest
'''
if self.source == 'Camera':
self.cap = WebcamVideoStream(src=int(self.source_info))
else:
self.cap = FileVideoStream(self.source_info)
self.cap.start()
frame = self.cap.read()
self.prev_frame = frame.copy()
shape = self.prev_frame.shape
time.sleep(2)
(self.source_height, self.source_width) = self.prev_frame.shape[:2]
self._is_loaded = True
self.logger.info('Source is ' + self.source)
video_roi = user_roi
if self.source == 'Camera':
camera_roi = dict(row_low=0,
row_high=self.source_height - 1,
column_low=0,
column_high=self.source_width - 1)
video_roi = camera_roi
self.logger.info(video_roi)
self.row_range = (video_roi['row_low'], video_roi['row_high'])
self.column_range = (video_roi['column_low'], video_roi['column_high'])
self.settings = initialize_settings_from_video_roi(video_roi)
self.width = video_roi['column_high'] - video_roi['column_low']
self.height = video_roi['row_high'] - video_roi['row_low']
assert (self.width <= self.source_width)
assert (self.height <= self.source_height)
self.logger.info((self.source_width, self.source_height))
'''
Initialize tracking parameters
'''
self.feature_params = self.settings['feature_params']
self.lk_params = self.settings['lk_params']
self.max_dist = self.settings['max_distance']
self.prev_frame = None
self.keypoint_dist = 0
self.min_features = self.settings['min_features']
self.old_points = []
self.new_points = []
self.keypoints = []
self.display = None
self.debug = False
self.direction = []
self.avg_angle = None
self.show_vectors = False
self.show_axis = False
def is_loaded(self):
return self._is_loaded
def is_valid(self):
return self._is_valid
def run(self):
frame = self.prev_frame.copy()
self.prev_frame = self.prepare_frame(frame)
self.logger.info(('frame: ', self.frame_count))
self.frame_count = self.frame_count + 1
cv.namedWindow('Frame', cv.WINDOW_NORMAL)
while self.cap.running():
captured_frame = self.cap.read()
if not (captured_frame is None):
self.frame_count = self.frame_count + 1
frame = self.prepare_frame(captured_frame)
assert (not (self.prev_frame is None))
self.update_features()
self.get_flow(frame)
self.prev_frame = frame.copy()
# self.draw_tracks( self.keypoints, self.new_points)
cv.imshow("Frame", self.display)
def prepare_frame(self, frame):
frame = frame[self.row_range[0]:self.row_range[1],
self.column_range[0]:self.column_range[1]]
self.display = frame.copy()
frame = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
return frame
def get_features(self):
self.keypoint_dist = 0
# collect features on the prev frame and compute its geometric median
self.old_points = cv.goodFeaturesToTrack(self.prev_frame,
mask=None,
**self.feature_params)
self.keypoints = np.copy(self.old_points)
def update_features(self):
# if moved too much re create features on the prev frame
if self.keypoint_dist > self.max_dist:
self.get_features()
def set_state(_instant_angle):
if _instant_angle <= self.angle_posts['Right']:
self.current_state = State.eRight
elif _instant_angle >= self.angle_posts['Left']:
self.current_state = State.eLeft
else:
self.current_state = State.eSame
if self.debug: self.logger.debug('max distance passed: reset ')
avg_angle = self.measure_tracks(self.keypoints, self.new_points)
avg_angle = math.degrees(avg_angle) % 360
self.direction.append(avg_angle)
instant_angle = int(np.average(self.direction))
if self.prev_angle is None:
self.prev_angle = instant_angle
self.prev_state = State.eUnknown
self.current_angle = instant_angle
diff = self.current_angle - self.prev_angle
ok = diff < self.angle_diff_threshold
if ok and self.current_state == self.prev_state:
self.current_state = State.eSame
else:
set_state(instant_angle)
self.prev_angle = self.current_angle
self.prev_state = self.current_state
## determine current direction:
## difference = current - prev
## if difference < 0 threshold and current is not Unknow ==> Straight
## if prev != current ==> current
self.logger.info(('frame, direction:', self.frame_count,
instant_angle, self.current_state))
# if few new points create features on the prev frame
elif len(self.new_points) < self.min_features:
self.get_features()
if self.debug:
self.logger.debug('copied old points to keypoints ')
else:
# check number of features in each quadrant to ensure a good distribution of features across entire image
nw = ne = sw = se = 0
w = self.width
h = self.height
for x, y in self.new_points:
if x > w // 2:
if y > h // 2:
se += 1
else:
ne += 1
else:
if y > h // 2:
sw += 1
else:
nw += 1
self.num_features = min((nw, ne))
if self.num_features < self.min_features // 4:
self.get_features()
# if self.debug: self.logger.debug('too few features reset ')
else:
# just copy new points to old points
dim = np.shape(self.new_points)
self.old_points = np.reshape(self.new_points, (-1, 1, 2))
if self.debug: self.logger.debug('ok')
def get_flow(self, frame):
self.new_points, self.status, self.error = cv.calcOpticalFlowPyrLK(
self.prev_frame, frame, self.old_points, None, **self.lk_params)
self.keypoints = np.reshape(
self.keypoints,
(-1, 1, 2)) # TODO find out why this is necessary?!
self.old_points = self.old_points[self.status == 1]
self.new_points = self.new_points[self.status == 1]
self.keypoints = self.keypoints[self.status == 1]
self.keypoint_dist += self.get_mean_distance_2d(
self.old_points, self.new_points)
def get_mean_distance_2d(self, features1, features2):
num_features = min((len(features1), len(features2)))
features1 = np.reshape(features1, (num_features, 2))
features2 = np.reshape(features2, (num_features, 2))
features = zip(features1, features2)
n = 0
dist = 0
for f1, f2 in features:
dx = f1[0] - f2[0]
dy = f1[1] - f2[1]
d = math.sqrt(dx**2 + dy**2)
dist += d
n += 1
if n == 0:
return 0
dist /= n
return dist
def draw_direction(self):
if self.prev_state != State.eUnknown and self.current_state != State.eUnknown:
direction = self.angle_states[self.current_state]
cv.putText(self.display, direction,
(self.width // 2, self.height // 2),
cv.FONT_HERSHEY_DUPLEX, 2, (225, 0, 0), 7)
def draw_tracks(self, points1, points2):
if self.show_vectors:
for i, (new, old) in enumerate(zip(points1, points2)):
a, b = new.ravel()
c, d = old.ravel()
cl = (0, 255, 0)
if a < self.width / 2: cl = (255, 0, 0)
frame = cv.line(self.display, (c, d), (a, b), cl, 1)
cv.circle(self.display, (a, b), 5, cl, 0, 3)
self.draw_direction()
cv.putText(self.display, str(self.frame_count),
(self.width // 16, (15 * self.height) // 16),
cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
if self.show_axis:
cv.line(self.display, (0, self.height // 2),
(self.width, self.height // 2), (255, 0, 0), 1)
cv.line(self.display, (self.width // 2, 0),
(self.width // 2, self.height), (255, 0, 0), 1)
def measure_tracks(self, points1, points2):
angles = []
for i, (new, old) in enumerate(zip(points1, points2)):
a, b = new.ravel()
c, d = old.ravel()
line = [c, d, a, b]
angle = get_line_angle(line)
angles.append(angle)
angles = np.asanyarray(angles)
mean_angle = np.arctan2(np.sin(angles).mean(), np.cos(angles).mean())
if mean_angle < 0:
mean_angle = math.pi + math.pi + mean_angle
return mean_angle
class FlightTab(Tab, flight_tab_class):
uiSetupReadySignal = pyqtSignal()
_motor_data_signal = pyqtSignal(int, object, object)
_imu_data_signal = pyqtSignal(int, object, object)
_baro_data_signal = pyqtSignal(int, object, object)
_input_updated_signal = pyqtSignal(float, float, float, float)
_rp_trim_updated_signal = pyqtSignal(float, float)
_emergency_stop_updated_signal = pyqtSignal(bool)
_assisted_control_updated_signal = pyqtSignal(bool)
_heighthold_input_updated_signal = pyqtSignal(float, float, float, float)
_hover_input_updated_signal = pyqtSignal(float, float, float, float)
_log_error_signal = pyqtSignal(object, str)
# UI_DATA_UPDATE_FPS = 10
connectionFinishedSignal = pyqtSignal(str)
disconnectedSignal = pyqtSignal(str)
_limiting_updated = pyqtSignal(bool, bool, bool)
def __init__(self, tabWidget, helper, *args):
super(FlightTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Flight Control"
self.menuName = "Flight Control"
self.tabWidget = tabWidget
self.helper = helper
self.isConnected = False
self.disconnectedSignal.connect(self.disconnected)
self.connectionFinishedSignal.connect(self.connected)
# Incomming signals
self.helper.cf.connected.add_callback(
self.connectionFinishedSignal.emit)
self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self._input_updated_signal.connect(self.updateInputControl)
self.helper.inputDeviceReader.input_updated.add_callback(
self._input_updated_signal.emit)
self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
self.helper.inputDeviceReader.rp_trim_updated.add_callback(
self._rp_trim_updated_signal.emit)
self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
self._emergency_stop_updated_signal.emit)
self.helper.inputDeviceReader.heighthold_input_updated.add_callback(
self._heighthold_input_updated_signal.emit)
self._heighthold_input_updated_signal.connect(
self._heighthold_input_updated)
self.helper.inputDeviceReader.hover_input_updated.add_callback(
self._hover_input_updated_signal.emit)
self._hover_input_updated_signal.connect(self._hover_input_updated)
self.helper.inputDeviceReader.assisted_control_updated.add_callback(
self._assisted_control_updated_signal.emit)
self._assisted_control_updated_signal.connect(
self._assisted_control_updated)
self._imu_data_signal.connect(self._imu_data_received)
self._baro_data_signal.connect(self._baro_data_received)
self._motor_data_signal.connect(self._motor_data_received)
self._log_error_signal.connect(self._logging_error)
# Connect UI signals that are in this tab
self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
self.thrustLoweringSlewRateLimit.valueChanged.connect(
self.thrustLoweringSlewRateLimitChanged)
self.slewEnableLimit.valueChanged.connect(
self.thrustLoweringSlewRateLimitChanged)
self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
self.maxAngle.valueChanged.connect(self.maxAngleChanged)
self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
self.uiSetupReadySignal.connect(self.uiSetupReady)
self.clientXModeCheckbox.toggled.connect(self.changeXmode)
self.isInCrazyFlightmode = False
self.uiSetupReady()
self.clientXModeCheckbox.setChecked(Config().get("client_side_xmode"))
self.crazyflieXModeCheckbox.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"flightmode.x", str(enabled)))
self.helper.cf.param.add_update_callback(
group="flightmode",
name="xmode",
cb=(lambda name, checked: self.crazyflieXModeCheckbox.setChecked(
eval(checked))))
self.ratePidRadioButton.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"flightmode.ratepid", str(enabled)))
self.angularPidRadioButton.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"flightmode.ratepid", str(not enabled)))
self._led_ring_headlight.clicked.connect(
lambda enabled: self.helper.cf.param.set_value(
"ring.headlightEnable", str(enabled)))
self.helper.cf.param.add_update_callback(
group="flightmode",
name="ratepid",
cb=(lambda name, checked: self.ratePidRadioButton.setChecked(
eval(checked))))
self.helper.cf.param.add_update_callback(
group="cpu", name="flash", cb=self._set_enable_client_xmode)
self.helper.cf.param.add_update_callback(
group="ring",
name="headlightEnable",
cb=(lambda name, checked: self._led_ring_headlight.setChecked(
eval(checked))))
self._ledring_nbr_effects = 0
self.helper.cf.param.add_update_callback(group="ring",
name="effect",
cb=self._ring_effect_updated)
self.helper.cf.param.add_update_callback(
group="imu_sensors", cb=self._set_available_sensors)
self.helper.cf.param.all_updated.add_callback(self._all_params_updated)
self.logBaro = None
self.logAltHold = None
self.ai = AttitudeIndicator()
self.verticalAttitudeIndicator.addWidget(self.ai)
self.splitter.setSizes([1000, 1])
self.targetCalPitch.setValue(Config().get("trim_pitch"))
self.targetCalRoll.setValue(Config().get("trim_roll"))
self.helper.inputDeviceReader.alt1_updated.add_callback(
self.alt1_updated)
self.helper.inputDeviceReader.alt2_updated.add_callback(
self.alt2_updated)
self._tf_state = 0
self._ring_effect = 0
# Connect callbacks for input device limiting of rpöö/pitch/yaw/thust
self.helper.inputDeviceReader.limiting_updated.add_callback(
self._limiting_updated.emit)
self._limiting_updated.connect(self._set_limiting_enabled)
self.threads = []
self.goproCamera = None
self.fvs = None
self.fps = None
self.stop = False
self.video_window.setVisible(False)
self.checkEnableGoPro.stateChanged.connect(self.click_enable_gopro)
def click_enable_gopro(self, state):
if self.checkEnableGoPro.isChecked():
self.checkEnableGoPro.setText(
"EnableGopro. Trying to connect GoPro.")
self.stop = False
self.gopro_connect()
else:
self.checkEnableGoPro.setText("EnableGopro")
self.stop = True
self.video_window.setVisible(False)
self.gopro_disconnect()
def start_stream(self):
self.goproCamera.livestream("stop")
self.goproCamera.stream("udp://@127.0.0.1:10000", quality="high")
def stop_stream(self):
self.goproCamera.livestream("stop")
def stop_threads(self):
while len(self.threads) > 0:
for thread in self.threads:
if thread.is_alive():
thread.join()
self.threads.remove(thread)
def play_stream(self):
self.fvs = FileVideoStream('udp://@127.0.0.1:10000').start()
self.fps = FPS().start()
while self.fvs.running():
if self.stop:
break
try:
frame = self.fvs.read()
frame = imutils.resize(frame, width=self.video_window.width())
#frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
height, width, channel = frame.shape
bytesPerLine = 3 * width
qimg = QImage(frame.data, width, height, bytesPerLine,
QImage.Format_RGB888).rgbSwapped()
pixmap = QPixmap(qimg)
if not self.video_window.isVisible():
self.checkEnableGoPro.setText("EnableGopro")
self.video_window.setVisible(True)
self.video_window.setPixmap(pixmap)
except Exception as exc:
pass
if self.fvs.Q.qsize(
) < 2: # If we are low on frames, give time to producer
time.sleep(
0.001) # Ensures producer runs now, so 2 is sufficient
self.fps.update()
QApplication.processEvents()
self.fps.stop()
self.fvs.stop()
cv2.destroyAllWindows()
def gopro_connect(self):
stream_stop_threads = Thread(target=self.stop_threads, args=())
stream_stop_threads.start()
if not self.goproCamera:
self.goproCamera = GoProCamera.GoPro(constants.gpcontrol)
stream_start_threads = Thread(target=self.start_stream, args=())
stream_play_thread = Thread(target=self.play_stream, args=())
self.threads.append(stream_start_threads)
self.threads.append(stream_play_thread)
stream_start_threads.start()
stream_play_thread.start()
def gopro_disconnect(self):
self.stop_stream()
stream_stop_threads = Thread(target=self.stop_threads, args=())
stream_stop_threads.start()
self.goproCamera = None
def _set_enable_client_xmode(self, name, value):
if eval(value) <= 128:
self.clientXModeCheckbox.setEnabled(True)
else:
self.clientXModeCheckbox.setEnabled(False)
self.clientXModeCheckbox.setChecked(False)
def _set_limiting_enabled(self, rp_limiting_enabled, yaw_limiting_enabled,
thrust_limiting_enabled):
self.maxAngle.setEnabled(rp_limiting_enabled)
self.targetCalRoll.setEnabled(rp_limiting_enabled)
self.targetCalPitch.setEnabled(rp_limiting_enabled)
self.maxYawRate.setEnabled(yaw_limiting_enabled)
self.maxThrust.setEnabled(thrust_limiting_enabled)
self.minThrust.setEnabled(thrust_limiting_enabled)
self.slewEnableLimit.setEnabled(thrust_limiting_enabled)
self.thrustLoweringSlewRateLimit.setEnabled(thrust_limiting_enabled)
def thrustToPercentage(self, thrust):
return ((thrust / MAX_THRUST) * 100.0)
def uiSetupReady(self):
flightComboIndex = self.flightModeCombo.findText(
Config().get("flightmode"), Qt.MatchFixedString)
if (flightComboIndex < 0):
self.flightModeCombo.setCurrentIndex(0)
self.flightModeCombo.currentIndexChanged.emit(0)
else:
self.flightModeCombo.setCurrentIndex(flightComboIndex)
self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)
def _logging_error(self, log_conf, msg):
QMessageBox.about(
self, "Log error",
"Error when starting log config [%s]: %s" % (log_conf.name, msg))
def _motor_data_received(self, timestamp, data, logconf):
if self.isVisible():
self.actualM1.setValue(data["motor.m1"])
self.actualM2.setValue(data["motor.m2"])
self.actualM3.setValue(data["motor.m3"])
self.actualM4.setValue(data["motor.m4"])
def _baro_data_received(self, timestamp, data, logconf):
if self.isVisible():
estimated_z = data[LOG_NAME_ESTIMATED_Z]
asl = data["baro.asl"]
temp = data["baro.temp"]
pressure = data["baro.pressure"]
self.actualHeight.setText(("%.2f" % estimated_z))
self.ai.setBaro(estimated_z, self.is_visible())
self.ai.setAsl(asl, self.is_visible())
self.ai.setTemp(temp, self.is_visible())
self.ai.setPressure(pressure, self.is_visible())
def _heighthold_input_updated(self, roll, pitch, yaw, height):
if (self.isVisible() and
(self.helper.inputDeviceReader.get_assisted_control()
== self.helper.inputDeviceReader.ASSISTED_CONTROL_HEIGHTHOLD)):
self.targetRoll.setText(("%0.2f deg" % roll))
self.targetPitch.setText(("%0.2f deg" % pitch))
self.targetYaw.setText(("%0.2f deg/s" % yaw))
self.targetHeight.setText(("%.2f m" % height))
self.ai.setHover(height, self.is_visible())
def _hover_input_updated(self, vx, vy, yaw, height):
if (self.isVisible()
and (self.helper.inputDeviceReader.get_assisted_control()
== self.helper.inputDeviceReader.ASSISTED_CONTROL_HOVER)):
self.targetRoll.setText(("%0.2f m/s" % vy))
self.targetPitch.setText(("%0.2f m/s" % vx))
self.targetYaw.setText(("%0.2f deg/s" % yaw))
self.targetHeight.setText(("%.2f m" % height))
self.ai.setHover(height, self.is_visible())
def _imu_data_received(self, timestamp, data, logconf):
if self.isVisible():
self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
self.actualThrust.setText(
"%.2f%%" % self.thrustToPercentage(data["stabilizer.thrust"]))
self.ai.setRollPitch(-data["stabilizer.roll"],
data["stabilizer.pitch"], self.is_visible())
def connected(self, linkURI):
# IMU & THRUST
self.isConnected = True
lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
lg.add_variable("stabilizer.roll", "float")
lg.add_variable("stabilizer.pitch", "float")
lg.add_variable("stabilizer.yaw", "float")
lg.add_variable("stabilizer.thrust", "uint16_t")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
# MOTOR
lg = LogConfig("Motors", Config().get("ui_update_period"))
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
self.actualHeight.setEnabled(True)
self.helper.inputDeviceReader.set_alt_hold_available(available)
if not self.logBaro:
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable(LOG_NAME_ESTIMATED_Z, "float")
self.logBaro.add_variable("baro.asl", "float")
self.logBaro.add_variable("baro.temp", "float")
self.logBaro.add_variable("baro.pressure", "float")
try:
self.helper.cf.log.add_config(self.logBaro)
self.logBaro.data_received_cb.add_callback(
self._baro_data_signal.emit)
self.logBaro.error_cb.add_callback(self._log_error_signal.emit)
self.logBaro.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def disconnected(self, linkURI):
self.isConnected = False
self.ai.setRollPitch(0, 0)
self.actualM1.setValue(0)
self.actualM2.setValue(0)
self.actualM3.setValue(0)
self.actualM4.setValue(0)
self.actualRoll.setText("")
self.actualPitch.setText("")
self.actualYaw.setText("")
self.actualThrust.setText("")
self.actualHeight.setText("")
self.targetHeight.setText("Not Set")
self.ai.setHover(0, self.is_visible())
self.targetHeight.setEnabled(False)
self.actualHeight.setEnabled(False)
self.clientXModeCheckbox.setEnabled(False)
self.logBaro = None
self.logAltHold = None
self._led_ring_effect.setEnabled(False)
self._led_ring_effect.clear()
try:
self._led_ring_effect.currentIndexChanged.disconnect(
self._ring_effect_changed)
except TypeError:
# Signal was not connected
pass
self._led_ring_effect.setCurrentIndex(-1)
self._led_ring_headlight.setEnabled(False)
try:
self._assist_mode_combo.currentIndexChanged.disconnect(
self._assist_mode_changed)
except TypeError:
# Signal was not connected
pass
self._assist_mode_combo.setEnabled(False)
self._assist_mode_combo.clear()
def minMaxThrustChanged(self):
self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
if (self.isInCrazyFlightmode is True):
Config().set("min_thrust", self.minThrust.value())
Config().set("max_thrust", self.maxThrust.value())
def thrustLoweringSlewRateLimitChanged(self):
self.helper.inputDeviceReader.thrust_slew_rate = (
self.thrustLoweringSlewRateLimit.value())
self.helper.inputDeviceReader.thrust_slew_limit = (
self.slewEnableLimit.value())
if (self.isInCrazyFlightmode is True):
Config().set("slew_limit", self.slewEnableLimit.value())
Config().set("slew_rate", self.thrustLoweringSlewRateLimit.value())
def maxYawRateChanged(self):
logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
self.helper.inputDeviceReader.max_yaw_rate = self.maxYawRate.value()
if (self.isInCrazyFlightmode is True):
Config().set("max_yaw", self.maxYawRate.value())
def maxAngleChanged(self):
logger.debug("MaxAngle changed to %d", self.maxAngle.value())
self.helper.inputDeviceReader.max_rp_angle = self.maxAngle.value()
if (self.isInCrazyFlightmode is True):
Config().set("max_rp", self.maxAngle.value())
def _trim_pitch_changed(self, value):
logger.debug("Pitch trim updated to [%f]" % value)
self.helper.inputDeviceReader.trim_pitch = value
Config().set("trim_pitch", value)
def _trim_roll_changed(self, value):
logger.debug("Roll trim updated to [%f]" % value)
self.helper.inputDeviceReader.trim_roll = value
Config().set("trim_roll", value)
def calUpdateFromInput(self, rollCal, pitchCal):
logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
rollCal, pitchCal)
self.targetCalRoll.setValue(rollCal)
self.targetCalPitch.setValue(pitchCal)
def updateInputControl(self, roll, pitch, yaw, thrust):
self.targetRoll.setText(("%0.2f deg" % roll))
self.targetPitch.setText(("%0.2f deg" % pitch))
self.targetYaw.setText(("%0.2f deg/s" % yaw))
self.targetThrust.setText(
("%0.2f %%" % self.thrustToPercentage(thrust)))
self.thrustProgress.setValue(thrust)
if not self.isConnected:
self.ai.setRollPitch(-roll, pitch, self.is_visible())
def setMotorLabelsEnabled(self, enabled):
self.M1label.setEnabled(enabled)
self.M2label.setEnabled(enabled)
self.M3label.setEnabled(enabled)
self.M4label.setEnabled(enabled)
def emergencyStopStringWithText(self, text):
return ("<html><head/><body><p>"
"<span style='font-weight:600; color:#7b0005;'>{}</span>"
"</p></body></html>".format(text))
def updateEmergencyStop(self, emergencyStop):
if emergencyStop:
self.setMotorLabelsEnabled(False)
self.emergency_stop_label.setText(
self.emergencyStopStringWithText("Kill switch active"))
else:
self.setMotorLabelsEnabled(True)
self.emergency_stop_label.setText("")
def flightmodeChange(self, item):
Config().set("flightmode", str(self.flightModeCombo.itemText(item)))
logger.debug("Changed flightmode to %s",
self.flightModeCombo.itemText(item))
self.isInCrazyFlightmode = False
if (item == 0): # Normal
self.maxAngle.setValue(Config().get("normal_max_rp"))
self.maxThrust.setValue(Config().get("normal_max_thrust"))
self.minThrust.setValue(Config().get("normal_min_thrust"))
self.slewEnableLimit.setValue(Config().get("normal_slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
Config().get("normal_slew_rate"))
self.maxYawRate.setValue(Config().get("normal_max_yaw"))
if (item == 1): # Advanced
self.maxAngle.setValue(Config().get("max_rp"))
self.maxThrust.setValue(Config().get("max_thrust"))
self.minThrust.setValue(Config().get("min_thrust"))
self.slewEnableLimit.setValue(Config().get("slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
Config().get("slew_rate"))
self.maxYawRate.setValue(Config().get("max_yaw"))
self.isInCrazyFlightmode = True
if (item == 0):
newState = False
else:
newState = True
self.maxThrust.setEnabled(newState)
self.maxAngle.setEnabled(newState)
self.minThrust.setEnabled(newState)
self.thrustLoweringSlewRateLimit.setEnabled(newState)
self.slewEnableLimit.setEnabled(newState)
self.maxYawRate.setEnabled(newState)
def _assist_mode_changed(self, item):
mode = None
if (item == 0): # Altitude hold
mode = JoystickReader.ASSISTED_CONTROL_ALTHOLD
if (item == 1): # Position hold
mode = JoystickReader.ASSISTED_CONTROL_POSHOLD
if (item == 2): # Position hold
mode = JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD
if (item == 3): # Position hold
mode = JoystickReader.ASSISTED_CONTROL_HOVER
self.helper.inputDeviceReader.set_assisted_control(mode)
Config().set("assistedControl", mode)
def _assisted_control_updated(self, enabled):
if self.helper.inputDeviceReader.get_assisted_control() == \
JoystickReader.ASSISTED_CONTROL_POSHOLD:
self.targetThrust.setEnabled(not enabled)
self.targetRoll.setEnabled(not enabled)
self.targetPitch.setEnabled(not enabled)
elif ((self.helper.inputDeviceReader.get_assisted_control()
== JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD)
or (self.helper.inputDeviceReader.get_assisted_control()
== JoystickReader.ASSISTED_CONTROL_HOVER)):
self.targetThrust.setEnabled(not enabled)
self.targetHeight.setEnabled(enabled)
else:
self.helper.cf.param.set_value("flightmode.althold", str(enabled))
@pyqtSlot(bool)
def changeXmode(self, checked):
self.helper.cf.commander.set_client_xmode(checked)
Config().set("client_side_xmode", checked)
logger.info("Clientside X-mode enabled: %s", checked)
def alt1_updated(self, state):
if state:
new_index = (self._ring_effect + 1) % (self._ledring_nbr_effects +
1)
self.helper.cf.param.set_value("ring.effect", str(new_index))
def alt2_updated(self, state):
self.helper.cf.param.set_value("ring.headlightEnable", str(state))
def _all_params_updated(self):
self._ring_populate_dropdown()
self._populate_assisted_mode_dropdown()
def _ring_populate_dropdown(self):
try:
nbr = int(self.helper.cf.param.values["ring"]["neffect"])
current = int(self.helper.cf.param.values["ring"]["effect"])
except KeyError:
return
# Used only in alt1_updated function
self._ring_effect = current
self._ledring_nbr_effects = nbr
hardcoded_names = {
0: "Off",
1: "White spinner",
2: "Color spinner",
3: "Tilt effect",
4: "Brightness effect",
5: "Color spinner 2",
6: "Double spinner",
7: "Solid color effect",
8: "Factory test",
9: "Battery status",
10: "Boat lights",
11: "Alert",
12: "Gravity",
13: "LED tab",
14: "Color fader",
15: "Link quality"
}
for i in range(nbr + 1):
name = "{}: ".format(i)
if i in hardcoded_names:
name += hardcoded_names[i]
else:
name += "N/A"
self._led_ring_effect.addItem(name, i)
self._led_ring_effect.currentIndexChanged.connect(
self._ring_effect_changed)
self._led_ring_effect.setCurrentIndex(current)
if int(self.helper.cf.param.values["deck"]["bcLedRing"]) == 1:
self._led_ring_effect.setEnabled(True)
self._led_ring_headlight.setEnabled(True)
def _ring_effect_changed(self, index):
self._ring_effect = index
if index > -1:
i = self._led_ring_effect.itemData(index)
logger.info("Changed effect to {}".format(i))
if i != int(self.helper.cf.param.values["ring"]["effect"]):
self.helper.cf.param.set_value("ring.effect", str(i))
def _ring_effect_updated(self, name, value):
if self.helper.cf.param.is_updated:
self._led_ring_effect.setCurrentIndex(int(value))
def _populate_assisted_mode_dropdown(self):
self._assist_mode_combo.addItem("Altitude hold", 0)
self._assist_mode_combo.addItem("Position hold", 1)
self._assist_mode_combo.addItem("Height hold", 2)
self._assist_mode_combo.addItem("Hover", 3)
heightHoldPossible = False
hoverPossible = False
if int(self.helper.cf.param.values["deck"]["bcZRanger"]) == 1:
heightHoldPossible = True
self.helper.inputDeviceReader.set_hover_max_height(1.0)
if int(self.helper.cf.param.values["deck"]["bcZRanger2"]) == 1:
heightHoldPossible = True
self.helper.inputDeviceReader.set_hover_max_height(2.0)
if int(self.helper.cf.param.values["deck"]["bcFlow"]) == 1:
heightHoldPossible = True
hoverPossible = True
self.helper.inputDeviceReader.set_hover_max_height(1.0)
if int(self.helper.cf.param.values["deck"]["bcFlow2"]) == 1:
heightHoldPossible = True
hoverPossible = True
self.helper.inputDeviceReader.set_hover_max_height(2.0)
if not heightHoldPossible:
self._assist_mode_combo.model().item(2).setEnabled(False)
else:
self._assist_mode_combo.model().item(0).setEnabled(False)
if not hoverPossible:
self._assist_mode_combo.model().item(3).setEnabled(False)
else:
self._assist_mode_combo.model().item(0).setEnabled(False)
self._assist_mode_combo.currentIndexChanged.connect(
self._assist_mode_changed)
self._assist_mode_combo.setEnabled(True)
try:
assistmodeComboIndex = Config().get("assistedControl")
if assistmodeComboIndex == 3 and not hoverPossible:
self._assist_mode_combo.setCurrentIndex(0)
self._assist_mode_combo.currentIndexChanged.emit(0)
elif assistmodeComboIndex == 0 and hoverPossible:
self._assist_mode_combo.setCurrentIndex(3)
self._assist_mode_combo.currentIndexChanged.emit(3)
elif assistmodeComboIndex == 2 and not heightHoldPossible:
self._assist_mode_combo.setCurrentIndex(0)
self._assist_mode_combo.currentIndexChanged.emit(0)
elif assistmodeComboIndex == 0 and heightHoldPossible:
self._assist_mode_combo.setCurrentIndex(2)
self._assist_mode_combo.currentIndexChanged.emit(2)
else:
self._assist_mode_combo.setCurrentIndex(assistmodeComboIndex)
self._assist_mode_combo.currentIndexChanged.emit(
assistmodeComboIndex)
except KeyError:
defaultOption = 0
if hoverPossible:
defaultOption = 3
elif heightHoldPossible:
defaultOption = 2
self._assist_mode_combo.setCurrentIndex(defaultOption)
self._assist_mode_combo.currentIndexChanged.emit(defaultOption)
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video", required=True,
help="path to input video file")
args = vars(ap.parse_args())
# start the file video stream thread and allow the buffer to
# start to fill
print("[INFO] starting video file thread...")
fvs = FileVideoStream(args["video"], transform=filterFrame).start()
time.sleep(1.0)
# start the FPS timer
fps = FPS().start()
# loop over frames from the video file stream
while fvs.running():
# grab the frame from the threaded video file stream, resize
# it, and convert it to grayscale (while still retaining 3
# channels)
frame = fvs.read()
# Relocated filtering into producer thread with transform=filterFrame
# Python 2.7: FPS 92.11 -> 131.36
# Python 3.7: FPS 41.44 -> 50.11
#frame = filterFrame(frame)
# display the size of the queue on the frame
cv2.putText(frame, "Queue Size: {}".format(fvs.Q.qsize()),
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
# show the frame and update the FPS counter
# vs = VideoStream(usePiCamera=True).start()
#fileStream = False
time.sleep(1.0)
x = []
i = 0
with open('EAR_orig_8.csv', 'a', newline='') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_ALL)
wr.writerow(['index', 'EAR'])
ear_prev = 0
# loop over frames from the video stream
while True:
# if this is a file video stream, then we need to check if
# there any more frames left in the buffer to process
if fileStream and (not vs.running()):
break
# grab the frame from the threaded video file stream, resize
# it, and convert it to grayscale
# channels)
frame = vs.read()
frame = imutils.resize(frame, width=720)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# detect faces in the grayscale frame
rects = detector(gray, 0)
fl = 0
# loop over the face detections
for rect in rects: