class MainClass(threading.Thread):
def __init__(self):
# Create the olympe.Drone object from its IP address
self.drone = olympe.Drone(DRONE_IP)
# subscribe to flight listener
listener = FlightListener(self.drone)
listener.subscribe()
self.last_frame = np.zeros((1, 1, 3), np.uint8)
self.frame_queue = queue.Queue()
self.flush_queue_lock = threading.Lock()
self.detector = Detector()
self.keypoints_image = np.zeros((1, 1, 3), np.uint8)
self.keypoints = deque(maxlen=5)
self.faces = deque(maxlen=10)
self.f = open("distances.csv", "w")
self.face_distances = deque(maxlen=10)
self.image_width = 1280
self.image_height = 720
self.half_face_detection_size = 150
self.poses_model = load("models/posesmodel.joblib")
self.pose_predictions = deque(maxlen=5)
self.pause_finding_condition = threading.Condition(threading.Lock())
self.pause_finding_condition.acquire()
self.pause_finding = True
self.person_thread = threading.Thread(target=self.fly_to_person)
self.person_thread.start()
# flight parameters in meter
self.flight_height = 0.0
self.max_height = 1.0
self.min_dist = 1.5
# keypoint map
self.nose = 0
self.left_eye = 1
self.right_eye = 2
self.left_ear = 3
self.right_ear = 4
self.left_shoulder = 5
self.right_shoulder = 6
self.left_elbow = 7
self.right_elbow = 8
self.left_wrist = 9
self.right_wrist = 10
self.left_hip = 11
self.right_hip = 12
self.left_knee = 13
self.right_knee = 14
self.left_ankle = 15
self.right_ankle = 16
# person distance
self.eye_dist = 0.0
# save images
self.save_image = False
self.image_counter = 243
self.pose_file = open("poses.csv", "w")
super().__init__()
super().start()
def start(self):
self.drone.connect()
# Setup your callback functions to do some live video processing
self.drone.set_streaming_callbacks(
raw_cb=self.yuv_frame_cb,
start_cb=self.start_cb,
end_cb=self.end_cb,
flush_raw_cb=self.flush_cb,
)
# Start video streaming
self.drone.start_video_streaming()
# set maximum speeds
print("rotation", self.drone(MaxRotationSpeed(1)).wait().success())
print("vertical", self.drone(MaxVerticalSpeed(0.1)).wait().success())
print("tilt", self.drone(MaxTilt(5)).wait().success())
def stop(self):
# Properly stop the video stream and disconnect
self.drone.stop_video_streaming()
self.drone.disconnect()
def yuv_frame_cb(self, yuv_frame):
"""
This function will be called by Olympe for each decoded YUV frame.
:type yuv_frame: olympe.VideoFrame
"""
yuv_frame.ref()
self.frame_queue.put_nowait(yuv_frame)
def flush_cb(self):
with self.flush_queue_lock:
while not self.frame_queue.empty():
self.frame_queue.get_nowait().unref()
return True
def start_cb(self):
pass
def end_cb(self):
pass
def show_yuv_frame(self, window_name, yuv_frame):
# the VideoFrame.info() dictionary contains some useful information
# such as the video resolution
info = yuv_frame.info()
height, width = info["yuv"]["height"], info["yuv"]["width"]
# yuv_frame.vmeta() returns a dictionary that contains additional
# metadata from the drone (GPS coordinates, battery percentage, ...)
# convert pdraw YUV flag to OpenCV YUV flag
cv2_cvt_color_flag = {
olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
}[info["yuv"]["format"]]
# yuv_frame.as_ndarray() is a 2D numpy array with the proper "shape"
# i.e (3 * height / 2, width) because it's a YUV I420 or NV12 frame
# Use OpenCV to convert the yuv frame to RGB
cv2frame = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)
# show video stream
cv2.imshow(window_name, cv2frame)
cv2.moveWindow(window_name, 0, 500)
# show other windows
self.show_face_detection(cv2frame)
self.show_keypoints()
cv2.waitKey(1)
def show_keypoints(self):
if len(self.keypoints) > 2:
# display eye distance
cv2.putText(
self.keypoints_image,
'Distance(eyes): ' + "{:.2f}".format(self.eye_dist) + "m",
(0, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (36, 255, 12), 2)
# display nose height
cv2.putText(
self.keypoints_image, 'Nose: ' + "{:.2f}".format(
get_point(np.average(self.keypoints, axis=0), self.nose)[1]
/ self.image_height), (0, 80), cv2.FONT_HERSHEY_SIMPLEX,
0.9, (36, 255, 12), 2)
cv2.imshow("keypoints", self.keypoints_image)
cv2.moveWindow("keypoints", 500, 0)
def show_face_detection(self, cv2frame):
# get sub image
img = self.get_face_detection_crop(cv2frame)
# get face rectangle
face, img = self.detector.detect_face(img)
if face.size > 0:
self.faces.append(face)
width = face[2] - face[0]
height = face[3] - face[1]
# get distances for rectangle width and height
width = get_distance_width(width)
height = get_distance_height(height)
# display distances
cv2.putText(img, 'width: ' + "{:.2f}".format(width), (0, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.9, (36, 255, 12), 2)
cv2.putText(img, 'height: ' + "{:.2f}".format(height), (0, 50),
cv2.FONT_HERSHEY_SIMPLEX, 0.9, (36, 255, 12), 2)
cv2.putText(
img,
'mean: ' + "{:.2f}".format(np.mean(np.array([width, height]))),
(0, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (36, 255, 12), 2)
# append outlier free distance to log
self.face_distances.append(self.get_face_distance())
elif len(self.faces) > 0:
# remove from dequeue if no face was detected
self.faces.popleft()
# show detection
cv2.imshow("face", img)
cv2.moveWindow("face", 0, 0)
# get 300*300 crop of frame based on nose location or from the middle
def get_face_detection_crop(self, cv2frame):
if len(self.keypoints) > 0:
x, y = get_point(
np.array(self.keypoints, dtype=object)[-1], self.nose)
else:
x = cv2frame.shape[1] / 2
y = cv2frame.shape[0] / 2
x = max(self.half_face_detection_size, x)
y = max(self.half_face_detection_size, y)
x = min(cv2frame.shape[1] - self.half_face_detection_size, x)
y = min(cv2frame.shape[0] - self.half_face_detection_size, y)
img = cv2frame[int(y - self.half_face_detection_size
):int(y + self.half_face_detection_size),
int(x - self.half_face_detection_size
):int(x + self.half_face_detection_size)]
return img
def get_face_distance(self):
if len(self.faces) > 2:
try:
faces = remove_outliers(np.array(self.faces, dtype=object))
face = np.mean(faces, axis=0)
width = face[2] - face[0]
height = face[3] - face[1]
width = get_distance_width(width)
height = get_distance_height(height)
return np.mean(np.array([width, height]))
except ZeroDivisionError:
logging.error("ZeroDivisionError in get_face_distance()")
logging.error(self.faces)
return -1
def run(self):
window_name = "videostream"
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
main_thread = next(
filter(lambda t: t.name == "MainThread", threading.enumerate()))
while main_thread.is_alive():
with self.flush_queue_lock:
try:
yuv_frame = self.frame_queue.get(timeout=0.01)
except queue.Empty:
continue
try:
# the VideoFrame.info() dictionary contains some useful information
# such as the video resolution
info = yuv_frame.info()
height, width = info["yuv"]["height"], info["yuv"]["width"]
# yuv_frame.vmeta() returns a dictionary that contains additional
# metadata from the drone (GPS coordinates, battery percentage, ...)
# convert pdraw YUV flag to OpenCV YUV flag
cv2_cvt_color_flag = {
olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
}[info["yuv"]["format"]]
# yuv_frame.as_ndarray() is a 2D numpy array with the proper "shape"
# i.e (3 * height / 2, width) because it's a YUV I420 or NV12 frame
# Use OpenCV to convert the yuv frame to RGB
cv2frame = cv2.cvtColor(yuv_frame.as_ndarray(),
cv2_cvt_color_flag)
self.last_frame = cv2frame
self.show_yuv_frame(window_name, yuv_frame)
except Exception:
# We have to continue popping frame from the queue even if
# we fail to show one frame
traceback.print_exc()
finally:
# Don't forget to unref the yuv frame. We don't want to
# starve the video buffer pool
yuv_frame.unref()
cv2.destroyWindow(window_name)
def command_window_thread(self, win):
win.nodelay(True)
key = ""
win.clear()
win.addstr("Detected key:")
while 1:
try:
key = win.getkey()
win.clear()
win.addstr("Detected key:")
win.addstr(str(key))
# disconnect drone
if str(key) == "c":
win.clear()
win.addstr("c, stopping")
self.f.close()
self.pose_file.close()
self.drone.disconnect()
# takeoff
if str(key) == "t":
win.clear()
win.addstr("takeoff")
assert self.drone(TakeOff()).wait().success()
win.addstr("completed")
# land
if str(key) == "l":
win.clear()
win.addstr("landing")
assert self.drone(Landing()).wait().success()
# turn left
if str(key) == "q":
win.clear()
win.addstr("turning left")
assert self.drone(
moveBy(0, 0, 0, -math.pi / 4) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
win.addstr("completed")
# turn right
if str(key) == "e":
win.clear()
win.addstr("turning right")
assert self.drone(
moveBy(0, 0, 0, math.pi / 4) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
win.addstr("completed")
# move front
if str(key) == "w":
win.clear()
win.addstr("front")
assert self.drone(
moveBy(0.2, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
win.addstr("completed")
# move back
if str(key) == "s":
win.clear()
win.addstr("back")
assert self.drone(
moveBy(-0.2, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
win.addstr("completed")
# move up
if str(key) == "r":
win.clear()
win.addstr("up")
assert self.drone(
moveBy(0, 0, -0.15, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
win.addstr("completed")
# move down
if str(key) == "f":
win.clear()
win.addstr("down")
assert self.drone(
moveBy(0, 0, 0.15, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
win.addstr("completed")
# move left
if str(key) == "a":
win.clear()
win.addstr("left")
assert self.drone(
moveBy(0, -0.2, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
win.addstr("completed")
# move right
if str(key) == "d":
win.clear()
win.addstr("right")
assert self.drone(
moveBy(0, 0.2, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
win.addstr("completed")
# start person detection thread
if str(key) == "p":
win.clear()
pause = self.check_for_pause()
if pause:
win.addstr("cannot start because of stop gesture")
else:
win.addstr("start detecting")
self.pause_finding = False
self.pause_finding_condition.notify()
self.pause_finding_condition.release()
# pause person detecting thread
if str(key) == "o":
win.clear()
win.addstr("stop detecting")
self.pause_finding = True
self.pause_finding_condition.acquire()
# self.person_thread.stop = True
# win.addstr("joined")
# measure distances
if str(key) == "x":
win.clear()
win.addstr("distances:")
arr = np.array(self.keypoints, dtype=object)
string = ""
for i in range(arr.shape[0]):
string += "{:.6f};".format(
get_point_distance(arr[i], self.left_eye,
self.right_eye))
win.addstr(string)
self.f.write(string + "\n")
# measure faces
if str(key) == "y":
win.clear()
win.addstr("distances:")
arr = np.array(self.faces, dtype=object)
width = ""
height = ""
for i in range(arr.shape[0]):
width += str(arr[i][2] - arr[i][0]) + ";"
height += str(arr[i][3] - arr[i][1]) + ";"
win.addstr(width + height)
self.f.write(width + "\n")
self.f.write(height + "\n")
# log user gesture
if str(key) == "g":
win.clear()
win.addstr("gesture made")
global event_time
event_time = time.time()
logging.info("stop gesture by user;{:.3f};{:.3f}".format(
self.get_face_distance(), time.time()))
# log face distances
if str(key) == "k":
win.clear()
win.addstr("distances logged")
string = ""
arr = np.array(self.face_distances, dtype=object)
win.addstr(str(len(arr)))
for i in range(len(arr)):
string += "{:.2f}".format(arr[i]) + ";"
logging.info("distances;{}".format(string))
win.addstr(string)
except Exception as e:
# No input
pass
def fly_to_person(self):
t = threading.currentThread()
while not getattr(t, "stop", False):
with self.pause_finding_condition:
# wait if thread is paused
while self.pause_finding:
self.pause_finding_condition.wait()
arr = np.array(self.keypoints, dtype=object)
if len(arr) > 2:
pose_predictions = np.array(self.pose_predictions,
dtype=object)
if pose_predictions[-1] > 1:
logging.info(
"stop gesture {} detected;{:.3f};{:.3f}".format(
pose_predictions[-1], self.get_face_distance(),
time.time() - event_time))
# check if multiple stop gestures were detected
pause = self.check_for_pause()
if pause:
logging.info(
f"stopping completely gesture {pose_predictions[-1]};{self.get_face_distance()};{time.time() - event_time}"
)
# land drone
assert self.drone(Landing()).wait().success()
self.pause_finding = True
self.pause_finding_condition.acquire()
time.sleep(0.2)
continue
distance = self.get_face_distance()
xn, yn = get_point(np.average(arr[-2:], axis=0), self.nose)
# calculate angle of nose
angle = (xn / self.image_width - 0.5) * 1.204
# calculate nose height in percent
nose_height = yn / self.image_height
# set nose to middle if none was detected
if nose_height == 0:
nose_height = 0.5
# adjust angle
if np.abs(angle) > 0.15:
assert self.drone(
moveBy(0, 0, 0, angle) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
# adjust height
elif nose_height < 0.4 and self.flight_height < self.max_height:
self.flight_height += 0.15
assert self.drone(
moveBy(0.0, 0, -0.15, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
time.sleep(0.4)
elif nose_height > 0.6 and self.flight_height > 0:
self.flight_height -= 0.15
assert self.drone(
moveBy(0.0, 0, 0.15, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
time.sleep(0.4)
# adjust distance
elif distance > self.min_dist:
assert self.drone(
moveBy(min(0.2, distance - self.min_dist), 0, 0, 0) >>
FlyingStateChanged(state="hovering", _timeout=5)).wait(
).success()
else:
assert self.drone(
moveBy(0, 0, 0, 0.3) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
# returns true if 4 of the 5 last pose predictions were stop
def check_for_pause(self):
pose_predictions = np.array(self.pose_predictions, dtype=object)
prediction_counts = np.asarray(
(np.unique(pose_predictions, return_counts=True)), dtype=object).T
for i in range(prediction_counts.shape[0]):
if prediction_counts[i, 0] > 1 and prediction_counts[i, 1] > 3:
return True
return False
# thread for keypoint predictions
def make_predictions(self):
while 1:
# check if camera stream has started
if not np.array_equal(self.last_frame, np.zeros(
(1, 1, 3), np.uint8)):
# get detections
start = time.time()
keypoint, self.keypoints_image = self.detector.keypoint_detection(
self.last_frame)
logging.info(
"time for prediction = {:0.3f}".format(time.time() -
start))
# check if detection returned results
if keypoint.size > 2:
self.keypoints.append(keypoint)
pred = self.poses_model.predict(keypoint[0].reshape(1, -1))
self.pose_predictions.append(int(pred[0]))
if pred > 1:
# stop moving if prediction is stop
logging.info("canceling move to;{:.3f};{:.3f}".format(
self.get_face_distance(),
time.time() - event_time))
self.drone(CancelMoveBy()).wait()
# put prediction on image
cv2.putText(self.keypoints_image,
'Classpred: ' + "{:.0f}".format(pred[0]),
(0, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.9,
(36, 255, 12), 2)
if self.save_image:
cv2.imwrite(
"images/" + str(self.image_counter) + ".jpg",
self.keypoints_image)
string = str(self.image_counter) + ";"
self.image_counter += 1
for i in range(keypoint.shape[1]):
string += "{:.2f};{:.2f};".format(
keypoint[0, i, 0], keypoint[0, i, 1])
self.pose_file.write(string + "\n")
# delete from last results if there is no detection
elif len(self.keypoints) > 0:
self.keypoints.popleft()
self.pose_predictions.popleft()
if len(self.keypoints) > 1 and self.keypoints[-1].size > 2:
# arr = np.array(self.keypoints, dtype=object)
# left_eye = remove_outliers(arr[:, 0, self.left_eye])
# if len(np.shape(left_eye)) > 1 and np.shape(left_eye)[0] > 1:
# left_eye = np.average(left_eye, axis=0)
# right_eye = remove_outliers(arr[:, 0, self.right_eye])
# if len(np.shape(right_eye)) > 1 and np.shape(right_eye)[0] > 1:
# right_eye = np.average(right_eye, axis=0)
# left_eye = left_eye.reshape((-1))
# right_eye = right_eye.reshape((-1))
# # eye_dist = 24.27 / np.max(
# # [0.001, np.power(get_distance(np.average(arr[-2:], axis=0), self.left_eye, self.right_eye),
# # 0.753)])
# if len(left_eye) > 1 and len(right_eye) > 1:
# self.eye_dist = 24.27 / np.max(
# [0.001,
# np.power(math.sqrt((left_eye[0] - right_eye[0]) ** 2 + (left_eye[1] - right_eye[1]) ** 2),
# 0.753)])
# display face distance
cv2.putText(
self.keypoints_image, 'Distance(face): ' +
"{:.2f}".format(self.get_face_distance()) + "m",
(0, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (36, 255, 12),
2)
else:
# wait for stream
time.sleep(1)
