def main(): # Init Tello object that interacts with the Tello drone tello = Tello() tello.set_speed(20) try: flybox(tello) except Exception as e: print(e) tello.emergency_land()
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])