def with_lib():
d = libardrone.ARDrone()
d.takeoff()
d.hover()
sleep(6)
d.move_forward()
sleep(3)
d.land()
d.halt()
def start(self):
self.drone = libardrone.ARDrone(True)
self.drone.reset()
if self.manuel:
try:
self.mutex = Lock()
t1 = Thread(target=self.getKeyInput, args=())
t2 = Thread(target=self.getVideoStream, args=())
t3 = Thread(target=self.getBoundingBoxes, args=())
t1.start()
t2.start()
t3.start()
t1.join()
t2.join()
t3.join()
except:
print "Error: unable to start thread"
else:
try:
self.mutex = Lock()
t1 = Thread(target=self.autonomousFlight,
args=(
448,
448,
98,
0.1,
self.labels,
))
t2 = Thread(target=self.getVideoStream, args=())
t3 = Thread(target=self.getBoundingBoxes, args=())
t1.start()
t2.start()
t3.start()
t1.join()
t2.join()
t3.join()
except:
print "Error: unable to start thread"
print("Shutting down...")
cv2.destroyAllWindows()
self.drone.land()
time.sleep(0.1)
self.drone.halt()
print("Ok.")
def main():
logger.info('Starting up')
drone = libardrone.ARDrone(True)
drone.reset()
pygame.init()
running = True
pid = PIDControllerExecutor(drone)
pipeline = Pipeline([BatteryOverlay(drone),
FaceDetector(),
FaceClassifier(),
TrackSanitizer(),
pid])
while running:
display_cv(drone, pipeline)
running = handle_events(drone, pid)
logger.info('Shutting down...')
drone.halt()
def main():
HUE_LOWERB = 40
HUE_UPPERB = 100
SAT_LOWERB = 50
SAT_UPPERB = 170
BRI_LOWERB = 50
BRI_UPPERB = 255
EROSION = 3
DILATION = 4
FOLLOW_WIDTH = 50
DRONE_SPEED = 10 / 100.
CIRCLE_SPEED = 10 / 100.
TURN_SPEED = 50 / 100.
HFUZZ = 100
VFUZZ = 100
testing = False
if len(sys.argv) > 1:
if sys.argv[1] == 'testing':
testing = True
elif sys.argv[1] == 'control':
control = True
pygame.init()
print "initialized"
screen = pygame.display.set_mode((320, 240))
navdata_0 = None
if not testing:
drone = libardrone.ARDrone(True)
drone.reset()
navdata_0 = drone.navdata.copy()
tracking_center = False
k2d = Kalman2D(1, 0.0001, 0.1)
estimated = False
namedWindow("output", 1)
namedWindow("threshold", 1)
moveWindow("threshold", 1000, 0)
namedWindow("control", WINDOW_NORMAL)
resizeWindow("control", 1024, 300)
moveWindow("control", 0, 1000)
addControl("hue lower", HUE_LOWERB, 255)
addControl("hue upper", HUE_UPPERB, 255)
addControl("sat lower", SAT_LOWERB, 255)
addControl("sat upper", SAT_UPPERB, 255)
addControl("bri lower", BRI_LOWERB, 255)
addControl(
"bri upper",
BRI_UPPERB,
255,
)
addControl("erosion", EROSION, 10)
addControl("dilation", DILATION, 10)
addControl("dilation", DILATION, 10)
addControl("follow width", FOLLOW_WIDTH, 100)
addControl("drone speed", int(DRONE_SPEED * 100), 100)
addControl("turn speed", int(TURN_SPEED * 100), 100)
addControl("circle speed", int(CIRCLE_SPEED * 100), 100)
if testing:
capture = VideoCapture(0)
while (not capture.isOpened()):
print "not opened"
running = True
while running:
HUE_LOWERB = getTrackbarPos("hue lower", "control")
HUE_UPPERB = getTrackbarPos("hue upper", "control")
SAT_LOWERB = getTrackbarPos("sat lower", "control")
SAT_UPPERB = getTrackbarPos("sat upper", "control")
BRI_LOWERB = getTrackbarPos("bri lower", "control")
BRI_UPPERB = getTrackbarPos("bri upper", "control")
EROSION = getTrackbarPos("erosion", "control")
DILATION = getTrackbarPos("dilation", "control")
FOLLOW_WIDTH = getTrackbarPos("follow width", "control")
DRONE_SPEED = getTrackbarPos("drone speed", "control") / 100.
CIRCLE_SPEED = getTrackbarPos("circle speed", "control") / 100.
TURN_SPEED = getTrackbarPos("turn speed", "control") / 100.
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYUP:
drone.hover()
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
drone.reset()
running = False
# takeoff / land
elif event.key == pygame.K_RETURN:
print("take off")
drone.takeoff()
elif event.key == pygame.K_SPACE:
print("land")
drone.land()
# emergency
elif event.key == pygame.K_BACKSPACE:
drone.reset()
# forward / backward
elif event.key == pygame.K_w:
drone.move_forward()
elif event.key == pygame.K_s:
drone.move_backward()
# left / right
elif event.key == pygame.K_a:
drone.move_left()
elif event.key == pygame.K_d:
drone.move_right()
# up / down
elif event.key == pygame.K_UP:
drone.move_up()
elif event.key == pygame.K_DOWN:
drone.move_down()
# turn left / turn right
elif event.key == pygame.K_LEFT:
drone.turn_left()
elif event.key == pygame.K_RIGHT:
drone.turn_right()
elif event.key == pygame.K_p:
psi_i = drone.navdata['psi']
while (drone.navdata['psi'] - psi_i) % 360 < 90:
drone.turn_right()
if testing:
ret, frame = capture.read()
frame = flip(frame, 1)
else:
frame = drone.get_image()
frame = cvtColor(frame, COLOR_RGB2BGR)
if frame != None:
processed_frame = cvtColor(frame, COLOR_BGR2HSV)
processed_frame = inRange(processed_frame,
(HUE_LOWERB, SAT_LOWERB, BRI_LOWERB),
(HUE_UPPERB, SAT_UPPERB, BRI_UPPERB))
kernel = np.ones((5, 5))
processed_frame = GaussianBlur(processed_frame, (5, 5), 0, 0)
processed_frame = erode(processed_frame,
kernel,
iterations=EROSION)
processed_frame = dilate(processed_frame,
kernel,
iterations=DILATION)
imshow("threshold", processed_frame)
contours = findContours(processed_frame, RETR_EXTERNAL,
CHAIN_APPROX_SIMPLE, (0, 0))[0]
center = (frame.shape[1] / 2, frame.shape[0] / 2)
hat = (center[0] - 1, center[1] - 1, 2, 2)
hat_distance = 90000
for contour in contours:
(x, y, w, h) = boundingRect(contour)
distance = np.linalg.norm(
np.array((x + w / 2, y + h / 2)) - estimated)
if distance < hat_distance:
hat = (x, y, w, h)
hat_distance = distance
(x1, y1, w, h) = hat
(x2, y2) = (x1 + w, y1 + h)
k2d.update(x1 + w / 2, y1 + h / 2)
estimated = [int(c) for c in k2d.getEstimate()]
tracking_center = estimated
circle(frame, (estimated[0], estimated[1]), 4, (0, 255, 0))
rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0))
action = ""
if not (testing or control):
drone.speed = DRONE_SPEED
if (center[1] + VFUZZ < tracking_center[1]):
action += "move down "
drone.move_down()
elif (center[1] - VFUZZ > tracking_center[1]):
action += "move up "
drone.move_up()
elif (center[0] + HFUZZ < tracking_center[0]):
action += "turn right "
drone.speed = TURN_SPEED
drone.turn_right()
drone.speed = DRONE_SPEED
elif (center[0] - HFUZZ > tracking_center[0]):
action += "turn left "
drone.speed = TURN_SPEED
drone.turn_left()
drone.speed = DRONE_SPEED
elif w < FOLLOW_WIDTH and w > 25:
action += "move forward "
drone.move_forward()
elif w > 2 * FOLLOW_WIDTH:
action += "move backward "
drone.move_backward()
elif w > 25:
action += "circle"
drone.speed = CIRCLE_SPEED
drone.move_right()
drone.speed = DRONE_SPEED
else:
action += "hover"
drone.turn_left()
putText(frame, action, (0, 20), FONT_HERSHEY_SIMPLEX, 1.0,
(0, 255, 0), 2)
try:
putText(frame,
str(drone.navdata.get(0, dict())["battery"]),
(0, 50), FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
except KeyError:
pass
imshow("output", frame)
else:
print "frame is none"
print("Shutting down...")
drone.halt()
print("Ok.")
import cv2 as cv
import libardrone.libardrone as lib_drone
import numpy as np
import asci_keys as keys
import time
import os
drone = lib_drone.ARDrone(True)
delay_time = int(40) # [ms]
img_manuals = cv.imread(os.path.join("..", "media", "commands.png"))
running = True
cv.imshow("Control window", img_manuals)
flying = False
while running:
key = cv.waitKey(delay_time)
if key in [keys.a, keys.A]:
drone.move_left()
elif key in [keys.d, keys.D]:
drone.move_right()
elif key in [keys.w, keys.W]:
drone.move_forward()
elif key in [keys.s, keys.S]:
drone.move_backward()
elif key in [keys.i, keys.I]:
drone.move_up()
elif key in [keys.m, keys.M]:
drone.move_down()
elif key in [keys.j, keys.J]:
drone.turn_left()
def main():
testing = False
if len(sys.argv) > 1 and sys.argv[1] == 'testing':
testing = True
pygame.init()
drone = libardrone.ARDrone(True)
drone.reset()
navdata_0 = drone.navdata.copy()
calibrated = False
control_start = False
controlled = False
tracking_start = False
tracking = False
tracking_center = False
k2d = Kalman2D(1, 0.0001, 0.1)
estimated = False
armband = Armband.Armband()
yaw_0 = armband.collector.yaw_w
roll_0 = armband.collector.roll_w
pitch_0 = armband.collector.pitch_w
clock = pygame.time.Clock()
namedWindow("output", 1)
person_cascade = CascadeClassifier(CASCADE_FILE)
if testing:
capture = VideoCapture(0)
while (not capture.isOpened()):
print "not opened"
running = True
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYUP:
drone.hover()
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
drone.reset()
running = False
# takeoff / land
elif event.key == pygame.K_RETURN:
print("return")
drone.takeoff()
elif event.key == pygame.K_SPACE:
print("space")
drone.land()
# emergency
elif event.key == pygame.K_BACKSPACE:
drone.reset()
# forward / backward
elif event.key == pygame.K_w:
drone.move_forward()
elif event.key == pygame.K_s:
drone.move_backward()
# left / right
elif event.key == pygame.K_a:
drone.move_left()
elif event.key == pygame.K_d:
drone.move_right()
# up / down
elif event.key == pygame.K_UP:
drone.move_up()
elif event.key == pygame.K_DOWN:
drone.move_down()
# turn left / turn right
elif event.key == pygame.K_LEFT:
drone.turn_left()
elif event.key == pygame.K_RIGHT:
drone.turn_right()
elif event.key == pygame.K_p:
psi_i = drone.navdata['psi']
while (drone.navdata['psi'] - psi_i) % 360 < 90:
drone.turn_right()
elif event.key == pygame.K_t:
tracking = not tracking
elif event.key == pygame.K_c:
yaw_0 = armband.collector.yaw_w
roll_0 = armband.collector.roll_w
pitch_0 = armband.collector.pitch_w
calibrated = True
print "calibrated!"
# speed
elif event.key == pygame.K_1:
drone.speed = 0.1
elif event.key == pygame.K_2:
drone.speed = 0.2
elif event.key == pygame.K_3:
drone.speed = 0.3
elif event.key == pygame.K_4:
drone.speed = 0.4
elif event.key == pygame.K_5:
drone.speed = 0.5
elif event.key == pygame.K_6:
drone.speed = 0.6
elif event.key == pygame.K_7:
drone.speed = 0.7
elif event.key == pygame.K_8:
drone.speed = 0.8
elif event.key == pygame.K_9:
drone.speed = 0.9
elif event.key == pygame.K_0:
drone.speed = 1.0
# MUST WEAR armband with light to outside
if (not calibrated):
frame = drone.get_image()
imshow("output", frame)
continue
yaw_d = armband.collector.yaw_w - yaw_0
pitch_d = armband.collector.pitch_w - pitch_0
roll_d = armband.collector.roll_w - roll_0
if (roll_d > 270): roll_d = roll_d - 360
if (not controlled and not tracking and pitch_d > 40 and roll_d > 60):
tracking_start = True
armband.collector.myo.vibrate('short')
if (tracking_start and not (pitch_d > 40 or roll_d > 60)):
tracking_start = False
if (tracking_start and armband.collector.current_pose == pose_t.fist):
tracking_start = False
tracking = True
armband.collector.myo.vibrate('long')
if (not controlled and roll_d > 60
and not (pitch_d > 10 or pitch_d < -10)):
control_start = True
armband.collector.myo.vibrate('short')
if (control_start and (pitch_d > 10 or pitch_d < -10)):
control_start = False
if (control_start and roll_d < 10):
yaw_0 = armband.collector.yaw_w
roll_0 = armband.collector.roll_w
controlled = True
control_start = False
tracking = False
armband.collector.myo.vibrate('long')
#print yaw_d, pitch_d, roll_d
if (controlled):
if (yaw_d < -20):
print "turn right"
drone.turn_right()
if (yaw_d > 20):
print "turn left"
drone.turn_left()
if (pitch_d < -15):
print "move backward"
drone.move_backward()
if (pitch_d > 20):
print "move forward"
drone.move_forward()
if (roll_d > 30):
print "move right"
drone.move_right()
if (roll_d < -30):
print "move left"
drone.move_left()
if (pitch_d > 30):
controlled = False
if (armband.collector.current_pose == pose_t.double_tap):
print "shoot"
urllib2.urlopen("http://droneduino.local/arduino/shoot/lj")
armband.collector.current_pose = pose_t.rest
if (armband.collector.current_pose == pose_t.fingers_spread):
print "rise"
armband.collector.current_pose = pose_t.rest
if (armband.collector.current_pose == pose_t.fist):
print "descend"
armband.collector.current_pose = pose_t.rest
frame = drone.get_image()
imshow("output", frame)
continue
frame = drone.get_image()
if testing:
ret, frame = capture.read()
frame = flip(frame, 1)
if frame != None:
if not estimated:
estimated = np.array((frame.shape[1], frame.shape[0]))
b, g, gray = split(frame)
people = person_cascade.detectMultiScale(gray, 1.05, 3, 0,
(150, 150))
best_candidate = (0, 0, 0, 0)
best_distance = 9000
for (x, y, w, h) in people:
rectangle(frame, (x, y), (x + w, y + h), 0)
distance = np.linalg.norm(
np.array((x + w / 2, y + h / 2)) - estimated)
if distance < best_distance:
best_candidate = (x, y, w, h)
best_distance = distance
(x, y, w, h) = best_candidate
k2d.update(x + w / 2, y + h / 2)
estimated = [int(c) for c in k2d.getEstimate()]
circle(frame, (estimated[0], estimated[1]), 4, 1234)
rectangle(frame, (x, y), (x + w, y + h), 1234)
if tracking:
if not tracking_center:
tracking_center = [x + w / 2, y + h / 2]
circle(frame, (tracking_center[0], tracking_center[1]), 15,
0x00FF00)
circle(frame, (x + w / 2, y + w / 2), 10, 0xFF0000)
print tracking_center, x + w / 2, y + h / 2,
#if (y + h/2 < frame.shape[0]/2):
if False:
print "unable to track"
else:
FUZZ = 100
if (y + h / 2 + FUZZ < tracking_center[1]):
print "move forward"
drone.move_forward()
elif (y + h / 2 - FUZZ > tracking_center[1]):
print "move backward"
drone.move_backward()
if (x + w / 2 + FUZZ < tracking_center[0]):
print "move left"
drone.turn_left()
elif (x + w / 2 - FUZZ > tracking_center[0]):
print "move right"
drone.turn_right()
imshow("output", frame)
else:
print "frame is none"
#t = getTickCount() - t
#print "detection time = %gms" % (t/(getTickFrequency()*1000.))
print("Shutting down...")
drone.halt()
print("Ok.")
self.steer(command, speed)
def main_loop():
running = True
while running:
key = cv2.waitKey(33)
running = keyboard.on_key(key)
def init():
drone.reset()
print "setting up drone..."
drone = libardrone.ARDrone(True, False)
global interface
interface = Tower(drone)
print "starting up!"
if __name__ == '__main__':
keyboard = keyboard_control.KeyboardControl(drone, interface)
pipeline = video_pipeline.Pipeline(drone, interface)
pipeline.daemon = True
cv2.imshow('Fury', numpy.zeros((10, 10)))
try:
init()
pipeline.start()
main_loop()
except Exception, e:
def main():
GREENHUE_LOWERB = 50
GREENHUE_UPPERB = 100
testing = False
if len(sys.argv) > 1 and sys.argv[1] == 'testing':
testing = True
pygame.init()
print "initialized"
screen = pygame.display.set_mode((320, 240))
navdata_0 = None
if not testing:
drone = libardrone.ARDrone(True)
drone.reset()
navdata_0 = drone.navdata.copy()
tracking_center = False
k2d = Kalman2D(1, 0.0001, 0.1)
estimated = False
clock = pygame.time.Clock()
namedWindow("threshold", 1)
namedWindow("control", 1)
namedWindow("output", 1)
createTrackbar("green hue lower", "control", GREENHUE_LOWERB, 255,
report_lower_change)
createTrackbar("green hue upper", "control", GREENHUE_UPPERB, 255,
report_upper_change)
if testing:
capture = VideoCapture(0)
while (not capture.isOpened()):
print "not opened"
running = True
while running:
GREENHUE_LOWERB = getTrackbarPos("green hue lower", "control")
GREENHUE_UPPERB = getTrackbarPos("green hue upper", "control")
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYUP:
drone.hover()
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
drone.reset()
running = False
# takeoff / land
elif event.key == pygame.K_RETURN:
print("return")
drone.takeoff()
elif event.key == pygame.K_SPACE:
print("space")
drone.land()
# emergency
elif event.key == pygame.K_BACKSPACE:
drone.reset()
# forward / backward
elif event.key == pygame.K_w:
drone.move_forward()
elif event.key == pygame.K_s:
drone.move_backward()
# left / right
elif event.key == pygame.K_a:
drone.move_left()
elif event.key == pygame.K_d:
drone.move_right()
# up / down
elif event.key == pygame.K_UP:
drone.move_up()
elif event.key == pygame.K_DOWN:
drone.move_down()
# turn left / turn right
elif event.key == pygame.K_LEFT:
drone.turn_left()
elif event.key == pygame.K_RIGHT:
drone.turn_right()
elif event.key == pygame.K_p:
psi_i = drone.navdata['psi']
while (drone.navdata['psi'] - psi_i) % 360 < 90:
drone.turn_right()
elif event.key == pygame.K_1:
drone.speed = 0.1
elif event.key == pygame.K_2:
drone.speed = 0.2
elif event.key == pygame.K_3:
drone.speed = 0.3
elif event.key == pygame.K_4:
drone.speed = 0.4
elif event.key == pygame.K_5:
drone.speed = 0.5
elif event.key == pygame.K_6:
drone.speed = 0.6
elif event.key == pygame.K_7:
drone.speed = 0.7
elif event.key == pygame.K_8:
drone.speed = 0.8
elif event.key == pygame.K_9:
drone.speed = 0.9
elif event.key == pygame.K_0:
drone.speed = 1.0
if testing:
ret, frame = capture.read()
frame = flip(frame, 1)
else:
frame = drone.get_image()
frame = cvtColor(frame, COLOR_RGB2BGR)
if frame != None:
processed_frame = cvtColor(frame, COLOR_BGR2HSV)
processed_frame = inRange(processed_frame,
(GREENHUE_LOWERB, 25, 25),
(GREENHUE_UPPERB, 190, 190))
kernel = np.ones((5, 5))
#processed_frame = erode(processed_frame, kernel, iterations=2)
#processed_frame = dilate(processed_frame, kernel, iterations=2)
imshow("threshold", processed_frame)
contours = findContours(processed_frame, RETR_EXTERNAL,
CHAIN_APPROX_SIMPLE, (0, 0))[0]
hat = (0, 0, 0, 0)
hat_distance = 90000
for contour in contours:
(x, y, w, h) = boundingRect(contour)
distance = np.linalg.norm(
np.array((x + w / 2, y + h / 2)) - estimated)
if distance < hat_distance:
hat = (x, y, w, h)
hat_distance = distance
if hat is None:
continue
(x1, y1, w, h) = hat
(x2, y2) = (x1 + w, y1 + h)
k2d.update(x1 + w / 2, y1 + h / 2)
estimated = [int(c) for c in k2d.getEstimate()]
tracking_center = ((x1 + x2) / 2, (y1 + y2) / 2)
circle(frame, (estimated[0], estimated[1]), 4, 1234)
rectangle(frame, (x1, y1), (x2, y2), 0xFF0000)
if not testing:
center = (frame.shape[1] / 2, frame.shape[0] / 2)
FUZZ = 100
if (center[1] + FUZZ < tracking_center[1]):
#print "move forward"
#drone.move_forward()
pass
elif (center[1] - FUZZ > tracking_center[1]):
#print "move backward"
#drone.move_backward()
pass
if (center[0] + FUZZ < tracking_center[0]):
#print "move right"
#drone.turn_right()
pass
elif (center[0] - FUZZ > tracking_center[0]):
#print "move left"
#drone.turn_left()
pass
imshow("output", frame)
else:
print "frame is none"
print("Shutting down...")
drone.halt()
print("Ok.")
__author__ = 'Benedikt'
import threading
import autonomous_flight
import cv2
class Pipeline(threading.Thread):
def __init__(self, drone, interface):
super(Pipeline, self).__init__()
self.drone = drone
self.interface = interface
self.cv_win = "Fury"
self.af = autonomous_flight.AF(0, self.cv_win, self.drone, interface)
def stop(self):
self.af.stop()
def run(self):
self.af.run()
# to steer: interface.steer_autonomous("up")
if __name__ == '__main__':
import libardrone.libardrone as libardrone
print "video_pipeline main"
Pipeline(libardrone.ARDrone(1, 1)).run()
def main():
global drone
global thread_lock
global thread_lock_manual_mode
global thread_lock_camera_frame
global thread_lock_key
global exiting
global manual_mode
global W
global H
global render_frame
global new_frame
global key
global tx_prev
global uix_prev
global ex_prev
global uif_prev
global ef_prev
global uiy_prev
global ey_prev
#initialization
W, H = 640, 360
key = -1
drone = libardrone.ARDrone(True)
drone.reset()
exiting = False
manual_mode = True
new_frame = False
threads = []
thread_lock = threading.Lock()
thread_lock_manual_mode = threading.Lock()
thread_lock_camera_frame = threading.Lock()
thread_lock_key = threading.Lock()
tx_prev = 0
uix_prev = 0
ex_prev = 0
uif_prev = 0
ef_prev = 0
uiy_prev = 0
ey_prev = 0
# Create new threads
manual_control_thread = manualControlThread(1, "Manual Control Thread")
automatic_control_thread = automaticControlThread(
2, "Automatic Control Thread")
# Start new Threads
manual_control_thread.start()
automatic_control_thread.start()
# Add threads to thread list
threads.append(manual_control_thread)
threads.append(automatic_control_thread)
thread_lock.acquire()
while not exiting:
thread_lock.release()
# wait for pressed key
thread_lock_key.acquire()
key = cv2.waitKey(33)
thread_lock_key.release()
# display new frame from camera
thread_lock_camera_frame.acquire()
if new_frame:
cv2.imshow('Drone', render_frame)
thread_lock_camera_frame.release()
thread_lock.acquire()
# Wait for all threads to complete
for t in threads:
t.join()
print "Exiting Main Program"
def main():
#Constants
SCREEN_HEIGHT, SCREEN_WIDTH = 360, 720
FPS = 20
#Setting up Drone
pygame.init()
drone = libardrone.ARDrone(True, True) #CHANGE THIS TO IMAGE_PROCCESSING
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
drone.reset()
clock = pygame.time.Clock()
running = True
#Pygame Event Listener
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYUP:
drone.hover()
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
drone.reset()
running = False
elif event.key == pygame.K_RETURN:
print "takeoff"
drone.takeoff()
elif event.key == pygame.K_SPACE:
print "land"
drone.land()
# emergency reset
elif event.key == pygame.K_BACKSPACE:
drone.reset()
elif event.key == pygame.K_w:
drone.move_forward()
elif event.key == pygame.K_s:
drone.move_backward()
elif event.key == pygame.K_a:
drone.move_left()
elif event.key == pygame.K_d:
drone.move_right()
elif event.key == pygame.K_UP:
drone.move_up()
elif event.key == pygame.K_DOWN:
drone.move_down()
elif event.key == pygame.K_LEFT:
drone.turn_left()
elif event.key == pygame.K_RIGHT:
drone.turn_right()
try:
pixelarray = drone.get_image() #The image in pixels (RGB)
#Image Piping If Found Person in Picture
result = findFaces(pixelarray)
#print 'I get here'
img2 = Image.fromarray(result, 'RGB')
img2.save('./tmp/img2.png')
if pixelarray != None:
#Pygame Gui Design
surface = pygame.surfarray.make_surface(result)
rotsurface = pygame.transform.rotate(surface, 270)
screen.blit(rotsurface, (0, 0))
hud_color = (255, 0, 0) if drone.navdata.get(
'drone_state', dict()).get('emergency_mask', 1) else (10, 10,
255)
bat = drone.navdata.get(0, dict()).get('battery', 0)
f = pygame.font.Font(None, 20)
hud = f.render('Battery: %i%%' % bat, True, hud_color)
screen.blit(hud, (10, 10))
except:
pass
pygame.display.flip()
clock.tick(FPS)
pygame.display.set_caption("FPS: %.2f" % clock.get_fps())
print "Shutting down...",
drone.halt()
@author: patrickchrist
"""
import libardrone.libardrone as libardrone #import the libardone
import cv2 #import opencv
import numpy as np
TARGET_COLOR_MIN = np.array([0, 100, 100], np.uint8)
TARGET_COLOR_MAX = np.array([5, 255, 255], np.uint8)
W, H = 640, 360
screenMidX = W / 2
screenMidY = H / 2
speedCirclesDiff = H / 12
drone = libardrone.ARDrone(1, 1) #initalize the Drone Object
running = True
while running:
try:
# This should be an numpy image array
pixelarray = drone.get_image() # get an frame form the Drone
if pixelarray != None: # check whether the frame is not empty
frame = pixelarray[:, :, ::-1].copy() #convert to a frame
_frame = np.asarray(frame)
frameHSV = cv2.cvtColor(_frame, cv2.COLOR_BGR2HSV)
frameThreshold = cv2.inRange(frameHSV, TARGET_COLOR_MIN,
TARGET_COLOR_MAX)
element = cv2.getStructuringElement(cv2.MORPH_RECT, (1, 1))
frameThreshold = cv2.erode(frameThreshold, element, iterations=1)
frameThreshold = cv2.dilate(frameThreshold, element, iterations=1)
frameThreshold = cv2.erode(frameThreshold, element)
drone.speed = 0.9
else:
if return_to_hover:
return_to_hover = False
drone.hover()
show_frame()
def show_frame():
try:
pixelarray = drone.get_image() # get an frame form the Drone
frame = pixelarray[:, :, ::-1].copy() # convert to a frame
cv2.imshow('Drone', frame)
except Exception, ex:
print "Image showing failed", ex
if __name__ == '__main__':
drone = libardrone.ARDrone(True, True)
cv2.imshow('Drone', numpy.zeros((10, 10)))
try:
fly()
except Exception, e:
print "Going down.", e
drone.land()
finally:
drone.halt()
cv2.destroyAllWindows()
def controlLoop(self):
drone = libardrone.ARDrone(True)
drone.reset()
frame = drone.get_image()
cv2.imshow('img', frame)
#Wait for any key to start over
cv2.waitKey(0)
drone.takeoff()
print "Takeoff"
logFilePIDPath = "logFilePID_11.csv"
logFilePID = open(logFilePIDPath, "a")
logFileCmdPath = "logFileCmd_11.csv"
logFileCmd = open(logFileCmdPath, "a")
logFilePID.write(
"\n\n=================================================================================\n"
)
logFileCmd.write(
"\n\n=================================================================================\n"
)
running = True
while running:
key = cv2.waitKey(5)
if key == 32:
print "Land drone"
running = False
drone.land()
frame = drone.get_image()
# call imageRec
xlu, ylu, xrd, yrd = patternRecognition.cornerPointsChess(
frame, logFileCmd)
if not (xlu == -1 and ylu == -1 and xrd == -1 and yrd == -1):
# computeSize
currentsize = self.computeSize(xlu, ylu, xrd, yrd)
recipSize = self.reciprocalSize(self.standardSize, currentsize)
xAvg = (xlu + xrd) / 2.0
yAvg = (ylu + yrd) / 2.0
# call PIDController
x_PIDValue = self.x_PIDController.pidControl(
self.standardXCoord, xAvg)
y_PIDValue = self.y_PIDController.pidControl(
self.standardYCoord, yAvg)
bf_PIDValue = self.bf_PIDController.pidControl(
self.standardSize, recipSize)
#log-file entries
self.logFileWrite(logFileCmd, "x_PID: " + str(x_PIDValue))
self.logFileWrite(logFileCmd, "y_PID: " + str(y_PIDValue))
self.logFileWrite(logFileCmd, "bf_PID: " + str(bf_PIDValue))
self.logFileWrite(
logFilePID,
str(x_PIDValue) + "," + str(y_PIDValue) + "," +
str(bf_PIDValue))
# Actuate
xSpeed, ySpeed, bfSpeed, x2Speed = self.calcSpeed(
x_PIDValue, y_PIDValue, bf_PIDValue)
self.actuateAll(x2Speed, xSpeed, ySpeed, bfSpeed, drone)
else:
drone.hover()
pass
time.sleep(0.01)
#Close log-files
logFilePID.close
logFileCmd.close
print "Drone landed"
print("Shutting down...")
drone.halt()
print("Ok.")
return frame
except Exception, ex:
print "Frame grabbing failed", ex
return None
def show_frame(frame):
try:
cv2.putText(frame, "Press 'x' to go to circleDetection: ", (170, 450),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255))
cv2.imshow('Drone', frame)
except Exception, ex:
print "Image showing failed", ex
if __name__ == '__main__':
if debugMode:
drone = droneDummy.Drone()
else:
drone = libardrone.ARDrone(True, False) #no HD video
try:
fly()
except Exception, e:
print "Going down because an exception occured."
print e
print traceback.format_exc()
finally:
print "Going down on close"
drone.land()
cv2.destroyAllWindows()
def main():
W, H = 640, 360
screenMidX = W / 2
screenMidY = H / 2
speedCirclesDiff = H / 12
drone = libardrone.ARDrone(True, False)
time.sleep(0.5)
drone.reset()
time.sleep(0.5)
#drone.set_camera_view(True) # False for bottom camera
#sleep(0.5)
drone.set_speed(0.3)
time.sleep(0.5)
drone.takeoff()
print "take off"
time.sleep(0.5)
drone.hover()
time.sleep(0.5)
running = True
while running:
try:
# This should be an numpy image array
pixelarray = drone.get_image()
if pixelarray != None:
# Convert RGB to BGR & make OpenCV Image
frame = pixelarray[:, :, ::-1].copy()
_frame = np.asarray(frame)
frameHSV = cv2.cvtColor(_frame, cv2.COLOR_BGR2HSV)
frameThreshold = cv2.inRange(frameHSV, TARGET_COLOR_MIN,
TARGET_COLOR_MAX)
element = cv2.getStructuringElement(cv2.MORPH_RECT, (1, 1))
frameThreshold = cv2.erode(frameThreshold,
element,
iterations=1)
frameThreshold = cv2.dilate(frameThreshold,
element,
iterations=1)
frameThreshold = cv2.erode(frameThreshold, element)
# Blurs to smoothen frame
frameThreshold = cv2.GaussianBlur(frameThreshold, (9, 9), 2, 2)
frameThreshold = cv2.medianBlur(frameThreshold, 5)
# Find Contours
contours, hierarchy = cv2.findContours(frameThreshold,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
showingCNTs = [] # Contours that are visible
areas = [] # The areas of the contours
# Find Specific contours
for cnt in contours:
approx = cv2.approxPolyDP(cnt,
0.1 * cv2.arcLength(cnt, True),
True)
#if len(approx)==4:
area = cv2.contourArea(cnt)
if area > 300:
areas.append(area)
showingCNTs.append(cnt)
# Only Highlight the largest object
if len(areas) > 0:
m = max(areas)
maxIndex = 0
for i in range(0, len(areas)):
if areas[i] == m:
maxIndex = i
cnt = showingCNTs[maxIndex]
# Highlight the Object Red
#cv2.drawContours(frame,[cnt],0,(0,0,255),-1)
# Draw a bounding rectangle
x, y, w, h = cv2.boundingRect(cnt)
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0),
2)
# Draw a rotated bounding rectangle
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
cv2.drawContours(frame, [box], 0, (0, 255, 255), 2)
# Draw a circumcircle
(x, y), radius = cv2.minEnclosingCircle(cnt)
center = (int(x), int(y))
radius = int(radius)
cv2.circle(frame, center, radius, (255, 0, 255), 2)
# Draw line to center of screen
cv2.line(frame, (screenMidX, screenMidY), center,
(0, 0, 255), 2)
# Draw Speed Limit Circles and Determine Speed
diff = speedCirclesDiff # Increment of radius of the Speed Limit Circles
# Loop for drawing speed circles
for i in range(1, 10):
cv2.circle(frame, (screenMidX, screenMidY), diff * i,
(0, 0, 255), 2)
# Center of the object
x = center[0]
y = center[1]
'''
hor_speed = 0.0
vert_speed = 0.0
# Loop for checking speed
for i in range(1, 10):
_diff = diff*i
if (640-_diff)<x<(640+_diff): # On the x axis
hor_speed = (i-1)/10
break
for i in range(1, 10):
if (360-_diff)<y<(360+_diff): # On the y axis
vert_speed = (i-1)/10
break
'''
# Check direction of the ball
centerThresh = 50 # The 'centre' threshold for the ball
# On the X axis -> Note: Inverse
if (screenMidX -
x) < -centerThresh: # To the left to the left
drone.turn_right()
#print "Left"
if (screenMidX -
x) > centerThresh: # To the right to the right
drone.turn_left()
#print "right"
if -centerThresh < (screenMidX - x) < centerThresh:
#drone.hover()
pass
#print "Stop Rotating"
# On the Y axis
if (screenMidY - y) < -centerThresh: # Down!
drone.move_down()
if (screenMidY - y) > centerThresh: # Up!
drone.move_up()
if -centerThresh < (screenMidY - y) < centerThresh:
#drone.hover()
pass
#print "Stop Moving up or down"
# Get battery status of the drone
bat = drone.navdata.get(0, dict()).get('battery', 0)
#print str(bat)
if bat < 20:
running = False
print "Low Battery: " + str(bat)
# Display the Image
cv2.imshow('Drone', frame)
except:
print "Failed"
'''
if cv2.waitKey(1) & 0xFF == ord('w'):
print "Take OFF!!"
drone.reset()
drone.takeoff()
drone.hover()
'''
# Listen for Q Key Press
if cv2.waitKey(1) & 0xFF == ord('q'):
running = False
# Shutdown
print "Shutting Down..."
drone.land()
drone.halt()
print "Ok."