def main(): global manual_command, stop thread_fly = None tello = Tello() # Remove most of the log lines logger = logging.getLogger('djitellopy') logger.setLevel(logging.CRITICAL) tello.connect() try: tello.streamon() thread_fly = Thread(target=fly, args=[tello]) thread_fly.start() while not stop: manual_command = input( "Pulsa una tecla de dirección o escribe un comando ('despegar','aterrizar','giro horario', 'giro antihorario', 'salir'): " ) except: error = sys.exc_info()[0] print("Unexpected error:", error) finally: stop = True if not thread_fly is None: thread_fly.join() tello.land() tello.streamoff() tello.end()
def run(CommandsQueue): move_distance = 30 idle_sleep = 2 #sec video_ready_sleep = 7 #sec is_airborn = False global is_active tello = Tello() tello.connect() tello.set_speed(10) droneVid = threading.Thread(target=drone_video, args=(tello, )) droneVid.start() time.sleep(video_ready_sleep) while is_active: #execute next command from the que if not CommandsQueue.empty(): command = CommandsQueue.get() timeStamp = str(datetime.datetime.now()) print('Command for Drone: ' + str(command[0]) + ' at time ' + timeStamp) if command[0] == Commands.up: if is_airborn: tello.move_up(move_distance) else: tello.takeoff() is_airborn = True # elif command[0] == Commands.idle: # time.sleep(idle_sleep) elif command[0] == Commands.up and is_airborn == True: tello.move_up(move_distance) elif command[0] == Commands.down and is_airborn == True: tello.move_down(move_distance) elif command[0] == Commands.forward and is_airborn == True: tello.move_forward(move_distance) elif command[0] == Commands.back and is_airborn == True: tello.move_back(move_distance) elif command[0] == Commands.left and is_airborn == True: tello.move_left(move_distance) elif command[0] == Commands.right and is_airborn == True: tello.move_right(move_distance) elif command[0] == Commands.flip and is_airborn == True: tello.flip_back() elif command[0] == Commands.stop: is_active = False break # else: # time.sleep(idle_sleep) tello.land() is_airborn = False droneVid.join() tello.end()
def main(): tello = Tello() tello.connect() tello.streamoff() tello.streamon() frontend = FrontEnd(tello) right = 0 while(right<2): right = frontend.run(right) print("right is now hsdawqbvqwuja : {}".format(right)) frontend.clear() print("found the end of the code") tello.land() tello.end()
class FrontEnd(object): def __init__(self, window_width, window_height): # Init pygame pygame.init() # Creat pygame window self.screen = pygame.display.set_mode([window_width, window_height]) # [960, 720] # Init Tello object that interacts with the Tello drone self.tello = Tello() # create update timer #pygame.time.set_timer(pygame.USEREVENT + 1, 50) def run(self): pygame.display.set_caption("Starting frame reading...") frame_read = self.tello.get_frame_read() pygame.display.set_caption("Receiving Tello video stream") should_stop = False while not should_stop: for event in pygame.event.get(): if event.type == pygame.QUIT: should_stop = True elif event.type == pygame.KEYDOWN: if event.key == pygame.K_ESCAPE: should_stop = True if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) frame = np.rot90(frame) frame = np.flipud(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() time.sleep(1 / FPS) # Call it always before finishing. To deallocate resources. self.tello.end()
def main(): tello = Tello() tello.connect() print(f"battery level: {tello.get_battery()}") print(f"temp: {tello.get_temperature()}") tello.takeoff() tello.move_left(50) tello.move_right(50) tello.rotate_clockwise(360) print(f"Waiting for 1 secs") time.sleep(1) tello.flip_left() tello.land() tello.end()
class missions(object): def __init__(self): pygame.init() pygame.display.set_caption("Tello Feed") self.screen = pygame.display.set_mode([960, 720]) self.tello = Tello() def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() eye_cascade = cv2.CascadeClassifier('haarcascade_eye.xml') face_cascade = cv2.CascadeClassifier( 'haarcascade_frontalface_default.xml') while True: self.screen.fill([0, 0, 0]) frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) gray = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, 1.3, 5) for (x, y, w, h) in faces: cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2) roi_gray = gray[y:y + h, x:x + w] roi_color = frame[y:y + h, x:x + w] eyes = eye_cascade.detectMultiScale(roi_gray) for (ex, ey, ew, eh) in eyes: cv2.rectangle(roi_color, (ex, ey), (ex + ew, ey + eh), (0, 255, 0), 2) frame = np.rot90(frame) frame = np.flipud(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() time.sleep(1 / 60) self.tello.end()
def main(): print("Configuring tello Drone") tello = Tello() if not tello.connect(): print("Tello not connected") return if not tello.streamon(): print("Could not start video stream") return while (True): frame = tello.get_frame_read() cv2.imshow("DetectionResults", frame.frame) raw_key = cv2.waitKey(1) if raw_key == 27: #esc break cv2.destroyAllWindows() tello.end()
def demo(): # tello.send_rc_control(left_right_velocity, for_back_velocity, up_down_velocity, yaw_velocity) input("Press enter to go!") tello = Tello() tello.connect() time.sleep(1) tello.takeoff() time.sleep(1) # tello.move_up(100) time.sleep(1) print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") # tello.flip_left() time.sleep(1) print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") tello.move_forward(100) time.sleep(1) print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") tello.move_right(100) time.sleep(1) print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") tello.rotate_clockwise(180) time.sleep(1) print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") tello.move_right(100) time.sleep(1) print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") tello.move_forward(100) time.sleep(1) print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") tello.rotate_clockwise(180) time.sleep(1) # tello.flip_right() # time.sleep(1) print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") tello.land() print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}") tello.end() print("Power off! :-)")
def demo(): # tello.send_rc_control(left_right_velocity, for_back_velocity, up_down_velocity, yaw_velocity) tello = Tello() tello.connect() time.sleep(1) print("Take off...") tello.takeoff() input() do(tello, forward=50) input() do(tello, forward=-0) input() do(tello, forward=-50) input() do(tello, forward=-0) print("Landing...") tello.land() tello.end() print( f"Tello: battery {tello.get_battery()} temperature {tello.get_temperature()}" ) print("Power off! :-)")
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Tello video stream") self.width = 640 self.height = 480 self.screen = pygame.display.set_mode([self.width, self.height]) # create queue for data communications self.data_queue=queue.Queue() # Init Tello object that interacts with the Tello drone self.tello = Tello(self.data_queue) # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 # Variables for drone's states self.battery = 0 self.angles = [0., 0., 0., 0.] # Direction queue for navigation self.dir_queue=queue.Queue() self.dir_queue.queue.clear() # Bool variables for setting functions self.send_rc_control = False self.calibrate = False self.getPoints = False self.resetPoints = False self.save = False self.getOrigin = False # Creating video queue self.video_queue = queue.Queue() self.video_queue.queue.clear() self.END_event = threading.Event() self.END_event.clear() self.videoWrite = WriteVideo(self.video_queue, FPS, self.END_event) # Run video writer in the background thread_vid = threading.Thread(target=self.videoWrite.writer) thread_vid.daemon = True thread_vid.start() # Data collection event self.getCoords_event = threading.Event() self.getCoords_event.clear() # Navigate between markers self.navigate_event = threading.Event() self.navigate_event.clear() # Camera class self.cam = Camera(S_prog, self.dir_queue, 'camcalib.npz', self.getCoords_event, self.navigate_event, self.END_event) # Create update timer pygame.time.set_timer(USEREVENT + 1, 50) def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() directions = np.zeros(4) should_stop = False while not should_stop: img=cv2.resize(frame_read.frame, (960,720)) # Read from drone state queue if not self.data_queue.empty(): pitch, roll, yaw, tof, bat = self.data_queue.get() self.data_queue.queue.clear() self.battery = bat self.angles_tof = [pitch, roll, yaw, tof] #print([pitch, roll, yaw, tof]) # Calibrate drone camera if self.calibrate: img = self.cam.calibrator(img) # Detect ArUco markers img = self.cam.aruco(img, self.getPoints, self.resetPoints, self.angles_tof) # Reset measurements if self.resetPoints: self.resetPoints=False for event in pygame.event.get(): if event.type == USEREVENT + 1: self.update(directions) elif event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break # Save image on 'M' press if self.save: timestr = time.strftime("%Y%m%d_%H%M%S") cv2.imwrite("images/"+timestr+".jpg", img) self.save = False # Navigation started, points and video capture if self.getCoords_event.is_set(): self.video_queue.put(np.copy(img)) # Write battery percentage img = self.cam.writeBattery(img, self.battery) img=cv2.resize(img, (640,480)) # Resize pyGame window if (img.shape[1] != self.width) or (img.shape[0] != self.height): self.width = img.shape[1] self.height = img.shape[0] self.screen=pygame.display.set_mode((self.width, self.height)) self.screen.fill([0, 0, 0]) frame = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) frame = np.rot90(frame) frame = np.flipud(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() time.sleep(1 / FPS) # Call it always before finishing. I deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() self.send_rc_control = False elif key == pygame.K_k: # camera calibration if self.calibrate: self.calibrate = False else: self.calibrate = True elif key == pygame.K_c: # get aruco marker points if self.getPoints: self.getPoints=False else: self.getPoints = True self.resetPoints = True elif key == pygame.K_m: # save image self.save = True elif key == pygame.K_o: # start navigation if self.navigate_event.is_set(): self.navigate_event.clear() else: self.navigate_event.set() elif key == pygame.K_x: # end video self.END_event.set() self.getPoints = False def update(self, dirs): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: if self.navigate_event.is_set() and not self.dir_queue.empty(): # Auto navigation, read directions queue x, y, z, yaw = self.dir_queue.get() self.tello.send_rc_control(int(x), int(y), int(z), int(yaw)) else: # Clear directions queue to avoid storing old data self.dir_queue.queue.clear() # Control tello manually self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([960, 720]) # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.imgCount = 0 self.send_rc_control = False self.c = 0 self.g = 0 # create update timer pygame.time.set_timer(USEREVENT + 1, 50) self.set_var = 0 def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False while not should_stop: for event in pygame.event.get(): #if event.type == USEREVENT + 1: #pass #self.update() if event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) #frame = cv2.cvtColor(frame_read.frame) img_c = 0 cv2.imwrite("{}/tellocap{}.jpg".format(ddir, self.imgCount), frame) cv2.imwrite("{}/tellocap{}.jpg".format(ddir1, img_c), frame) self.imgCount = self.imgCount + 1 x_axis = 0 y_axis = 0 z_axis = 0 self.g = 0 command_arr = [] print("yooooo start") try: with open( r"C:\Users\hp\Desktop\Tello\ubuntushare\\commands.csv" ) as f: rows = csv.reader(f) for command in rows: command_arr.append(command) print(command_arr) except: print("1st error") try: if (int(command_arr[2][0]) == 2): print("land") self.tello.land() self.set_var = 0 self.g = 1 elif (int(command_arr[1][0]) == 1): print("takeoff") self.tello.takeoff() self.set_var = 1 self.g = 1 with open( r"C:\Users\hp\Desktop\Tello\ubuntushare\\commands.csv", 'w') as fl: writer = csv.writer(fl, delimiter=',') fl.truncate(0) fl.close() except: print("error speech") if (self.g == 0): if (self.set_var == 1): try: with open(r"C:\Users\hp\Desktop\\ginnovators.csv", 'r') as csvfile: # creating a csv reader object csvreader = csv.reader(csvfile) # extracting field names through first row for row in csvreader: values = row print(values) x_axis = int(values[0]) y_axis = int(values[1]) z_axis = int(values[2]) except: print("csv error") try: if (-50 < x_axis < 50): if (-38 < y_axis < 38): if (-7 < z_axis < 18): self.tello.send_rc_control(0, 0, 0, 0) else: if (z_axis > 18): self.for_back_velocity = -30 self.tello.send_rc_control( 0, self.for_back_velocity, 0, 0) elif (z_axis < -7): self.for_back_velocity = 30 self.tello.send_rc_control( 0, self.for_back_velocity, 0, 0) else: if (y_axis > 38): self.up_down_velocity = 30 self.tello.send_rc_control( 0, 0, self.up_down_velocity, 0) elif (y_axis < -38): self.up_down_velocity = -30 self.tello.send_rc_control( 0, 0, self.up_down_velocity, 0) else: if (x_axis > 50): self.yaw_velocity = -20 self.tello.send_rc_control( 0, 0, 0, self.yaw_velocity) elif (x_axis < -50): self.yaw_velocity = 20 self.tello.send_rc_control( 0, 0, 0, self.yaw_velocity) except: print("error") frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() time.sleep(1 / 25) # Call it always before finishing. I deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.set_var = 1 self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() self.send_rc_control = False def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): def __init__(self): # 드론과 상호작용하는 Tello 객체 self.tello = Tello() # 드론의 속도 (-100~100) #수직, 수평 속도 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.send_rc_control = False # 실행 함수 def run(self): #드론이 연결이 되지 않으면 함수 종료 if not self.tello.connect(): print("Tello not connected") return #drone의 제한속도가 적절하지 않은 경우 if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # 프로그램을 비정상적인 방법으로 종료를 시도하여 비디오 화면이 꺼지지 않은 경우 종료. if not self.tello.streamoff(): print("Could not stop video stream") return # 비디오가 켜지지않는 경우 종료. if not self.tello.streamon(): print("Could not start video stream") return #프레임 단위로 인식 frame_read = self.tello.get_frame_read() should_stop = False imgCount = 0 OVERRIDE = False oSpeed = args.override_speed tDistance = args.distance self.tello.get_battery() # X축 안전 범위 szX = args.saftey_x # Y축 안전 범위 szY = args.saftey_y #디버깅 모드 if args.debug: print("DEBUG MODE ENABLED!") #비행을 멈취야할 상황이 주어지지 않은 경우 while not should_stop: self.update() #프레임 입력이 멈췄을 경우 while문 탈출 if frame_read.stopped: frame_read.stop() break theTime = str(datetime.datetime.now()).replace(':', '-').replace( '.', '_') frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) frameRet = frame_read.frame vid = self.tello.get_video_capture() #저장할 경우 if args.save_session: cv2.imwrite("{}/tellocap{}.jpg".format(ddir, imgCount), frameRet) frame = np.rot90(frame) imgCount += 1 time.sleep(1 / FPS) # 키보드 입력을 기다림 k = cv2.waitKey(20) # 0을 눌러서 거리를 0으로 설정 if k == ord('0'): if not OVERRIDE: print("Distance = 0") tDistance = 0 # 1을 눌러서 거리를 1으로 설정 if k == ord('1'): if OVERRIDE: oSpeed = 1 else: print("Distance = 1") tDistance = 1 # 2을 눌러서 거리를 2으로 설정 if k == ord('2'): if OVERRIDE: oSpeed = 2 else: print("Distance = 2") tDistance = 2 # 3을 눌러서 거리를 3으로 설정 if k == ord('3'): if OVERRIDE: oSpeed = 3 else: print("Distance = 3") tDistance = 3 # 4을 눌러서 거리를 4으로 설정 if k == ord('4'): if not OVERRIDE: print("Distance = 4") tDistance = 4 # 5을 눌러서 거리를 5으로 설정 if k == ord('5'): if not OVERRIDE: print("Distance = 5") tDistance = 5 # 6을 눌러서 거리를 6으로 설정 if k == ord('6'): if not OVERRIDE: print("Distance = 6") tDistance = 6 # T를 눌러서 이륙 if k == ord('t'): if not args.debug: print("Taking Off") self.tello.takeoff() self.tello.get_battery() self.send_rc_control = True # L을 눌러서 착륙 if k == ord('l'): if not args.debug: print("Landing") self.tello.land() self.send_rc_control = False # Backspace를 눌러서 명령을 덮어씀 if k == 8: if not OVERRIDE: OVERRIDE = True print("OVERRIDE ENABLED") else: OVERRIDE = False print("OVERRIDE DISABLED") if OVERRIDE: # S & W 눌러서 앞 & 뒤로 비행 if k == ord('w'): self.for_back_velocity = int(S * oSpeed) elif k == ord('s'): self.for_back_velocity = -int(S * oSpeed) else: self.for_back_velocity = 0 # a & d 를 눌러서 왼쪽 & 오른쪽으로 회전 if k == ord('d'): self.yaw_velocity = int(S * oSpeed) elif k == ord('a'): self.yaw_velocity = -int(S * oSpeed) else: self.yaw_velocity = 0 # Q & E 를 눌러서 위 & 아래로 비행 if k == ord('e'): self.up_down_velocity = int(S * oSpeed) elif k == ord('q'): self.up_down_velocity = -int(S * oSpeed) else: self.up_down_velocity = 0 # c & z 를 눌러서 왼쪽 & 오른쪽으로 비행 if k == ord('c'): self.left_right_velocity = int(S * oSpeed) elif k == ord('z'): self.left_right_velocity = -int(S * oSpeed) else: self.left_right_velocity = 0 # 프로그램 종료 if k == 27: should_stop = True break gray = cv2.cvtColor(frameRet, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, scaleFactor=1.5, minNeighbors=2) # 대상 크기 tSize = faceSizes[tDistance] # 중심 차원들 cWidth = int(dimensions[0] / 2) cHeight = int(dimensions[1] / 2) noFaces = len(faces) == 0 # 컨트롤을 얻고, 얼굴 좌표 등을 얻으면 if self.send_rc_control and not OVERRIDE: for (x, y, w, h) in faces: # roi_gray = gray[y:y + h, x:x + w] #(ycord_start, ycord_end) roi_color = frameRet[y:y + h, x:x + w] # 얼굴 상자 특성 설정 fbCol = (255, 0, 0) #BGR 0-255 fbStroke = 2 # 끝 좌표들은 x와 y를 제한하는 박스의 끝에 존재 end_cord_x = x + w end_cord_y = y + h end_size = w * 2 # 목표 좌표들 targ_cord_x = int((end_cord_x + x) / 2) targ_cord_y = int((end_cord_y + y) / 2) + UDOffset # 얼굴에서 화면 중심까지의 벡터를 계산 vTrue = np.array((cWidth, cHeight, tSize)) vTarget = np.array((targ_cord_x, targ_cord_y, end_size)) vDistance = vTrue - vTarget # if not args.debug: # 회전 if vDistance[0] < -szX: self.yaw_velocity = S # self.left_right_velocity = S2 elif vDistance[0] > szX: self.yaw_velocity = -S # self.left_right_velocity = -S2 else: self.yaw_velocity = 0 # 위 & 아래 (상승/하강) if vDistance[1] > szY: self.up_down_velocity = S elif vDistance[1] < -szY: self.up_down_velocity = -S else: self.up_down_velocity = 0 F = 0 if abs(vDistance[2]) > acc[tDistance]: F = S # 앞, 뒤 if vDistance[2] > 0: self.for_back_velocity = S + F elif vDistance[2] < 0: self.for_back_velocity = -S - F else: self.for_back_velocity = 0 # 얼굴 테두리 박스를 그림 cv2.rectangle(frameRet, (x, y), (end_cord_x, end_cord_y), fbCol, fbStroke) # 목표를 원으로 그림 cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10, (0, 255, 0), 2) # 안전 구역을 그림 cv2.rectangle(frameRet, (targ_cord_x - szX, targ_cord_y - szY), (targ_cord_x + szX, targ_cord_y + szY), (0, 255, 0), fbStroke) # 드론의 얼굴 상자로부터의 상대적 벡터 위치를 구함. cv2.putText(frameRet, str(vDistance), (0, 64), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) # 인식되는 얼굴이 없으면 아무것도 안함. if noFaces: self.yaw_velocity = 0 self.up_down_velocity = 0 self.for_back_velocity = 0 print("NO TARGET") # 화면의 중심을 그림. 드론이 목표 좌표와 맞추려는 대상이 됨. cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2) dCol = lerp(np.array((0, 0, 255)), np.array((255, 255, 255)), tDistance + 1 / 7) if OVERRIDE: show = "OVERRIDE: {}".format(oSpeed) dCol = (255, 255, 255) else: show = "AI: {}".format(str(tDistance)) # 선택된 거리를 그림 cv2.putText(frameRet, show, (32, 664), cv2.FONT_HERSHEY_SIMPLEX, 1, dCol, 2) # 결과 프레임을 보여줌. cv2.imshow(f'Tello Tracking...', frameRet) # 종료시에 배터리를 출력 self.tello.get_battery() # 전부 완료되면 캡쳐를 해제함. cv2.destroyAllWindows() # 종료 전에 항상 호출. 자원들을 해제함. self.tello.end() def battery(self): return self.tello.get_battery()[:2] def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): def __init__(self): # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.safe_zone = args.SafeZone self.oSpeed = args.override_speed self.send_rc_control = False def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return should_stop = False imgCount = 0 OVERRIDE = False self.tello.get_battery() result = cv2.VideoWriter('Prueba_Tello.avi', cv2.VideoWriter_fourcc(*'MJPG'), 10, dimensions) vid = self.tello.get_video_capture() if args.debug: print("DEBUG MODE ENABLED!") while not should_stop: #self.update() #time.sleep(1 / FPS) # Listen for key presses k = cv2.waitKey(20) self.Set_Action(k, OVERRIDE) if OVERRIDE: self.Action_OVERRRIDE(k) # Quit the software if k == 27: should_stop = True break if vid.isOpened(): ret, image = vid.read() imgCount += 1 if imgCount % 2 != 0: continue if ret: image = imutils.resize(image, width=min(350, image.shape[1])) # Detecting all the regions # in the Image that has a # pedestrians inside it (regions, weigths) = hog.detectMultiScale(image, winStride=(4, 4), padding=(4, 4), scale=1.1) # Safety Zone Z szZ = Safe_Zones[self.safe_zone][2] # Safety Zone X szX = Safe_Zones[self.safe_zone][0] # Safety Zone Y szY = Safe_Zones[self.safe_zone][1] center = (int(image.shape[1] / 2), int(image.shape[0] / 2)) # if we've given rc controls & get body coords returned if self.send_rc_control and not OVERRIDE: if len(regions) > 0: max_index = np.where(weigths == np.amax(weigths)) (x, y, w, h) = regions[max_index[0][0]] cv2.rectangle(image, (x, y), (x + w, y + h), (0, 0, 255), 2) # points person = (int(x + w / 2), int(y + h / 2)) distance = (person[0] - center[0], center[1] - person[1]) theta0 = 86.6 B = image.shape[1] person_width = w # z theta1 = theta0 * (2 * abs(distance[0]) + person_width) / (2 * B) z = (2 * abs(distance[0]) + person_width) / ( 2 * math.tan(math.radians(abs(theta1)))) distance = (int(distance[0]), int(distance[1]), int(z)) if not args.debug: # for turning self.update() if distance[0] < -szX: self.yaw_velocity = S # self.left_right_velocity = S2 elif distance[0] > szX: self.yaw_velocity = -S # self.left_right_velocity = -S2 else: self.yaw_velocity = 0 # for up & down if distance[1] > szY: self.up_down_velocity = S elif distance[1] < -szY: self.up_down_velocity = -S else: self.up_down_velocity = 0 F = 0 if abs(distance[2]) > acc[self.safe_zone]: F = S # for forward back if distance[2] > szZ: self.for_back_velocity = S + F elif distance[2] < szZ: self.for_back_velocity = -S - F else: self.for_back_velocity = 0 cv2.line(image, center, (center[0] + distance[0], center[1]), (255, 0, 0)) cv2.line(image, (center[0] + distance[0], center[1]), person, (0, 255, 0)) cv2.circle(image, center, 3, (0, 255, 0)) cv2.circle(image, person, 3, (0, 0, 255)) cv2.putText( image, "d:" + str(distance), (0, 20), 2, 0.7, (0, 0, 0), ) # if there are no body detected, don't do anything else: self.yaw_velocity = 0 self.up_down_velocity = 0 self.for_back_velocity = 0 print("NO TARGET") dCol = lerp(np.array((0, 0, 255)), np.array( (255, 255, 255)), self.safe_zone + 1 / 7) if OVERRIDE: show = "OVERRIDE: {}".format(self.oSpeed) dCol = (255, 255, 255) else: show = "AI: {}".format(str(self.safe_zone)) cv2.rectangle( image, (int(center[0] - szX / 2), int(center[1] - szY / 2)), (int(center[0] + szX / 2), int(center[1] + szY / 2)), (255, 0, 0), 1) # Showing the output Image image = cv2.resize(image, dimensions, interpolation=cv2.INTER_AREA) # Write the frame into the # file 'Prueba_Tello.avi' result.write(image) # Draw the distance choosen cv2.putText(image, show, (32, 664), cv2.FONT_HERSHEY_SIMPLEX, 1, dCol, 2) # Display the resulting frame cv2.imshow(f'Tello Tracking...', image) else: break # On exit, print the battery self.tello.get_battery() # When everything done, release the capture cv2.destroyAllWindows() result.release() # Call it always before finishing. I deallocate resources. self.tello.end() def battery(self): return self.tello.get_battery()[:2] def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity) def Action_OVERRRIDE(self, k): # S & W to fly forward & back if k == ord('w'): self.for_back_velocity = int(S * oSpeed) elif k == ord('s'): self.for_back_velocity = -int(S * oSpeed) else: self.for_back_velocity = 0 # a & d to pan left & right if k == ord('d'): self.yaw_velocity = int(S * self.oSpeed) elif k == ord('a'): self.yaw_velocity = -int(S * self.oSpeed) else: self.yaw_velocity = 0 # Q & E to fly up & down if k == ord('e'): self.up_down_velocity = int(S * self.oSpeed) elif k == ord('q'): self.up_down_velocity = -int(S * self.oSpeed) else: self.up_down_velocity = 0 # c & z to fly left & right if k == ord('c'): self.left_right_velocity = int(S * self.oSpeed) elif k == ord('z'): self.left_right_velocity = -int(S * self.oSpeed) else: self.left_right_velocity = 0 return def Set_Action(self, k, OVERRIDE): # Press 0 to set distance to 0 if k == ord('0'): if not OVERRIDE: print("Distance = 0") self.safe_zone = 0 # Press 1 to set distance to 1 if k == ord('1'): if OVERRIDE: self.oSpeed = 1 else: print("Distance = 1") self.safe_zone = 1 # Press 2 to set distance to 2 if k == ord('2'): if OVERRIDE: self.oSpeed = 2 else: print("Distance = 2") self.safe_zone = 2 # Press 3 to set distance to 3 if k == ord('3'): if OVERRIDE: self.oSpeed = 3 else: print("Distance = 3") self.safe_zone = 3 # Press 4 to set distance to 4 if k == ord('4'): if not OVERRIDE: print("Distance = 4") self.safe_zone = 4 # Press 5 to set distance to 5 if k == ord('5'): if not OVERRIDE: print("Distance = 5") self.safe_zone = 5 # Press 6 to set distance to 6 if k == ord('6'): if not OVERRIDE: print("Distance = 6") self.safe_zone = 6 # Press T to take off if k == ord('t'): if not args.debug: print("Taking Off") self.tello.takeoff() self.tello.get_battery() self.send_rc_control = True # Press L to land if k == ord('l'): if not args.debug: print("Landing") self.tello.land() self.send_rc_control = False # Press Backspace for controls override if k == 8: if not OVERRIDE: OVERRIDE = True print("OVERRIDE ENABLED") else: OVERRIDE = False print("OVERRIDE DISABLED")
class FrontEnd(object): def __init__(self): # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.send_rc_control = False def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False imgCount = 0 OVERRIDE = False oSpeed = args.override_speed tDistance = args.distance self.tello.get_battery() # Safety Zone X szX = args.saftey_x # Safety Zone Y szY = args.saftey_y if args.debug: print("DEBUG MODE ENABLED!") while not should_stop: self.update() if frame_read.stopped: frame_read.stop() break theTime = str(datetime.datetime.now()).replace(':', '-').replace( '.', '_') frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) frameRet = frame_read.frame vid = self.tello.get_video_capture() if args.save_session: cv2.imwrite("{}/tellocap{}.jpg".format(ddir, imgCount), frameRet) frame = np.rot90(frame) imgCount += 1 time.sleep(1 / FPS) # Listen for key presses k = cv2.waitKey(20) # Press 0 to set distance to 0 if k == ord('0'): if not OVERRIDE: print("Distance = 0") tDistance = 0 # Press 1 to set distance to 1 if k == ord('1'): if OVERRIDE: oSpeed = 1 else: print("Distance = 1") tDistance = 1 # Press 2 to set distance to 2 if k == ord('2'): if OVERRIDE: oSpeed = 2 else: print("Distance = 2") tDistance = 2 # Press 3 to set distance to 3 if k == ord('3'): if OVERRIDE: oSpeed = 3 else: print("Distance = 3") tDistance = 3 # Press 4 to set distance to 4 if k == ord('4'): if not OVERRIDE: print("Distance = 4") tDistance = 4 # Press 5 to set distance to 5 if k == ord('5'): if not OVERRIDE: print("Distance = 5") tDistance = 5 # Press 6 to set distance to 6 if k == ord('6'): if not OVERRIDE: print("Distance = 6") tDistance = 6 # Press T to take off if k == ord('t'): if not args.debug: print("Taking Off") self.tello.takeoff() self.tello.get_battery() self.send_rc_control = True # Press L to land if k == ord('l'): if not args.debug: print("Landing") self.tello.land() self.send_rc_control = False # Press Backspace for controls override if k == 8: if not OVERRIDE: OVERRIDE = True print("OVERRIDE ENABLED") else: OVERRIDE = False print("OVERRIDE DISABLED") if OVERRIDE: # S & W to fly forward & back if k == ord('w'): self.for_back_velocity = int(S * oSpeed) elif k == ord('s'): self.for_back_velocity = -int(S * oSpeed) else: self.for_back_velocity = 0 # a & d to pan left & right if k == ord('d'): self.yaw_velocity = int(S * oSpeed) elif k == ord('a'): self.yaw_velocity = -int(S * oSpeed) else: self.yaw_velocity = 0 # Q & E to fly up & down if k == ord('e'): self.up_down_velocity = int(S * oSpeed) elif k == ord('q'): self.up_down_velocity = -int(S * oSpeed) else: self.up_down_velocity = 0 # c & z to fly left & right if k == ord('c'): self.left_right_velocity = int(S * oSpeed) elif k == ord('z'): self.left_right_velocity = -int(S * oSpeed) else: self.left_right_velocity = 0 # Quit the software if k == 27: should_stop = True break gray = cv2.cvtColor(frameRet, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, scaleFactor=1.5, minNeighbors=2) # Target size tSize = faceSizes[tDistance] # These are our center dimensions cWidth = int(dimensions[0] / 2) cHeight = int(dimensions[1] / 2) noFaces = len(faces) == 0 # if we've given rc controls & get face coords returned if self.send_rc_control and not OVERRIDE: for (x, y, w, h) in faces: # roi_gray = gray[y:y + h, x:x + w] #(ycord_start, ycord_end) roi_color = frameRet[y:y + h, x:x + w] # setting Face Box properties fbCol = (255, 0, 0) #BGR 0-255 fbStroke = 2 # end coords are the end of the bounding box x & y end_cord_x = x + w end_cord_y = y + h end_size = w * 2 # these are our target coordinates targ_cord_x = int((end_cord_x + x) / 2) targ_cord_y = int((end_cord_y + y) / 2) + UDOffset # This calculates the vector from your face to the center of the screen vTrue = np.array((cWidth, cHeight, tSize)) vTarget = np.array((targ_cord_x, targ_cord_y, end_size)) vDistance = vTrue - vTarget # if not args.debug: # for turning if vDistance[0] < -szX: self.yaw_velocity = S # self.left_right_velocity = S2 elif vDistance[0] > szX: self.yaw_velocity = -S # self.left_right_velocity = -S2 else: self.yaw_velocity = 0 # for up & down if vDistance[1] > szY: self.up_down_velocity = S elif vDistance[1] < -szY: self.up_down_velocity = -S else: self.up_down_velocity = 0 F = 0 if abs(vDistance[2]) > acc[tDistance]: F = S # for forward back if vDistance[2] > 0: self.for_back_velocity = S + F elif vDistance[2] < 0: self.for_back_velocity = -S - F else: self.for_back_velocity = 0 # Draw the face bounding box cv2.rectangle(frameRet, (x, y), (end_cord_x, end_cord_y), fbCol, fbStroke) # Draw the target as a circle cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10, (0, 255, 0), 2) # Draw the safety zone cv2.rectangle(frameRet, (targ_cord_x - szX, targ_cord_y - szY), (targ_cord_x + szX, targ_cord_y + szY), (0, 255, 0), fbStroke) # Draw the estimated drone vector position in relation to face bounding box cv2.putText(frameRet, str(vDistance), (0, 64), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) # if there are no faces detected, don't do anything if noFaces: self.yaw_velocity = 0 self.up_down_velocity = 0 self.for_back_velocity = 0 print("NO TARGET") # Draw the center of screen circle, this is what the drone tries to match with the target coords cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2) dCol = lerp(np.array((0, 0, 255)), np.array((255, 255, 255)), tDistance + 1 / 7) if OVERRIDE: show = "OVERRIDE: {}".format(oSpeed) dCol = (255, 255, 255) else: show = "AI: {}".format(str(tDistance)) # Draw the distance choosen cv2.putText(frameRet, show, (32, 664), cv2.FONT_HERSHEY_SIMPLEX, 1, dCol, 2) # Display the resulting frame cv2.imshow(f'Tello Tracking...', frameRet) # On exit, print the battery self.tello.get_battery() # When everything done, release the capture cv2.destroyAllWindows() # Call it always before finishing. I deallocate resources. self.tello.end() def battery(self): return self.tello.get_battery()[:2] def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class Drone: def __init__(self, speed, forward_backward_speed, steering_speed, up_down_speed, save_session, save_path, distance, safety_x, safety_y, safety_z, face_recognition, face_path): # Initialize Drone self.tello = Tello() # Initialize Person Detector self.detector = detectFaces() self.face_recognition = face_recognition self.face_encoding = self.detector.getFaceEncoding(cv2.imread(face_path)) self.speed = speed self.fb_speed = forward_backward_speed self.steering_speed = steering_speed self.ud_speed = up_down_speed self.save_session = save_session self.save_path = save_path self.distance = distance self.safety_x = safety_x self.safety_y = safety_y self.safety_z = safety_z self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.flight_mode = False self.send_rc_control = False self.dimensions = (960, 720) if self.save_session: os.makedirs(self.save_path, exist_ok=True) def run(self): if not self.tello.connect(): print("Tello not connected") return elif not self.tello.set_speed(self.speed): print('Not set speed to lowest possible') return elif not self.tello.streamoff(): print("Could not stop video stream") return elif not self.tello.streamon(): print("Could not start video stream") return self.tello.get_battery() frame_read = self.tello.get_frame_read() count = 0 while True: if frame_read.stopped: frame_read.stop() break # Listen for key presses k = cv2.waitKey(20) if k == 8: self.flight_mode = not self.flight_mode elif k == 27: break elif k == ord('t'): self.tello.takeoff() self.send_rc_control = True elif k == ord('l'): self.tello.land() self.send_rc_control = False # read frame frameBGR = frame_read.frame if self.save_path: cv2.imwrite(f'{self.save_path}/{count}.jpg', frameBGR) count += 1 x_min, y_min, x_max, y_max = 0, 0, 0, 0 # detect person and get coordinates if self.face_recognition: boundingBoxes = self.detector.detectMultiple(frameBGR) for x_mi, y_mi, x_ma, y_ma in boundingBoxes: image = frameBGR[y_mi:y_ma, x_mi:x_ma] if image.shape[0] != 0 and image.shape[1] != 0: face_encoding = self.detector.getFaceEncoding(image) dist = np.linalg.norm(self.face_encoding - face_encoding) if dist < 0.95: x_min, y_min, x_max, y_max = x_mi, y_mi, x_ma, y_ma cv2.rectangle(frameBGR, (x_mi, y_mi), (x_ma, y_ma), (0, 255, 0), 2) else: cv2.rectangle(frameBGR, (x_mi, y_mi), (x_ma, y_ma), (255, 0, 0), 2) else: x_min, y_min, x_max, y_max = self.detector.detectSingle(frameBGR) cv2.rectangle(frameBGR, (x_min, y_min), (x_max, y_max), (255, 0, 0), 2) if not self.flight_mode and self.send_rc_control and x_max != 0 and y_max != 0: # these are our target coordinates targ_cord_x = int((x_min + x_max) / 2) targ_cord_y = int((y_min + y_max) / 2) # This calculates the vector from your face to the center of the screen vTrue = np.array((int(self.dimensions[0]/2), int(self.dimensions[1]/2), sizes[self.distance])) vTarget = np.array((targ_cord_x, targ_cord_y, (x_max-x_min)*2)) vDistance = vTrue - vTarget # turn drone if vDistance[0] < -self.safety_x: self.yaw_velocity = self.steering_speed elif vDistance[0] > self.safety_x: self.yaw_velocity = -self.steering_speed else: self.yaw_velocity = 0 # for up & down if vDistance[1] > self.safety_y: self.up_down_velocity = self.ud_speed elif vDistance[1] < -self.safety_y: self.up_down_velocity = -self.ud_speed else: self.up_down_velocity = 0 # forward & backward if vDistance[2] > self.safety_z: self.for_back_velocity = self.fb_speed elif vDistance[2] < self.safety_z: self.for_back_velocity = -self.fb_speed else: self.for_back_velocity = 0 # always set left_right_velocity to 0 self.left_right_velocity = 0 # Draw the target as a circle cv2.circle(frameBGR, (targ_cord_x, targ_cord_y), 10, (0, 255, 0), 2) # Draw the safety zone cv2.rectangle(frameBGR, (targ_cord_x - self.safety_x, targ_cord_y - self.safety_y), (targ_cord_x + self.safety_x, targ_cord_y + self.safety_y), (0, 255, 0), 2) elif not self.flight_mode and self.send_rc_control and x_max==0 and y_max==0: self.for_back_velocity = 0 self.left_right_velocity = 0 self.yaw_velocity = 0 self.up_down_velocity = 0 elif self.flight_mode and self.send_rc_control: # fligh forward and back if k == ord('w'): self.for_back_velocity = self.speed elif k == ord('s'): self.for_back_velocity = -self.speed else: self.for_back_velocity = 0 # fligh left & right if k == ord('d'): self.left_right_velocity = self.speed elif k == ord('a'): self.left_right_velocity = -self.speed else: self.left_right_velocity = 0 # fligh up & down if k == 38: self.up_down_velocity = self.speed elif k == 40: self.up_down_velocity = -self.speed else: self.up_down_velocity = 0 # turn if k == 37: self.yaw_velocity = self.speed elif k == 39: self.yaw_velocity = -self.speed else: self.yaw_velocity = 0 if self.send_rc_control: # Send velocities to Drone self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity) cv2.imshow('Tello Drone', frameBGR) # Destroy cv2 windows and end drone connection cv2.destroyAllWindows() self.tello.end()
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def getTakeOff(self): return self.hasTakenOff def setTakeoff(self, hasTakenOff): self.hasTakenOff = hasTakenOff def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([960, 720]) # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 ################################################################### ##Drone Project Defined variables self.hasTakenOff = False self.specifiedTarget = known_face_names[0] self.targetSeen = False self.targetLeftSide = 320 # set to default self.targetRightSide = 640 ################################################################### self.send_rc_control = False # create update timer pygame.time.set_timer(USEREVENT + 1, 50) def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False detector = dlib.get_frontal_face_detector() spinCounter = 0 while not should_stop: for event in pygame.event.get(): if event.type == USEREVENT + 1: self.update() elif event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) #dets = detector(frame) #for det in dets: # cv2.rectangle(frame, (det.left(), det.top()), (det.right(), det.bottom()), color=(0,255,0), thickness=3) # Resize frame of video to 1/4 size for faster face recognition processing small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25) # Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses) rgb_small_frame = small_frame[:, :, ::-1] # Only process every other frame of video to save time face_locations = face_recognition.face_locations(rgb_small_frame) face_encodings = face_recognition.face_encodings( rgb_small_frame, face_locations) face_names = [] for face_encoding in face_encodings: # See if the face is a match for the known face(s) matches = face_recognition.compare_faces( known_face_encodings, face_encoding) name = "Unknown" # If a match was found in known_face_encodings, just use the first one. if True in matches: first_match_index = matches.index(True) name = known_face_names[ first_match_index] # This might not actually be correct, given it only uses the first one face_names.append(name) for (top, right, bottom, left), name in zip(face_locations, face_names): # Scale back up face locations since the frame we detected in was scaled to 1/4 size top *= 4 right *= 4 bottom *= 4 left *= 4 # Draw a box around the face cv2.rectangle(frame, (left, top), (right, bottom), (0, 0, 255), 2) # Draw a label with a name below the face cv2.rectangle(frame, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED) font = cv2.FONT_HERSHEY_DUPLEX cv2.putText(frame, name, (left + 6, bottom - 6), font, 1.0, (255, 255, 255), 1) # Updates when the target is seen if (name == self.specifiedTarget): self.targetSeen = True self.targetLeftSide = left self.targetRightSide = right frame = np.rot90(frame) frame = np.flipud(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() # Circles right until face detected #while (targetSeen != True and self.hasTakenOff): #self.tello.rotate_clockwise(self, 10) #self.keydown(self, pygame.K_a) if (self.targetSeen != True and self.hasTakenOff): self.yaw_velocity = turnSpeed spinCounter = spinCounter + 1 if (spinCounter > (5 * FPS)): self.tello.land() self.send_rc_control = False self.tello.end() #break ## ends loop and land drone else: if (self.targetLeftSide < 320): #self.tello.rotate_clockwise(self, 10) # self.keydown(self, pygame.K_a) self.yaw_velocity = turnSpeed # rotate left, target outside of the middle 1/3 elif (self.targetRightSide > 640): #self.tello.rotate_counter_clockwise(self, 10) # self.keydown(self, pygame.K_d) self.yaw_velocity = -turnSpeed else: self.yaw_velocity = 0 time.sleep(1 / FPS) # Call it always before finishing. I deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.hasTakenOff = True self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() self.send_rc_control = False def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations (yaw) - W and S: Up and down. """ def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([960, 720]) # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.send_rc_control = False # create update timer pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS) def control_manual(self): key = input('entraste en control manual') time.sleep(2) if key == '': print return else: if key == 'w': print('entraste al comando') self.tello.move_forward(30) elif key == 's': self.tello.move_down(30) elif key == 'a': self.tello.move_left(30) elif key == ord('d'): self.tello.move_right(30) elif key == ord('e'): self.tello.rotate_clockwise(30) elif key == ord('q'): self.tello.rotate_counter_clockwise(30) elif key == ord('r'): self.tello.move_up(30) elif key == ord('f'): self.tello.move_down(30) print('termino control manual') def run(self): self.tello.connect() self.tello.set_speed(self.speed) # In case streaming is on. This happens when we quit this program without the escape key. self.tello.streamoff() self.tello.streamon() #self.tello.takeoff() frame_read = self.tello.get_frame_read() should_stop = False while not should_stop: for event in pygame.event.get(): if event.type == pygame.USEREVENT + 1: self.update() elif event.type == pygame.QUIT: should_stop = True elif event.type == pygame.KEYDOWN: if event.key == pygame.K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == pygame.KEYUP: self.keyup(event.key) if frame_read.stopped: break self.screen.fill([0, 0, 0]) frame = frame_read.frame text = "Battery: {}%".format(self.tello.get_battery()) cv2.putText(frame, text, (5, 720 - 5), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) frame = np.rot90(frame) frame = np.flipud(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() #x=threading.Thread(target=self.control_manual) #x.start() #x.join() #time.sleep(1 / FPS) # Call it always before finishing. To deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw counter clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw clockwise velocity self.yaw_velocity = S def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.send_rc_control = True elif key == pygame.K_l: # land not self.tello.land() self.send_rc_control = False def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. - B: Go to Ground - K: Emergency Land - Q: Emergency Motor Kill - F: Face Follow mode """ def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([960, 720]) # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.mode = None self.send_rc_control = False self.yolo = Yolo() self.yolo.initializeModel() self.tracker = tracker = cv2.TrackerCSRT().create() self.locked = False self.locked_frame = None # create update timer pygame.time.set_timer(USEREVENT + 1, 50) logger.info("Game Initialized") def run(self): if not self.tello.connect(): print("Tello not connected") logger.error("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") logger.error("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") logger.error("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") logger.error("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False while not should_stop: for event in pygame.event.get(): if event.type == USEREVENT + 1: if self.mode != None: frame_read.frame = self.get_update(frame_read.frame) self.update() elif event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) cv2.putText( img=frame, text="Height : {}".format(self.tello.get_tello_status().h), org=(0, 50), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.15 * 5, color=(255, 255, 255), ) cv2.putText( img=frame, text="Battery : {}".format(self.tello.get_tello_status().bat), org=(0, 70), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.15 * 5, color=(255, 255, 255), ) cv2.putText( img=frame, text="Temp : {} - {}".format( self.tello.get_tello_status().temph, self.tello.get_tello_status().templ, ), org=(0, 90), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.15 * 5, color=(255, 255, 255), ) frame = np.rot90(frame) frame = np.flipud(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) time.sleep((1 / FPS)) pygame.display.update() self.tello.get_tello_status() # Call it always before finishing. I deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S elif key == pygame.K_f: if self.mode == "Aquire lock" or self.mode == "Follow": logger.info("Back to normal mode") self.mode = None self.locked = False self.locked_frame = None else: logger.info("Aquiring lock") self.mode = "Aquire lock" def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if (key == pygame.K_UP or key == pygame.K_DOWN): # set zero forward/backward velocity self.for_back_velocity = 0 elif (key == pygame.K_LEFT or key == pygame.K_RIGHT): # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() logger.info("Take off") self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() logger.info("Land") self.send_rc_control = False elif key == pygame.K_b: # go to ground self.tello.go_to_ground() logger.info("Got to Ground") self.send_rc_control = False elif key == pygame.K_k: # Manual land and kill motors self.tello.emergency_land() logger.info("Emmergency land") self.send_rc_control = False # elif key ==pygame.K_q: # self.tello.emergency() # logger.info("Kill motors") # self.send_rc_control = False def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control( self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity, ) def get_update(self, frame_read): if self.mode == "Aquire lock": return self.aquire_lock(frame_read) if self.mode == "Follow": return self.follow(frame_read) def face_follow(self, frame_read): cv2.cvtColor(frame_read, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(frame_read, 1.3, 5) frame_x = frame_read.shape[1] frame_y = frame_read.shape[0] face_center_x = 0 for (x, y, w, h) in faces: cv2.rectangle(frame_read, (x, y), (x + w, y + h), (255, 0, 0), 2) face_center_x = x + (w / 2) - frame_x break if face_center_x > 200: self.yaw_velocity = -S if face_center_x < -200: self.yaw_velocity = S else: self.yaw_velocity = 0 logger.info("Frame_x: {} \tface_center: {} \tyaw_vel: {}".format( frame_x, face_center_x, self.yaw_velocity)) return frame_read def aquire_lock(self, frame): bbox, _, _ = self.yolo.detect(frame, "person") if len(bbox) > 0: self.locked = True self.locked_frame = [int(i) for i in bbox[0]] logger.info("Lock Aquired : {}".format(self.locked_frame)) self.mode = "Follow" self.tracker.init( frame, ( self.locked_frame[0], self.locked_frame[1], self.locked_frame[0] + self.locked_frame[2], self.locked_frame[1] + self.locked_frame[3], ), ) return self.follow(frame) return frame def follow(self, frame_read): ok, self.locked_frame = self.tracker.update(frame_read) self.locked_frame = [int(i) for i in self.locked_frame] frame_shape = (frame_read.shape[1], frame_read.shape[0]) logger.info("Locked Frame : {}".format(self.locked_frame)) if ok: self.calcMovementVector(frame_shape, self.locked_frame) cv2.rectangle( frame_read, (int(self.locked_frame[0]), self.locked_frame[1]), ( self.locked_frame[0] + self.locked_frame[2], self.locked_frame[1] + self.locked_frame[3], ), (255, 125, 0), 2, ) else: self.mode = "Aquire Lock" self.locked = False self.locked_frame = None return frame_read def calcMovementVector(self, frame_shape, frame): frame_center = (int(frame_shape[0] / 2), int(frame_shape[1] / 2)) x_mov = (frame[0] + frame[2] / 2) - frame_center[0] logger.info("X mov : {}".format(x_mov)) if x_mov > 50 or x_mov < -50: self.yaw_velocity = int(S * x_mov / frame_center[0])
class DroneUI(object): def __init__(self): # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.mode = PMode.NONE # Can be '', 'FIND', 'OVERRIDE' or 'FOLLOW' self.send_rc_control = False def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return if args.cat == 'any': print('Using CatDetectionModel') self.model = CatDetectionModel(0.5) else: print('Using MyCatsDetectionModel ({})'.format(args.cat)) self.model = MyCatsDetectionModel(0.5) frame_read = self.tello.get_frame_read() should_stop = False imgCount = 0 OVERRIDE = False DETECT_ENABLED = False # Set to true to automatically start in follow mode self.mode = PMode.NONE self.tello.get_battery() # Safety Zone X szX = args.saftey_x # Safety Zone Y szY = args.saftey_y if args.debug: print("DEBUG MODE ENABLED!") while not should_stop: frame_time_start = time.time() # self.update() # Moved to the end before sleep to have more accuracy if frame_read.stopped: frame_read.stop() self.update() ## Just in case break print('---') # TODO: Analize if colors have to be tweaked frame = cv2.flip(frame_read.frame, 0) # Vertical flip due to the mirror frameRet = frame.copy() vid = self.tello.get_video_capture() imgCount += 1 #time.sleep(1 / FPS) # Listen for key presses k = cv2.waitKey(20) try: if chr(k) in 'ikjluoyhp': OVERRIDE = True except: ... if k == ord('e'): DETECT_ENABLED = True elif k == ord('d'): DETECT_ENABLED = False # Press T to take off if k == ord('t'): if not args.debug: print("Taking Off") self.tello.takeoff() self.tello.get_battery() self.send_rc_control = True if k == ord('s') and self.send_rc_control == True: self.mode = PMode.FIND DETECT_ENABLED = True # To start following with autopilot OVERRIDE = False print('Switch to spiral mode') # This is temporary, follow mode should start automatically if k == ord('f') and self.send_rc_control == True: DETECT_ENABLED = True OVERRIDE = False print('Switch to follow mode') # Press L to land if k == ord('g'): self.land_and_set_none() # self.update() ## Just in case # break # Press Backspace for controls override if k == 8: if not OVERRIDE: OVERRIDE = True print("OVERRIDE ENABLED") else: OVERRIDE = False print("OVERRIDE DISABLED") # Quit the software if k == 27: should_stop = True self.update() ## Just in case break autoK = -1 if k == -1 and self.mode == PMode.FIND: if not OVERRIDE: autoK = next_auto_key() if autoK == -1: self.mode = PMode.NONE print('Queue empty! no more autokeys') else: print('Automatically pressing ', chr(autoK)) key_to_process = autoK if k == -1 and self.mode == PMode.FIND and OVERRIDE == False else k if self.mode == PMode.FIND and not OVERRIDE: #frame ret will get the squares drawn after this operation if self.process_move_key_andor_square_bounce( key_to_process, frame, frameRet) == False: # If the queue is empty and the object hasn't been found, land and finish self.land_and_set_none() #self.update() # Just in case break else: self.process_move_key(key_to_process) dCol = (0, 255, 255) #detected = False if not OVERRIDE and self.send_rc_control and DETECT_ENABLED: self.detect_subjects(frame, frameRet, szX, szY) show = "" if OVERRIDE: show = "MANUAL" dCol = (255, 255, 255) elif self.mode == PMode.FOLLOW or self.mode == PMode.FLIP: show = "FOUND!!!" elif self.mode == PMode.FIND: show = "Finding.." mode_label = 'Mode: {}'.format(self.mode) # Draw the distance choosen cv2.putText(frameRet, mode_label, (32, 664), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) cv2.putText(frameRet, show, (32, 600), cv2.FONT_HERSHEY_SIMPLEX, 1, dCol, 2) # Display the resulting frame cv2.imshow('FINDER DRONE', frameRet) if (self.mode == PMode.FLIP): self.flip() # OVERRIDE = True self.update( ) # Moved here instead of beginning of loop to have better accuracy frame_time = time.time() - frame_time_start sleep_time = 1 / FPS - frame_time if sleep_time < 0: sleep_time = 0 print('SLEEEP TIME NEGATIVE FOR FRAME {} ({}s).. TURNING IT 0'. format(imgCount, frame_time)) if args.save_session and self.send_rc_control == True: # To avoid recording before takeoff self.create_frame_files(frame, frameRet, imgCount) time.sleep(sleep_time) # On exit, print the battery self.tello.get_battery() # When everything done, release the capture # cv2.destroyWindow('FINDER DRONE') # cv2.waitKey(0) cv2.destroyAllWindows() # Call it always before finishing. I deallocate resources. self.tello.end() def create_frame_files(self, frame, frameRet, imgCount): def create_frame_file(image, subdir, print_log=False): global ddir path = ddir + '/' + subdir if not os.path.exists(path): os.makedirs(path) filename = "{}/tellocap{}.jpg".format(path, imgCount) if print_log: print('Created {}'.format(filename)) cv2.imwrite(filename, image) create_frame_file(frame, 'raw') create_frame_file(frameRet, 'output', True) def flip(self): print('Flip!') self.left_right_velocity = self.for_back_velocity = 0 self.update() time.sleep(self.tello.TIME_BTW_COMMANDS * 2) if not args.debug: self.tello.flip_left() #self.tello.flip_right() # The following 2 lines allow going back to follow mode self.mode = PMode.FOLLOW global onFoundAction onFoundAction = PMode.FOLLOW # So it doesn't flip over and over def land_and_set_none(self): if not args.debug: print("------------------Landing--------------------") self.tello.land() self.send_rc_control = False self.mode = PMode.NONE # TODO: Consider calling reset def oq_discard_keys(self, keys_to_pop): oq = globals.mission.operations_queue keys_to_pop += 'p' while len(oq) > 0: oqkey = oq[0]['key'] if oqkey in keys_to_pop: print('Removing {} from queue'.format(oqkey)) oq.popleft() else: break def process_move_key_andor_square_bounce(self, k, frame, frameRet=None): self.process_move_key(k) # By default use key direction (hor_dir, ver_dir) = get_squares_push_directions(frame, frameRet) print('(hor_dir, ver_dir): ({}, {})'.format(hor_dir, ver_dir)) oq = globals.mission.operations_queue print('operations_queue len: ', len(oq)) keys_to_pop = '' if ver_dir == 'forward': self.for_back_velocity = int(S) if k != ord('i'): print('Square pushing forward') keys_to_pop += 'k' elif ver_dir == 'back': self.for_back_velocity = -int(S) if k != ord('k'): print('Square pushing back') keys_to_pop += 'i' if hor_dir == 'right': self.left_right_velocity = int(S) if k != ord('l'): print('Square pushing right') keys_to_pop += 'j' elif hor_dir == 'left': self.left_right_velocity = -int(S) if k != ord('j'): print('Square pushing left') keys_to_pop += 'l' if (len(keys_to_pop) > 0): self.oq_discard_keys(keys_to_pop) return (len(oq) > 0) def process_move_key(self, k): # i & k to fly forward & back if k == ord('i'): self.for_back_velocity = int(S) elif k == ord('k'): self.for_back_velocity = -int(S) else: self.for_back_velocity = 0 # o & u to pan left & right if k == ord('o'): self.yaw_velocity = int(S) elif k == ord('u'): self.yaw_velocity = -int(S) else: self.yaw_velocity = 0 # y & h to fly up & down if k == ord('y'): self.up_down_velocity = int(S) elif k == ord('h'): self.up_down_velocity = -int(S) else: self.up_down_velocity = 0 # l & j to fly left & right if k == ord('l'): self.left_right_velocity = int(S) elif k == ord('j'): self.left_right_velocity = -int(S) else: self.left_right_velocity = 0 # p to keep still if k == ord('p'): print('pressing p') def show_save_detection(self, frame, frameRet, firstDetection): output_filename_det_full = "{}/detected_full.jpg".format(ddir) cv2.imwrite(output_filename_det_full, frameRet) print('Created {}'.format(output_filename_det_full)) (x, y, w, h) = firstDetection['box'] add_to_borders = 100 (xt, yt) = (x + w + add_to_borders, y + h + add_to_borders) (x, y) = (max(0, x - add_to_borders), max(0, y - add_to_borders)) # subframeRet = frameRet[y:yt, x:xt].copy() subframe = frame[y:yt, x:xt].copy() def show_detection(): output_filename_det_sub = "{}/detected_sub.jpg".format(ddir) cv2.imwrite(output_filename_det_sub, subframe) print('Created {}'.format(output_filename_det_sub)) # Shows detection in a window. If it doesn't exist yet, waitKey waitForKey = cv2.getWindowProperty('Detected', 0) < 0 # True for first time cv2.imshow('Detected', subframe) if waitForKey: cv2.waitKey(0) Timer(0.5, show_detection).start() def detect_subjects(self, frame, frameRet, szX, szY): detections = self.model.detect(frameRet) # print('detections: ', detections) self.model.drawDetections(frameRet, detections) class_wanted = 0 if args.cat == 'any' else self.model.LABELS.index( args.cat) detection = next( filter(lambda d: d['classID'] == class_wanted, detections), None) isSubjectDetected = not detection is None if isSubjectDetected: print('{} FOUND!!!!!!!!!!'.format(self.model.LABELS[class_wanted])) #if self.mode != onFoundAction: # To create it only the first time self.mode = onFoundAction # if we've given rc controls & get object coords returned # if self.send_rc_control and not OVERRIDE: if self.mode == PMode.FOLLOW: self.follow(detection, frameRet, szX, szY) self.show_save_detection(frame, frameRet, detection) elif self.mode == onFoundAction: # if there are no objects detected, don't do anything print("CAT NOT DETECTED NOW") return isSubjectDetected def follow(self, detection, frameRet, szX, szY): print('Following...') # These are our center dimensions (frame_h, frame_w) = frameRet.shape[:2] cWidth = int(frame_w / 2) cHeight = int(frame_h / 2) (x, y, w, h) = detection['box'] # end coords are the end of the bounding box x & y end_cord_x = x + w end_cord_y = y + h # This is not face detection so we don't need offset UDOffset = 0 # these are our target coordinates targ_cord_x = int((end_cord_x + x) / 2) targ_cord_y = int((end_cord_y + y) / 2) + UDOffset # This calculates the vector from the object to the center of the screen vTrue = np.array((cWidth, cHeight)) vTarget = np.array((targ_cord_x, targ_cord_y)) vDistance = vTrue - vTarget if True or not args.debug: if vDistance[0] < -szX: # Right self.left_right_velocity = S elif vDistance[0] > szX: # Left self.left_right_velocity = -S else: self.left_right_velocity = 0 # for up & down if vDistance[1] > szY: self.for_back_velocity = S elif vDistance[1] < -szY: self.for_back_velocity = -S else: self.for_back_velocity = 0 # Draw the center of screen circle, this is what the drone tries to match with the target coords cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2) # Draw the target as a circle cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10, (0, 255, 0), 2) # Draw the safety zone obStroke = 2 cv2.rectangle(frameRet, (targ_cord_x - szX, targ_cord_y - szY), (targ_cord_x + szX, targ_cord_y + szY), (0, 255, 0), obStroke) # Draw the estimated drone vector position in relation to object bounding box cv2.putText(frameRet, str(vDistance), (0, 64), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) def battery(self): return self.tello.get_battery()[:2] def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: print('Sending speeds to tello. H: {} V: {}'.format( self.left_right_velocity, self.for_back_velocity)) if not args.debug: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([960, 720]) # Init Tello object that interacts with the Tello drone self.tello = Tello() self.faceCascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_alt2.xml') # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.show_stats = False self.send_rc_control = False # create update timer pygame.time.set_timer(USEREVENT + 1, 50) def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False while not should_stop: frameRet = frame_read.frame for event in pygame.event.get(): if event.type == USEREVENT + 1: self.update() elif event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) gray = cv2.cvtColor(frameRet, cv2.COLOR_BGR2GRAY) faces = self.faceCascade.detectMultiScale(gray, scaleFactor=1.5, minNeighbors=2) for (x, y, w, h) in faces: fbCol = (255, 0, 0) #BGR 0-255 fbStroke = 2 end_cord_x = x + w end_cord_y = y + h cv2.rectangle(frameRet, (x, y), (end_cord_x, end_cord_y), fbCol, fbStroke) if self.show_stats: cv2.putText(frameRet, "Batt: " + str(self.tello.get_battery()),(0,32),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2) cv2.putText(frameRet, "Wifi: " + str(self.tello.get_wifi()),(0,64),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2) cv2.putText(frameRet, "Faces: " + str(len(faces)),(0,96),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2) frameRet = cv2.cvtColor(frameRet,cv2.COLOR_BGR2RGB) frameRet = np.rot90(frameRet) frameRet = np.flipud(frameRet) frame = pygame.surfarray.make_surface(frameRet) self.screen.blit(frame, (0, 0)) pygame.display.update() time.sleep(1 / FPS) # Call it always before finishing. I deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() self.send_rc_control = False elif key == pygame.K_h: # stats self.show_stats = not self.show_stats def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. - P: enter/exit follow person mode. """ def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([960, 360]) # create object detector coco_frozen_graph_path = './ssd_mobilenet_v2_coco_2018_03_29/frozen_inference_graph.pb' coco_labels_path = './ssd_mobilenet_v2_coco_2018_03_29/mscoco_label_map.pbtxt' self.coco_obj_detector = Object_Detector(coco_frozen_graph_path, coco_labels_path, debug_mode=True) # create object detector face_frozen_graph_path = './ssd_face_detection/frozen_inference_graph_face.pb' face_labels_path = './ssd_face_detection/face_label_map.pbtxt' self.face_obj_detector = Object_Detector(face_frozen_graph_path, face_labels_path, debug_mode=True) #create tracker object self.tracker = Object_Tracker(class_id=1) # create depth detector self.depth_detector = Depth_Detector() self.object_depth_detector = Object_Depth_Detector() #create planner self.planner = Planner() self.face_bounding_box = [ 0.3, 0.4, 0.5, 0.6, 0.1, 0.2 ] #[min_y, min_x, max_y, max_x, min_size, max_size] self.person_bounding_box = [ 0.45, 0.45, 0.55, 0.55, 0.3, 0.5 ] #[min_y, min_x, max_y, max_x, min_size, max_size] # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.follow_human = False self.send_rc_control = False # create update timer pygame.time.set_timer(USEREVENT + 1, 50) def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False while not should_stop: print('Battery: ' + str(self.tello.get_battery()) + '%') for event in pygame.event.get(): if event.type == USEREVENT + 1: self.update() elif event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) current_frame = frame_read.frame #get depth map # depth_map, depth_img = self.depth_detector.detect_depth(current_frame) #detect objects output_dict, img = self.face_obj_detector.detect_objects( current_frame) #track object box = self.tracker.track_object(current_frame, output_dict) bounding_box = self.face_bounding_box # if box is None:#not found a face for n frames (where n is defined in tracker constructor) # #so use coco object detector instead to look for a person # #detect objects # output_dict, img = self.coco_obj_detector.detect_objects(current_frame) # #track object # box = self.tracker.track_object(current_frame, output_dict) # bounding_box = self.person_bounding_box #get objects depth # depth = self.object_depth_detector.get_depth(box, depth_map) depth = None # temp to_show = concat_images(img, current_frame) commands = self.planner.follow_object(box, depth, bounding_box, 60) if self.follow_human: for c in commands: key, speed = c self.make_move(key, speed) frame = cv2.cvtColor(to_show, cv2.COLOR_BGR2RGB) frame = np.rot90(frame) frame = np.flipud(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() # time.sleep(1 / FPS) # Call it always before finishing. I deallocate resources. self.tello.end() def make_move(self, key, speed): if key == 'FORWARD': # set forward velocity self.for_back_velocity = speed elif key == 'BACK': # set backward velocity self.for_back_velocity = -speed elif key == 'LEFT': # set left velocity self.left_right_velocity = -speed elif key == 'RIGHT': # set right velocity self.left_right_velocity = speed elif key == 'UP': # set up velocity self.up_down_velocity = speed elif key == 'DOWN': # set down velocity self.up_down_velocity = -speed elif key == 'CLOCKWISE': # set yaw clockwise velocity self.yaw_velocity = -speed elif key == 'ANTICLOCKWISE': # set yaw counter clockwise velocity self.yaw_velocity = speed def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S elif key == pygame.K_p: self.follow_human = not self.follow_human def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() self.send_rc_control = False def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame pygame.init() # Create pygame window pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([SCREEN_WIDTH, SCREEN_HEIGHT]) # Init Tello object that interacts with the Tello drone if WEBCAM: self.tello = MockTello() else: self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.send_rc_control = False # create update timer pygame.time.set_timer(USEREVENT + 1, 50) def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False while not should_stop: for event in pygame.event.get(): if event.type == USEREVENT + 1: self.update() elif event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) frame = frame_read.frame frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) # frame = np.flipud(frame) gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY) faces = faceCascade.detectMultiScale(gray, scaleFactor=1.1, minNeighbors=5, minSize=(30, 30), flags=cv2.CASCADE_SCALE_IMAGE) # frame = cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB) print(len(faces)) for (x, y, w, h) in faces: cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2) if len(faces) > 0: center_x = x + w / 2 center_y = y + h / 2 self.yaw_velocity = S if center_x < SCREEN_WIDTH / 2 else -S self.up_down_velocity = S if center_y < SCREEN_HEIGHT / 2 else -S else: self.yaw_velocity = 0 self.up_down_velocity = 0 frame = np.rot90(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() time.sleep(1 / FPS) # Call it always before finishing. I deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() self.send_rc_control = False def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Video FPV Tello") self.screen = pygame.display.set_mode([960, 720]) # Creat pygame fuente myFont = pygame.font.Font(None, 30) self.mitexto = myFont.render("prueba Pantalla", 0, (200, 60, 80)) # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.send_rc_control = False # create update timer pygame.time.set_timer(USEREVENT + 1, 50) def run(self): if not self.tello.connect(): print("Drone no conectado") return if not self.tello.set_speed(self.speed): print("No es posible menos velocidad") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("no se pudo parar el etreaming") return if not self.tello.streamon(): print("no se pudo iniciar el etreaming") return frame_read = self.tello.get_frame_read() should_stop = False while not should_stop: for event in pygame.event.get(): if event.type == USEREVENT + 1: self.update() elif event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) frame = np.rot90(frame) frame = np.flipud(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) self.screen.blit(self.mitexto, (100, 100)) time.sleep(1 / FPS) # Face detecction gray = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, scaleFactor=1.3, minNeighbors=4, minSize=(30, 30)) for (x, y, w, h) in faces: # print (x,y,w,h) color = (255, 0, 0) #BGR color stroke = 2 end_coord_x = x + w end_coord_y = y + h cv2.rectangle(frame_read.frame, (x, y), (end_coord_x, end_coord_y), color, stroke) pygame.display.update() print('Response: ' + str(self.tello.send_read_command('battery?'))) # Call it always before finishing. I deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() self.send_rc_control = False def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame # Init Tello object that interacts with the Tello drone self.tello = Tello() self.telloPose = np.zeros((1,3)) self.telloEulerAngles = np.zeros((1,3)) self.rcOut=np.zeros(4) self.poseQueue = np.zeros((7,3)) self.supreQueue = np.zeros((7,3)) self.flag = 0 self.telloPoseVariance = np.zeros(3) self.telloPoseMean = np.zeros(3) self.tello.TIME_BTW_RC_CONTROL_COMMANDS = 20 self.R = np.zeros((3,3)) # self.telloPose = np.array([]) # self.telloEulerAngles = EulerAngles def run(self): if not self.tello.connect(): print("Tello not connected") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False Height = 100 while not should_stop: if frame_read.stopped: frame_read.stop() break frameBGR = np.copy(frame_read.frame) # frameBGR = np.rot90(frameBGR) # frameBGR = np.flipud(frameBGR) frame2use = im.resize(frameBGR,width=720) key = cv2.waitKey(1) & 0xFF; K = np.array([[7.092159469231584126e+02,0.000000000000000000e+00,3.681653710406367850e+02],[0.000000000000000000e+00,7.102890453175559742e+02,2.497677007139825491e+02],[0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00]]) dist = np.array([-1.428750372096417864e-01,-3.544750945429044758e-02,1.403740315118516459e-03,-2.734988255518019593e-02,1.149084393996809700e-01]) K_inv = np.linalg.inv(K) h , w = frame2use.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix(K,dist,(w,h),1,(w,h)) mapx,mapy = cv2.initUndistortRectifyMap(K,dist,None,newcameramtx,(w,h),5) dst = cv2.remap(frame2use,mapx,mapy,cv2.INTER_LINEAR) x,y,w,h = roi dst = dst[y:y+h,x:x+w] # print("ROI: ",x,y,w,h) cv2.imshow("Orignal",frame2use) cv2.imshow("rectified",dst) # gray = cv2.cvtColor(frame2use, cv2.COLOR_BGR2GRAY) gray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY) detector = apriltag.Detector() result = detector.detect(gray) if len(result) != 0: self.flag = 1 else : self.flag = 0 self.FindPose(result,K_inv) if self.flag == 1: varN = np.linalg.norm(self.telloPoseVariance) print "varN",varN Pose = self.telloPoseMean xEr = 0 - Pose[0] yEr = 0 - Pose[1] zEr = Height - Pose[2] ErrorN = np.linalg.norm([xEr,yEr,zEr]) print "Height",Height print "ErrorN",ErrorN if varN < 370 and key == ord("e"): if varN < 120 and ErrorN < 10: Height = Height -5 print "#######################################################################" if Height <30: self.tello.land() kp = 3 MtnCmd = np.matmul(1,[kp*xEr,kp*yEr,kp*zEr]) self.rcOut = [MtnCmd[0],MtnCmd[1],MtnCmd[2],0] if self.rcOut[0] > 60: self.rcOut[0] = 60 elif self.rcOut[0] < -60: self.rcOut[0] = -60 if self.rcOut[1] > 60: self.rcOut[1] = 60 elif self.rcOut[1] < -60: self.rcOut[1] = -60 if self.rcOut[2] > 60: self.rcOut[2] = 60 elif self.rcOut[2] < -60: self.rcOut[2] = -60 # elif self.rcOut[1] > 60: # self.rcOut[1] = 60 # elif self.rcOut[1] < -60: # self.rcOut[1] = -60 else : if key == ord("w"): self.rcOut[1] = 50 elif key == ord("a"): self.rcOut[0] = -50 elif key == ord("s"): self.rcOut[1] = -50 elif key == ord("d"): self.rcOut[0] = 50 elif key == ord("u"): self.rcOut[2] = 50 elif key == ord("j"): self.rcOut[2] = -50 else: self.rcOut = [0,0,0,0] else: if key == ord("w"): self.rcOut[1] = 50 elif key == ord("a"): self.rcOut[0] = -50 elif key == ord("s"): self.rcOut[1] = -50 elif key == ord("d"): self.rcOut[0] = 50 elif key == ord("u"): self.rcOut[2] = 50 elif key == ord("j"): self.rcOut[2] = -50 else: self.rcOut = [0,0,0,0] print self.rcOut self.tello.send_rc_control(int(self.rcOut[0]),int(self.rcOut[1]),int(self.rcOut[2]),int(self.rcOut[3])) self.rcOut = [0,0,0,0] if key == ord("q"): break if key == ord("t"): self.tello.takeoff() if key == ord("l"): self.tello.land() Height = 100 if cv2.waitKey(1) & 0xFF == ord('q'): break time.sleep(1 / FPS) # Call it always before finishing. I deallocate resources. self.tello.end() def FindPose(self,result,K_inv): if self.flag == 1: center = result[0].center H = np.array(result[0].homography) # print H h1h2h3 = np.matmul(K_inv,H) h1T = h1h2h3[:,0] h2T = h1h2h3[:,1] h3T = h1h2h3[:,2] h1Xh2T = np.cross(h1T,h2T) h1_h2_h1Xh2T = np.array([h1T,h2T,h1Xh2T]) h1_h2_h1Xh2 = np.transpose(h1_h2_h1Xh2T) u, s, vh = np.linalg.svd(h1_h2_h1Xh2, full_matrices=True) uvh = np.matmul(u,vh) det_OF_uvh = np.linalg.det(uvh) M = np.array([[1,0,0],[0,1,0],[0,0,det_OF_uvh]]) T = h3T/np.linalg.norm(h1T) # Translation Matrix T = T*100/17.5 r = np.matmul(u,M) R = np.matmul(r,vh) # Rotation matrix T = T self.R = R T_t = np.reshape(T,(3,1)) neg_Rt_T = -1*np.dot(R.T,T_t) f = np.array([[0,0,0,1]]) if neg_Rt_T[2,0] < 0: flag = -1 else: flag = 1 neg_Rt_T[2,0] = neg_Rt_T[2,0]*flag neg_Rt_T[0,0] = neg_Rt_T[0,0]*(-1) Pose = neg_Rt_T.T EulerAngles = rotationMatrixToEulerAngles(R.T) self.telloPose = Pose self.telloEulerAngles = EulerAngles self.poseQueue = np.roll(self.poseQueue,1,axis = 0) self.poseQueue[0,:] = [Pose[0,0],Pose[0,1],Pose[0,2]] self.telloPoseVariance = np.var(self.poseQueue,axis=0) self.telloPoseMean = np.mean(self.poseQueue,axis = 0) # print "PoseQueue",self.poseQueue print "PoseMean",self.telloPoseMean # print"telloPoseVariance",self.telloPoseVariance # Pose,EulerAngles return
def main(): # init global vars global gesture_buffer global gesture_id global battery_status # Argument parsing args = get_args() KEYBOARD_CONTROL = args.is_keyboard WRITE_CONTROL = False in_flight = False # Camera preparation tello = Tello() #print(dir(tello)) tello.connect() tello.set_speed(speed["slow"]) print("\n\n" + tello.get_speed() + "\n\n") tello.streamon() cap_drone = tello.get_frame_read() cap_webcam = cv.VideoCapture(0) # Init Tello Controllers gesture_controller = TelloGestureController(tello) keyboard_controller = TelloKeyboardController(tello) gesture_detector = GestureRecognition(args.use_static_image_mode, args.min_detection_confidence, args.min_tracking_confidence) gesture_buffer = GestureBuffer(buffer_len=args.buffer_len) def tello_control(key, keyboard_controller, gesture_controller): global gesture_buffer if KEYBOARD_CONTROL: keyboard_controller.control(key) else: gesture_controller.gesture_control(gesture_buffer) def tello_battery(tello): global battery_status battery_status = tello.get_battery() # FPS Measurement cv_fps_calc = CvFpsCalc(buffer_len=10) mode = 0 number = -1 battery_status = -1 tello.move_down(20) while True: fps = cv_fps_calc.get() # Process Key (ESC: end) key = cv.waitKey(1) & 0xff if key == 27: # ESC break elif key == 32: # Space if not in_flight: # Take-off drone tello.takeoff() in_flight = True elif in_flight: # Land tello tello.land() in_flight = False elif key == ord('k'): mode = 0 KEYBOARD_CONTROL = True WRITE_CONTROL = False tello.send_rc_control(0, 0, 0, 0) # Stop moving elif key == ord('g'): KEYBOARD_CONTROL = False elif key == ord('n'): mode = 1 WRITE_CONTROL = True KEYBOARD_CONTROL = True if WRITE_CONTROL: number = -1 if 48 <= key <= 57: # 0 ~ 9 number = key - 48 # Camera capture image_drone = cap_drone.frame image = cap_webcam.read()[1] try: debug_image, gesture_id = gesture_detector.recognize( image, number, mode) gesture_buffer.add_gesture(gesture_id) # Start control thread threading.Thread(target=tello_control, args=( key, keyboard_controller, gesture_controller, )).start() threading.Thread(target=tello_battery, args=(tello, )).start() debug_image = gesture_detector.draw_info(debug_image, fps, mode, number) battery_str_postion = (5, 100) # dustin webcam #battery_str_postion = (5, 720 - 5) # drone camera # Battery status and image rendering cv.putText(debug_image, "Battery: {}".format(battery_status), battery_str_postion, cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) modeStr = "gestures" if KEYBOARD_CONTROL: modeStr = "keyboard" cv.putText(debug_image, modeStr + " control", (5, 150), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) cv.imshow('Webcam Gesture Recognition', debug_image) cv.imshow('Tello drone camera', image_drone) except: print("exception") tello.land() tello.end() cv.destroyAllWindows()
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame # Init Tello object that interacts with the Tello drone self.tello = Tello() self.telloPose = np.zeros((1, 3)) self.telloEulerAngles = np.zeros((1, 3)) self.rcOut = np.zeros(4) self.poseQueue = np.zeros((7, 3)) self.supreQueue = np.zeros((7, 3)) self.flag = 0 self.telloPoseVariance = np.zeros(3) self.telloPoseMean = np.zeros(3) self.tello.TIME_BTW_RC_CONTROL_COMMANDS = 20 self.R = np.zeros((3, 3)) # self.telloPose = np.array([]) # self.telloEulerAngles = EulerAngles def run(self): if not self.tello.connect(): print("Tello not connected") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False Height = 100 while not should_stop: if frame_read.stopped: frame_read.stop() break frameBGR = np.copy(frame_read.frame) # frameBGR = np.rot90(frameBGR) # frameBGR = np.flipud(frameBGR) frame2use = im.resize(frameBGR, width=720) frame = frame2use kernel = np.ones((5, 5), np.uint8) #param 1 blurred = cv2.GaussianBlur(frame, (7, 7), 0) #param 1 hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV) h, s, v = cv2.split(hsv) dilS = cv2.dilate(s, kernel, iterations=1) newS = dilS - s newS = cv2.equalizeHist(newS) # newS = cv2.GaussianBlur(newS, (11, 11), 0) dilV = cv2.dilate(v, kernel, iterations=1) #param 1 newV = dilV - v newV = cv2.equalizeHist(newV) dilH = cv2.dilate(h, kernel, iterations=1) newH = dilH - h newH = cv2.equalizeHist(newH) sabKaAnd = cv2.bitwise_or(newS, newV) kernel2 = np.ones((3, 3), np.uint8) #param 1 sabKaAnd = cv2.erode(sabKaAnd, kernel2, iterations=1) #param 1 sabKaAnd = cv2.erode(sabKaAnd, kernel2, iterations=1) #param 1 sabKaAnd = cv2.dilate(sabKaAnd, kernel2, iterations=1) #param 1 sabKaAnd = cv2.GaussianBlur(sabKaAnd, (11, 11), 0) maskSab = cv2.inRange(sabKaAnd, 120, 255) #param 1**** maskSab = cv2.erode(maskSab, kernel2, iterations=1) maskSab = cv2.dilate(maskSab, kernel2, iterations=1) maskSab = cv2.bitwise_and(maskSab, newV) maskSab = cv2.equalizeHist(maskSab) maskSab = cv2.inRange(maskSab, 190, 255) # param ***** kernel2 = np.ones((2, 2), np.uint8) #param **** maskSab = cv2.erode(maskSab, kernel2, iterations=1) maskSab = cv2.dilate(maskSab, kernel2, iterations=1) cv2.imshow("fff", frame) mask = maskSab # Contours detection _, contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) oldArea = 300 for cnt in contours: area = cv2.contourArea(cnt) approx = cv2.approxPolyDP(cnt, 0.012 * cv2.arcLength(cnt, True), True) # 0.012 param x = approx.ravel()[0] y = approx.ravel()[1] if area > 300: #param # if len(approx) == 3: # cv2.putText(frame, "Triangle", (x, y), font, 1, (0, 0, 0)) if len(approx) == 4: (cx, cy), (MA, ma), angle = cv2.fitEllipse(cnt) ar = MA / ma # area = cv2.contourArea(cnt) hull = cv2.convexHull(cnt) hull_area = cv2.contourArea(hull) solidity = float(area) / hull_area # print "Angle",angle # print "solidity",solidity # print "ar",ar if solidity > 0.9 and ar < 0.4: if area > oldArea: cv2.drawContours(frame, [approx], 0, (0, 0, 0), 5) cv2.circle(frame, (int(cx), int(cy)), 3, (0, 0, 255), -1) cv2.putText(frame, "Rectangle" + str(angle), (x, y), font, 1, (0, 0, 0)) cntMain = approx rect = order_points(cntMain) print "rect", rect Pose = PoseEstimation(rect) pX = Pose[0, 0] pY = Pose[0, 1] pZ = Pose[0, 2] Pose[0, 0] = pZ Pose[0, 1] = -pX Pose[0, 2] = -pY self.telloPose = np.transpose(Pose) self.poseQueue = np.roll(self.poseQueue, 1, axis=0) self.poseQueue[0, :] = [ Pose[0, 0], Pose[0, 1], Pose[0, 2] ] self.telloPoseVariance = np.var(self.poseQueue, axis=0) self.telloPoseMean = np.mean(self.poseQueue, axis=0) self.flag = 1 # print "PoseQueue",self.poseQueue print "PoseMean", self.telloPoseMean # print "telloPoseVariance" , self.telloPoseVariance varN = np.linalg.norm(self.telloPoseVariance) print "varN", varN oldArea = area cv2.imshow("Frame", frame) cv2.imshow("Mask", mask) key = cv2.waitKey(1) & 0xFF K = np.array([[ 6.981060802052014651e+02, 0.000000000000000000e+00, 3.783628172155137577e+02 ], [ 0.000000000000000000e+00, 6.932839845949604296e+02, 2.823973488087042369e+02 ], [ 0.000000000000000000e+00, 0.000000000000000000e+00, 1.000000000000000000e+00 ]]) dist = np.array([ -1.128288663079663086e-02, 1.551794079596884035e-02, 3.003426614702892333e-03, 1.319203673619398672e-03, 1.086713281720452368e-01 ]) K_inv = np.linalg.inv(K) h, w = frame2use.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix( K, dist, (w, h), 1, (w, h)) mapx, mapy = cv2.initUndistortRectifyMap(K, dist, None, newcameramtx, (w, h), 5) dst = cv2.remap(frame2use, mapx, mapy, cv2.INTER_LINEAR) x, y, w, h = roi dst = dst[y:y + h, x:x + w] # print("ROI: ",x,y,w,h) cv2.imshow("Orignal", frame2use) cv2.imshow("rectified", dst) # gray = cv2.cvtColor(frame2use, cv2.COLOR_BGR2GRAY) gray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY) if self.flag == 1: varN = np.linalg.norm(self.telloPoseVariance) # print "varN",varN Pose = self.telloPoseMean xEr = 450 - Pose[0] yEr = 0 - Pose[1] zEr = 0 - Pose[2] ErrorN = np.linalg.norm([xEr, yEr, zEr]) if key == ord("e"): kp = 0.35 MtnCmd = np.array([kp * xEr, kp * yEr, kp * zEr]) MtnCmd[0] = -1 * MtnCmd[0] self.rcOut = [MtnCmd[1], MtnCmd[0], MtnCmd[2], 0] if self.rcOut[0] > 35: self.rcOut[0] = 35 elif self.rcOut[0] < -35: self.rcOut[0] = -35 if self.rcOut[1] > 35: self.rcOut[1] = 35 elif self.rcOut[1] < -35: self.rcOut[1] = -35 if self.rcOut[2] > 35: self.rcOut[2] = 35 elif self.rcOut[2] < -35: self.rcOut[2] = -35 else: if key == ord("w"): self.rcOut[1] = 50 elif key == ord("a"): self.rcOut[0] = -50 elif key == ord("s"): self.rcOut[1] = -50 elif key == ord("d"): self.rcOut[0] = 50 elif key == ord("u"): self.rcOut[2] = 50 elif key == ord("j"): self.rcOut[2] = -50 else: self.rcOut = [0, 0, 0, 0] else: if key == ord("w"): self.rcOut[1] = 50 elif key == ord("a"): self.rcOut[0] = -50 elif key == ord("s"): self.rcOut[1] = -50 elif key == ord("d"): self.rcOut[0] = 50 elif key == ord("u"): self.rcOut[2] = 50 elif key == ord("j"): self.rcOut[2] = -50 else: self.rcOut = [0, 0, 0, 0] # print self.rcOut self.tello.send_rc_control(int(self.rcOut[0]), int(self.rcOut[1]), int(self.rcOut[2]), int(self.rcOut[3])) self.rcOut = [0, 0, 0, 0] if key == ord("q"): break if key == ord("t"): self.tello.takeoff() if key == ord("l"): self.tello.land() Height = 100 if cv2.waitKey(1) & 0xFF == ord('q'): break time.sleep(1 / FPS) self.flag = 0 # Call it always before finishing. I deallocate resources. self.tello.end()
from djitellopy import Tello import cv2 import time tello = Tello() tello.connect() cmd = raw_input("What would you like to do?\n") while cmd != 'q': if cmd == "t": tello.takeoff() elif cmd == 'l': tello.move_left(50) elif cmd == 'r': tello.move_right(50) elif cmd == '45': tello.rotate_counter_clockwise(45) else: tello.land() cmd = raw_input("Next cmd\n") tello.land() tello.end()
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame # Init Tello object that interacts with the Tello drone self.tello = Tello() def run(self): if not self.tello.connect(): print("Tello not connected") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False name = "outImg" i = 0 while not should_stop: if frame_read.stopped: frame_read.stop() break frameBGR = np.copy(frame_read.frame) # frameBGR = np.rot90(frameBGR) # frameBGR = np.flipud(frameBGR) frame2use = im.resize(frameBGR, width=720) key = cv2.waitKey(1) & 0xFF cv2.imshow("lkgs", frame2use) if key == ord("c"): new = name + str(i) + ".jpg" print new i = i + 1 cv2.imwrite(new, frame2use) key = cv2.waitKey(1) & 0xFF if key == ord("q"): break if cv2.waitKey(1) & 0xFF == ord('q'): break frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) # frame = np.rot90(frame) # frame = np.flipud(frame) # a = frameGray.dtype # print a time.sleep(1 / FPS) # Call it always before finishing. I deallocate resources. self.tello.end()
class FrontEnd(object): """ Maintains the Tello display and moves it through the keyboard keys. Press escape key to quit. The controls are: - T: Takeoff - L: Land - Arrow keys: Forward, backward, left and right. - A and D: Counter clockwise and clockwise rotations - W and S: Up and down. """ def __init__(self): # Init pygame pygame.init() # Creat pygame window pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([960, 720]) # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 self.send_rc_control = False # create update timer pygame.time.set_timer(USEREVENT + 1, 50) def run(self): if not self.tello.connect(): print("Tello not connected") return if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") return # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") return if not self.tello.streamon(): print("Could not start video stream") return frame_read = self.tello.get_frame_read() should_stop = False while not should_stop: for event in pygame.event.get(): if event.type == USEREVENT + 1: self.update() elif event.type == QUIT: should_stop = True elif event.type == KEYDOWN: if event.key == K_ESCAPE: should_stop = True else: self.keydown(event.key) elif event.type == KEYUP: self.keyup(event.key) if frame_read.stopped: frame_read.stop() break self.screen.fill([0, 0, 0]) frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) frame = np.rot90(frame) frame = pygame.surfarray.make_surface(frame) self.screen.blit(frame, (0, 0)) pygame.display.update() time.sleep(1 / FPS) # Call it always before finishing. I deallocate resources. self.tello.end() def keydown(self, key): """ Update velocities based on key pressed Arguments: key: pygame key """ if key == pygame.K_UP: # set forward velocity self.for_back_velocity = S elif key == pygame.K_DOWN: # set backward velocity self.for_back_velocity = -S elif key == pygame.K_LEFT: # set left velocity self.left_right_velocity = -S elif key == pygame.K_RIGHT: # set right velocity self.left_right_velocity = S elif key == pygame.K_w: # set up velocity self.up_down_velocity = S elif key == pygame.K_s: # set down velocity self.up_down_velocity = -S elif key == pygame.K_a: # set yaw clockwise velocity self.yaw_velocity = -S elif key == pygame.K_d: # set yaw counter clockwise velocity self.yaw_velocity = S def keyup(self, key): """ Update velocities based on key released Arguments: key: pygame key """ if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity self.for_back_velocity = 0 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity self.left_right_velocity = 0 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity self.up_down_velocity = 0 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity self.yaw_velocity = 0 elif key == pygame.K_t: # takeoff self.tello.takeoff() self.send_rc_control = True elif key == pygame.K_l: # land self.tello.land() self.send_rc_control = False def update(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity)
class DroneControl(object): def __init__(self): # Init Tello object that interacts with the Tello drone self.tello = Tello() # Drone velocities between -100~100 self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.speed = 10 # Autonomous mode: Navigate autonomously self.autonomous = True # Enroll mode: Try to find new faces self.enroll_mode = False # Create arrays of known face encodings and their names self.known_face_encodings = [] self.known_face_names = [] # Logic used for navigation self.face_locations = None self.face_encodings = None self.target_name = "" self.has_face = False self.detect_faces = True self.wait = 0 self.load_all_faces() # Video frame for Streaming self.frame_available = None if not self.tello.connect(): print("Tello not connected") raise Exception("Tello not connected") if not self.tello.set_speed(self.speed): print("Not set speed to lowest possible") raise Exception("Not set speed to lowest possible") # In case streaming is on. This happens when we quit this program without the escape key. if not self.tello.streamoff(): print("Could not stop video stream") raise Exception("Could not stop video stream") if not self.tello.streamon(): print("Could not start video stream") raise Exception("Could not start video stream") self.loop() def loop(self): self.update_rc_control() if self.tello.get_frame_read().stopped: self.tello.get_frame_read().stop() self.shutdown() video_frame = self.tello.get_frame_read().frame # Resize the frame capture_divider = 0.5 recognition_frame = cv2.resize(video_frame, (0, 0), fx=capture_divider, fy=capture_divider) #BGR is used, not RGB # Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses) # recognition_frame = bgr_recognition_frame[:, :, ::-1] # Copy of the frame for capturing faces if self.enroll_mode: capture_frame = video_frame.copy() # Only detect faces every other frame if self.detect_faces: self.face_locations = face_recognition.face_locations(recognition_frame) self.face_encodings = face_recognition.face_encodings(recognition_frame, self.face_locations) self.detect_faces = False else: self.detect_faces = True # Navigate Autonomously if self.autonomous: # Loop through detected faces for (top, right, bottom, left), name in self.identify_faces(self.face_locations, self.face_encodings): # Scale back up face locations since the frame we detected in was scaled to 1/4 size top = int(top * 1/capture_divider) right = int(right * 1/capture_divider) bottom = int(bottom * 1/capture_divider) left = int(left * 1/capture_divider) x = left y = top w = right - left h = bottom - top # Draw a box around the face cv2.rectangle(video_frame, (left, top), (right, bottom), (0, 0, 255), 2) if name is not unknown_face_name: # Draw a label with a name below the face font = cv2.FONT_HERSHEY_DUPLEX cv2.rectangle(video_frame, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED) cv2.putText(video_frame, name, (left + 6, bottom - 6), font, 1.0, (255, 255, 255), 1) print(self.target_name + '-' + name) if self.enroll_mode: if name is unknown_face_name: target_reached = self.approach_target(video_frame, top,right, bottom, left) if target_reached: newUUID = uuid.uuid4() newFacePath = "new_faces/{}.png".format(newUUID) roi = capture_frame[y:y+h, x:x+w] cv2.imwrite(newFacePath, roi) if self.load_face(newFacePath, str(newUUID)): shutil.copy2(newFacePath, "known_faces/{}.png".format(newUUID)) break elif (self.target_name and name == self.target_name) or ((not self.target_name) and name != unknown_face_name): target_reached = self.approach_target(video_frame, top,right, bottom, left) break # No Faces / Face lost if len(self.face_encodings) == 0: # Wait for a bit if the stream has collapsed if self.wait >= detection_wait_interval: self.wait = 0 if self.has_face: # Go back and down to try to find the face again self.has_face = False self.up_down_velocity = -30 self.for_back_velocity = -20 else: # Turn to find any face self.yaw_velocity = 25 self.up_down_velocity = 0 self.for_back_velocity = 0 else: self.wait += 1 # Show video stream self.frame_available = video_frame #cv2.imshow("Tello Drone Delivery", video_frame) def shutdown(self): # On exit, print the battery print(self.get_battery()) # When everything done, release the capture cv2.destroyAllWindows() # Call it always before finishing. I deallocate resources. self.tello.end() def update_rc_control(self): """ Update routine. Send velocities to Tello.""" self.tello.send_rc_control( self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity) def get_battery(self): """ Get Tello battery state """ battery = self.tello.get_battery() if battery: return battery[:2] else: return False def load_face(self, file, name): """ Load and enroll a face from the File System """ face_img = face_recognition.load_image_file(file) face_encodings = face_recognition.face_encodings(face_img) if len(face_encodings) > 0: self.known_face_encodings.append(face_encodings[0]) self.known_face_names.append(name) return True return False def load_all_faces(self): """ Load and enroll all faces from the known_faces folder, then clear out the new_faces folder """ for face in os.listdir("known_faces/"): print(face[:-4]) self.load_face("known_faces/" + face, face[:-4]) for file in os.listdir("new_faces/"): os.remove("new_faces/" + file) def identify_faces(self, face_locations, face_encodings): """ Identify known faces from face encodings """ face_names = [] for face_encoding in face_encodings: # See if the face is a match for the known face(s) matches = face_recognition.compare_faces(self.known_face_encodings, face_encoding) name = unknown_face_name # Use the known face with the smallest distance to the new face face_distances = face_recognition.face_distance(self.known_face_encodings, face_encoding) best_match_index = np.argmin(face_distances) if matches[best_match_index]: name = self.known_face_names[best_match_index] face_names.append(name) return zip(face_locations, face_names) def approach_target(self, video_frame, top, right, bottom, left): """ The main navigation algorithm """ x = left y = top w = right - left h = bottom - top self.has_face = True # The Center point of our Target target_point_x = int(left + (w/2)) target_point_y = int(top + (h/2)) # Draw the target point cv2.circle(video_frame, (target_point_x, target_point_y), 10, (0, 255, 0), 2) # The Center Point of the drone's view heading_point_x = int(dimensions[0] / 2) heading_point_y = int(dimensions[1] / 2) # Draw the heading point cv2.circle( video_frame, (heading_point_x, heading_point_y), 5, (0, 0, 255), 2) target_reached = heading_point_x >= (target_point_x - tolerance_x) \ and heading_point_x <= (target_point_x + tolerance_x) \ and heading_point_y >= (target_point_y - tolerance_y) \ and heading_point_y <= (target_point_y + tolerance_y) # Draw the target zone cv2.rectangle( video_frame, (target_point_x - tolerance_x, target_point_y - tolerance_y), (target_point_x + tolerance_x, target_point_y + tolerance_y), (0, 255, 0) if target_reached else (0, 255, 255), 2) close_enough = (right-left) > depth_box_size * 2 - depth_tolerance \ and (right-left) < depth_box_size * 2 + depth_tolerance \ and (bottom-top) > depth_box_size * 2 - depth_tolerance \ and (bottom-top) < depth_box_size * 2 + depth_tolerance # Draw the target zone cv2.rectangle( video_frame, (target_point_x - depth_box_size, target_point_y - depth_box_size), (target_point_x + depth_box_size, target_point_y + depth_box_size), (0, 255, 0) if close_enough else (255, 0, 0), 2) if not target_reached: target_offset_x = target_point_x - heading_point_x target_offset_y = target_point_y - heading_point_y self.yaw_velocity = round(v_yaw_pitch * map_values(target_offset_x, -dimensions[0], dimensions[0], -1, 1)) self.up_down_velocity = -round(v_yaw_pitch * map_values(target_offset_y, -dimensions[1], dimensions[1], -1, 1)) print("YAW SPEED {} UD SPEED {}".format(self.yaw_velocity, self.up_down_velocity)) if not close_enough: depth_offset = (right - left) - depth_box_size * 2 if (right - left) > depth_box_size * 1.5 and not target_reached: self.for_back_velocity = 0 else: self.for_back_velocity = -round(v_for_back * map_values(depth_offset, -depth_box_size, depth_box_size, -1, 1)) else: self.for_back_velocity = 0 return target_reached and close_enough def take_off(self): self.tello.takeoff() def land(self): self.tello.land() def set_autonomous(self, autonomous): self.autonomous = autonomous def set_enroll_mode(self, enroll_mode): self.enroll_mode = enroll_mode def set_target_name(self, target_name): self.target_name = target_name def set_for_back_velocity(self, for_back_velocity): self.for_back_velocity = for_back_velocity def set_left_right_velocity(self, left_right_velocity): self.left_right_velocity = left_right_velocity def set_up_down_velocity(self, up_down_velocity): self.up_down_velocity = up_down_velocity def set_yaw_velocity(self, yaw_velocity): self.yaw_velocity = yaw_velocity def get_video_frame(self): video_frame = self.frame_available self.frame_available = drone return video_frame