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."