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(pygame.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 == 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: 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 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 elif key == pygame.K_b: # get battery print(self.tello.get_battery()) elif key == pygame.K_c: # send command from terminal command = input( 'Enter command (speed?time?battery?tof?height?baro?temp?wifi?sn?sdk?altitude?)' ) print(self.tello.send_read_command(command)) elif key == pygame.K_1: print('flip right') self.tello.flip('r') elif key == pygame.K_2: print('flip left') self.tello.flip('l') elif key == pygame.K_3: print('flip forward') self.tello.flip('f') elif key == pygame.K_4: print('flip back') self.tello.flip('b') 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)
continue if global_coords[0] == -1: global_coords[0] = 0 global_coords[1] = 0 global_coords[2] = 2 cv2.circle(frame, (global_coords[0], global_coords[1]), global_coords[2], (0, 255, 0), 2) cv2.imshow('frame', frame) if cv2.waitKey(1) & 0xFF == 27: break drone.land() drone_cam.stop() detect_thread.join() #vid_process.join() # waits for video process to be killed #detect_process.terminate() # then kills detection process #vid_thread.join() #detect_thread.join() ''' final_drone_cam_memory = shared_memory.SharedMemory(name="drone_cam_object") final_drone_cam_array = np.ndarray((1,), dtype=np.object, buffer=final_drone_cam_memory.buf) # load camera in final_drone_cam_array[0].stop() # stop camera final_drone_cam_memory.close() # close camera storage final_drone_cam_memory.unlink() # delete camera storage
class DroneProcessor: FINISH_DRAWING_HOLD_TIME_S = 2 def __init__(self, max_area_cm=100, starting_move_up_cm=50, min_length_between_points_cm=5, max_speed=30): """ :param max_area_cm: Maximum length that drone can move from starting point in both axes. :param starting_move_up_cm: How many cms should drone go up after the takeoff :param min_length_between_points_cm: Minimum length between points, to reduce number of points from detection. """ self.max_area = max_area_cm self.min_length_between_points_cm = min_length_between_points_cm self.max_speed = max_speed self.tello = Tello() self.tello.connect() self.tello.streamon() self.tello.takeoff() self.tello.move_up(starting_move_up_cm) self.tello_ping_thread = Thread(target=self.ping_tello) self.should_stop_pinging_tello = False def get_last_frame(self): return self.tello.get_frame_read().frame def finish_drawing(self): """ Finish drawing, by stopping drone in air for a while and then force it to land. Disable video streaming. :return: """ self.tello.send_rc_control(0, 0, 0, 0) time.sleep(self.FINISH_DRAWING_HOLD_TIME_S) self.tello.land() self.tello.streamoff() def ping_tello(self): """ Ping tello to prevent it from landing while drawing. :return: """ while True: time.sleep(1) self.tello.send_command_with_return("command") print(f"Battery level: {self.tello.get_battery()}") if self.should_stop_pinging_tello: break def start_pinging_tello(self): """ Starts thread that pings Tello drone, to prevent it from landing while drawing :return: """ self.tello_ping_thread.start() def stop_pinging_tello(self): """ Stop pinging tello to make it available to control :return: """ self.should_stop_pinging_tello = True self.tello_ping_thread.join() def rescale_points(self, point_list, is_int=False): """ Rescale points from 0-1 range to range defined by max_area. :param point_list: :param is_int: :return: Points rescaled to max_area """ temp_list = [] for point in point_list: temp_point = [] for coordinate in point: coordinate = coordinate * self.max_area if is_int: temp_point.append(int(coordinate)) else: temp_point.append(coordinate) temp_list.append(temp_point) return temp_list def discrete_path(self, rescaled_points): """ Reduce number of points in list, so the difference between next points needs to be at least min_length_between_points_cm :param rescaled_points: :return: """ last_index = -1 length = 0 while length < self.min_length_between_points_cm: last_index -= 1 length = distance(rescaled_points[-1], rescaled_points[last_index]) last_index = len(rescaled_points) + last_index discrete_path = [rescaled_points[0]] actual_point = 0 for ind, point in enumerate(rescaled_points): if ind > last_index: discrete_path.append(rescaled_points[-1]) break if distance(rescaled_points[actual_point], point) > 5: discrete_path.append(point) actual_point = ind return discrete_path def reproduce_discrete_path_by_drone(self, discrete_path): """ Converts discrete path to velocity commands and sends them to drone, so the drone reproduce the path :param discrete_path: list of [x, y] points, which represents distance in each axis between previous point in list :return: """ for current_point in discrete_path: ang = np.arctan2(current_point[0], current_point[1]) x_speed = int(np.sin(ang) * self.max_speed) y_speed = -int(np.cos(ang) * self.max_speed) euclidean_distance = (current_point[0] ** 2 + current_point[1] ** 2) ** 0.5 move_time = euclidean_distance / self.max_speed self.tello.send_rc_control(x_speed, 0, y_speed, 0) time.sleep(move_time)
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 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. """ 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 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. """ 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() # 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. - 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() # 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 = 60 self.send_rc_control = False # create update timer pygame.time.set_timer(USEREVENT + 1, 2) pygame.time.set_timer(USEREVENT + 2, 10) # Run thread to find box in frame print('init done') def run(self): global frame_glob fourcc = cv2.VideoWriter_fourcc(*'XVID') out = cv2.VideoWriter('output.avi', fourcc, 60.0, (960, 720)) 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 print('loop started') last_yaw=0 box_cnt = 0 frame_glob = [] frame = [] while not should_stop: frame = frame_read.frame for event in pygame.event.get(): if event.type == USEREVENT + 1: self.update() if event.type == USEREVENT + 2: frame_glob = frame 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]) box_list = [] try: box_list, actions_list = box_q.get_nowait() box = box_list[0] actions = actions_list[0] except Exception: pass box_x = 0 box_y = 0 if len(box_list) > 0: box_cnt = 0 area = (box.xmax - box.xmin) * (box.ymax - box.ymin) area_p = (area / 691200.) * 100.0 box_x = int((box.xmax - box.xmin) / 2) + box.xmin box_y = int((box.ymax - box.ymin) / 2) + box.ymin draw_boxes(frame, box_list, config['model']['labels'], obj_thresh) done = bool(get_dist(box_x, box_y) < 100 and (15 < area_p < 50)) #If not done keep setting speeds if not done: self.yaw_velocity = -int((actions[0])/2) last_yaw = self.yaw_velocity self.up_down_velocity = int((actions[1])/2) if area_p < 25: self.for_back_velocity = 30 elif area_p > 50: self.for_back_velocity = -30 else: self.for_back_velocity = 0 frame = cv2.circle(frame, (box_x, box_y), 5, (255, 0, 0), -1) else: self.yaw_velocity = 0 self.up_down_velocity = 0 self.for_back_velocity = 0 #if no box is detected set the last yaw command so the drone goes into search. else: box_cnt += 1 if box_cnt > 10: self.yaw_velocity = last_yaw else: self.yaw_velocity = 0 self.up_down_velocity = 0 self.for_back_velocity = 0 frame = cv2.circle(frame, (480, 360), 5,(0, 0, 255),-1) out.write(frame) frame = np.rot90(frame) frame = np.flipud(frame) frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) 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() out.release() 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) def reset_speed(self): """ Update routine. Send velocities to Tello.""" if self.send_rc_control: self.tello.send_rc_control(0, 0, 0, 0)
class TelloDrone(): def __init__(self): """ Get the DJITelloPy Tello object to use for communication with the drone. speed: general speed for non rc control calls (10-100) """ self.tello = Tello() self.left_right_velocity = 0 self.forward_backward_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.has_target = False self.battery_level = 0 self.last_battery_check = 0 self.speed = 20 # Lowest def cleanup(self): """Call this function if the script has to be stopped for whatever reason to make sure everything is exited cleanly""" self.end_drone() cv2.destroyAllWindows() def end_drone(self): """Lands drone and ends drone interaction""" self.stop_moving() self.tello.streamoff() if self.tello.is_flying: self.tello.land() self.tello.end() def update_battery_level(self): if time.time() - self.last_battery_check >= TIME_BTW_BATTERY_CHECKS: self.battery_level = self.tello.get_battery() self.last_battery_check = time.time() def update_drone_velocities_if_flying(self): """Move drone based on velocities only if drone is flying""" if not self.tello.is_flying: return self.tello.send_rc_control(self.left_right_velocity, self.forward_backward_velocity, self.up_down_velocity, self.yaw_velocity) def update_drone_velocities(self): """Move drone based on velocities""" self.tello.send_rc_control(self.left_right_velocity, self.forward_backward_velocity, self.up_down_velocity, self.yaw_velocity) def stop_moving(self): """Freeze in place""" time.sleep(self.tello.TIME_BTW_RC_CONTROL_COMMANDS) # Delay makes sure it is sent self.left_right_velocity = 0 self.forward_backward_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = 0 self.update_drone_velocities() def scan_surroundings(self): """Start looking around""" self.yaw_velocity = SCAN_VELOCITY self.update_drone_velocities_if_flying() def find_target(self): # Returns False for now return False def handle_user_keypress(self): """Check for pressed keys. 't': Take off 'l': Land 'Esc': Exit Returns: key (int): unicode value of key that was pressed, or -1 if none were pressed""" key = cv2.waitKey(20) if key == ord('t'): print_warning("Drone taking off...") self.tello.takeoff() elif key == ord('l'): print_warning("Landing drone...") self.stop_moving() self.tello.land() elif key == ESC_KEY: print_warning("Stopping drone...") elif key == NO_KEY: if not self.has_target: self.scan_surroundings() return key def initialize_before_run(self): """Set up drone before we bring it to life""" if not self.tello.connect(): print_error("Failed to set drone to SDK mode") sys.exit(-1) print_status("Connected to Drone") if not self.tello.set_speed(self.speed): print_error("Failed to set initial drone speed") print_status(f"Drone speed set to {self.speed} cm/s ({self.speed}%)") # Make sure stream is off first self.tello.streamoff() self.tello.streamon() # Not sure why DJITelloPy doesn't return here self.battery_level = self.tello.get_battery() print_status(f"Drone battery is at {self.battery_level}%") # Reset velocities self.update_drone_velocities() print_status(f"All drone velocities initialized to 0") def run(self, user_interface=True): """Bring an autonomous being to life""" self.initialize_before_run() frame_read = self.tello.get_frame_read() while True: self.update_drone_velocities_if_flying() if frame_read.stopped: frame_read.stop() break frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) frame_ret = frame_read.frame tello_cam = self.tello.get_video_capture() time.sleep(1 / FPS) key_pressed = self.handle_user_keypress() if key_pressed == ESC_KEY: break # Show battery level self.update_battery_level() cv2.putText(frame_ret,f"Battery: {self.battery_level}%",(32,664),cv2.FONT_HERSHEY_SIMPLEX,1,(0, 0, 255), thickness=2) cv2.imshow(f'Tello Tracking...', frame_ret) self.cleanup() # Clean exit
from djitellopy import Tello myDrone = Tello() myDrone.connect() # Read datas from JSON file. f = open('waypoints.json', 'r') path_dict = json.load(f) path_wp = path_dict['wp'] path_pos = path_dict['pos'] print(path_wp) print(path_pos) # Follow the instructions myDrone.takeoff() for instruction in path_wp: angle = instruction['angle_deg'] length = instruction['dist_cm'] if (angle < 0): myDrone.rotate_counter_clockwise(-angle) else: myDrone.rotate_clockwise(angle) myDrone.move_forward(length) myDrone.land() # d.rotate_clockwise(90) # d.move_forward(20) # d.land()
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): """ 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
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): """ 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()
class TelloCV(object): """ TelloTracker builds keyboard controls on top of TelloPy as well as generating images from the video stream and enabling opencv support """ def __init__(self): self.prev_flight_data = None self.record = False self.tracking = False self.keydown = False self.date_fmt = '%Y-%m-%d_%H%M%S' self.speed = 50 self.go_speed = 80 self.drone = Tello() self.init_drone() self.init_controls() self.battery = self.drone.get_battery() self.frame_read = self.drone.get_frame_read() self.forward_time = 0 self.forward_flag = True self.takeoff_time = 0 self.command_time = 0 self.command_flag = False # trackingimport libh264decoder a color # green_lower = (30, 50, 50) # green_upper = (80, 255, 255) # red_lower = (0, 50, 50) # red_upper = (20, 255, 255) blue_lower = np.array([0, 0, 0]) upper_blue = np.array([255, 255, 180]) bh_lower = (180, 30, 100) bh_upper = (275, 50, 100) self.track_cmd = "" self.tracker = Tracker(960, 720, blue_lower, upper_blue) self.speed_list = [5, 10, 15, 20, 25] self.frame_center = 480, 360 self.error = 60 def init_drone(self): """Connect, uneable streaming and subscribe to events""" # self.drone.log.set_level(2) self.drone.connect() self.drone.streamon() def on_press(self, keyname): """handler for keyboard listener""" if self.keydown: return try: self.keydown = True keyname = str(keyname).strip('\'') print('+' + keyname) if keyname == 'Key.esc': self.drone.quit() exit(0) if keyname in self.controls: key_handler = self.controls[keyname] if isinstance(key_handler, str): getattr(self.drone, key_handler)(self.speed) else: key_handler(self.speed) except AttributeError: print('special key {0} pressed'.format(keyname)) def on_release(self, keyname): """Reset on key up from keyboard listener""" self.keydown = False keyname = str(keyname).strip('\'') print('-' + keyname) if keyname in self.controls: key_handler = self.controls[keyname] if isinstance(key_handler, str): getattr(self.drone, key_handler)(0) else: key_handler(0) def init_controls(self): """Define keys and add listener""" self.controls = { 'w': lambda speed: self.drone.move_forward(speed), 's': lambda speed: self.drone.move_back(speed), 'a': lambda speed: self.drone.move_left(speed), 'd': lambda speed: self.drone.move_right(speed), 'Key.space': 'up', 'Key.shift': 'down', 'Key.shift_r': 'down', 'q': 'counter_clockwise', 'e': 'clockwise', 'i': lambda speed: self.drone.flip_forward(), 'k': lambda speed: self.drone.flip_back(), 'j': lambda speed: self.drone.flip_left(), 'l': lambda speed: self.drone.flip_right(), # arrow keys for fast turns and altitude adjustments 'Key.left': lambda speed: self.drone.rotate_counter_clockwise(speed), 'Key.right': lambda speed: self.drone.rotate_clockwise(speed), 'Key.up': lambda speed: self.drone.move_up(speed), 'Key.down': lambda speed: self.drone.move_down(speed), 'Key.tab': lambda speed: self.drone.takeoff(), # 'Key.tab': self.drone.takeoff(60), 'Key.backspace': lambda speed: self.drone.land(), 'p': lambda speed: self.palm_land(speed), 't': lambda speed: self.toggle_tracking(speed), 'r': lambda speed: self.toggle_recording(speed), 'z': lambda speed: self.toggle_zoom(speed), 'Key.enter': lambda speed: self.take_picture(speed) } self.key_listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release) self.key_listener.start() # self.key_listener.join() def process_frame(self): """convert frame to cv2 image and show""" # print("TRACKING START") frame = self.frame_read.frame # self.drone.move_up(self.speed) # image = self.write_hud(image) # if self.record: # self.record_vid(frame) return frame def move_up(self): self.drone.move_up(self.speed) def take_off(self): self.drone.takeoff() def go(self): self.drone.move_forward(self.go_speed) def move_left(self): self.drone.move_left(270) # speed 테스트해서 조절하기 def go_window9(self): self.drone.move_forward() def rotate_right(self): self.drone.rotate_clockwise() def rotate_left(self): self.drone.rotate_counter_clockwise() def landing(self): self.drone.land() def write_hud(self, frame): """Draw drone info, tracking and record on frame""" stats = self.prev_flight_data.split('|') stats.append("Tracking:" + str(self.tracking)) if self.drone.zoom: stats.append("VID") else: stats.append("PIC") if self.record: diff = int(time.time() - self.start_time) mins, secs = divmod(diff, 60) stats.append("REC {:02d}:{:02d}".format(mins, secs)) for idx, stat in enumerate(stats): text = stat.lstrip() cv2.putText(frame, text, (0, 30 + (idx * 30)), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 0, 0), lineType=30) return frame def toggle_recording(self, speed): """Handle recording keypress, creates output stream and file""" if speed == 0: return self.record = not self.record if self.record: datename = [os.getenv('HOME'), datetime.datetime.now().strftime(self.date_fmt)] self.out_name = '{}/Pictures/tello-{}.mp4'.format(*datename) print("Outputting video to:", self.out_name) self.out_file = av.open(self.out_name, 'w') self.start_time = time.time() self.out_stream = self.out_file.add_stream( 'mpeg4', self.vid_stream.rate) self.out_stream.pix_fmt = 'yuv420p' self.out_stream.width = self.vid_stream.width self.out_stream.height = self.vid_stream.height if not self.record: print("Video saved to ", self.out_name) self.out_file.close() self.out_stream = None def record_vid(self, frame): """ convert frames to packets and write to file """ new_frame = av.VideoFrame( width=frame.width, height=frame.height, format=frame.format.name) for i in range(len(frame.planes)): new_frame.planes[i].update(frame.planes[i]) pkt = None try: pkt = self.out_stream.encode(new_frame) except IOError as err: print("encoding failed: {0}".format(err)) if pkt is not None: try: self.out_file.mux(pkt) except IOError: print('mux failed: ' + str(pkt)) def take_picture(self, speed): """Tell drone to take picture, image sent to file handler""" if speed == 0: return self.drone.take_picture() def palm_land(self, speed): """Tell drone to land""" if speed == 0: return self.drone.palm_land() def toggle_tracking(self, speed): """ Handle tracking keypress""" if speed == 0: # handle key up event return self.tracking = not self.tracking print("tracking:", self.tracking) return def toggle_zoom(self, speed): """ In "video" mode the self.drone sends 1280x720 frames. In "photo" mode it sends 2592x1936 (952x720) frames. The video will always be centered in the window. In photo mode, if we keep the window at 1280x720 that gives us ~160px on each side for status information, which is ample. Video mode is harder because then we need to abandon the 16:9 display size if we want to put the HUD next to the video. """ if speed == 0: return self.drone.set_video_mode(not self.drone.zoom) def flight_data_handler(self, event, sender, data): """Listener to flight data from the drone.""" text = str(data) if self.prev_flight_data != text: self.prev_flight_data = text def handle_flight_received(self, event, sender, data): """Create a file in ~/Pictures/ to receive image from the drone""" path = '%s/Pictures/tello-%s.jpeg' % ( os.getenv('HOME'), datetime.datetime.now().strftime(self.date_fmt)) with open(path, 'wb') as out_file: out_file.write(data) print('Saved photo to %s' % path) def enable_mission_pads(self): self.drone.enable_mission_pads() def disable_mission_pads(self): self.drone.disable_mission_pads() def go_xyz_speed_mid(self, x, y, z, speed, mid): self.drone.go_xyz_speed_mid(x, y, z, speed, mid) # if function return True, set drone center to object's center def track_mid(self, x, y): midx, midy = 480, 360 distance_x = abs(midx - x) distance_y = abs(midy - y) print(x, y, distance_x, distance_y) move_done = True if y > midy + self.error + 15: self.drone.move_down(20) move_done = False elif y < midy - self.error + 5: self.drone.move_up(20) move_done = False elif x < midx - self.error: self.drone.move_left(20) move_done = False elif x > midx + self.error: self.drone.move_right(20) move_done = False return move_done def track_x(self, x, left_count, right_count): midx = 480 move_done = True if x < midx - 100: self.drone.move_left(20) left_count += 1 move_done = False elif x > midx + 100: self.drone.move_right(20) right_count += 1 move_done = False return move_done def go_slow(self): self.drone.move_forward(30) def go_fast(self): self.drone.move_forward(200)
class hand_tello_control: def __init__(self): self.action = " " self.mp_drawing = mp.solutions.drawing_utils self.mp_drawing_styles = mp.solutions.drawing_styles self.mp_hands = mp.solutions.hands self.action_done = True self.ffoward = 1 self.fback = 1 self.fright = 1 self.fleft = 1 self.fsquare = 1 self.fmiddle = 1 self.fno = 1 self.fland = 1 def tello_startup(self): # For Tello input: self.tello = Tello() # Starts the tello object self.tello.connect() # Connects to the drone def define_orientation(self, results): if results.multi_hand_landmarks[0].landmark[ 4].x < results.multi_hand_landmarks[0].landmark[17].x: orientation = "right hand" else: orientation = "left hand" return orientation def follow_hand(self, results): normalizedLandmark = results.multi_hand_landmarks[0].landmark[ 9] # Normalizes the lowest middle-finger coordinate for hand tracking pixelCoordinatesLandmark = self.mp_drawing._normalized_to_pixel_coordinates( normalizedLandmark.x, normalizedLandmark.y, 255, 255 ) #Tracks the coordinates of the same landmark in a 255x255 grid print(pixelCoordinatesLandmark) if pixelCoordinatesLandmark == None: # If hand goes out of frame, stop following self.tello.send_rc_control(0, 0, 0, 0) return centerRange = [12, 12] #Range for detecting commands in the x and y axis. centerPoint = [128, 128] #Theoretical center of the image xCorrection = pixelCoordinatesLandmark[0] - centerPoint[0] yCorrection = pixelCoordinatesLandmark[1] - centerPoint[1] xSpeed = 0 ySpeed = 0 if xCorrection > centerRange[0] or xCorrection < -centerRange[ 0]: #If the hand is outside the acceptable range, changes the current speed to compensate. xSpeed = xCorrection // 3 if yCorrection > centerRange[1] or yCorrection < -centerRange[1]: ySpeed = -yCorrection // 3 self.tello.send_rc_control(xSpeed, 0, ySpeed, 0) time.sleep(0.5) self.tello.send_rc_control(0, 0, 0, 0) def action_to_do( self, fingers, orientation, results): #use the variable results for the hand tracking control if self.action_done == True: self.ffoward = 1 self.fback = 1 self.fright = 1 self.fleft = 1 self.fsquare = 1 self.fmiddle = 1 self.fno = 1 self.fland = 1 self.action_done = False #Left hand controls tricks, right hand controls movement if orientation == "left hand": #Thumb on the left = left hand! if fingers == [0, 1, 0, 0, 0]: if self.ffoward >= 15: self.action = "flip forward" self.tello.flip_forward() self.action_done = True self.ffoward = self.ffoward + 1 elif fingers == [0, 1, 1, 0, 0] and self.battery == True: if self.fback >= 15: self.action = "flip back" self.tello.flip_back() self.action_done = True self.fback = self.fback + 1 elif fingers == [1, 0, 0, 0, 0] and self.battery == True: if self.fright >= 15: self.action = "flip right" self.tello.flip_right() self.action_done = True self.fright = self.fright + 1 elif fingers == [0, 0, 0, 0, 1] and self.battery == True: if self.fleft >= 15: self.action = "flip left" self.tello.flip_left() self.action_done = True self.fleft = self.fleft + 1 elif fingers == [0, 1, 1, 1, 0]: if self.fsquare >= 15: self.action = "Square" self.tello.move_left(20) self.tello.move_up(40) self.tello.move_right(40) self.tello.move_down(40) self.tello.move_left(20) self.action_done = True self.fsquare = self.fsquare + 1 elif fingers == [0, 0, 1, 0, 0]: if self.fmiddle >= 15: self.action = " :( " self.tello.land() self.action_done = True self.fmiddle = self.fmiddle + 1 elif ((self.battery == False) and (fingers == [1, 0, 0, 0, 0] or fingers == [0, 1, 0, 0, 0] or fingers == [0, 0, 0, 0, 1])): #not avaiable to do tricks if self.fno >= 15: self.tello.rotate_clockwise(45) self.tello.rotate_counter_clockwise(90) self.tello.rotate_clockwise(45) self.action_done = True self.fno = self.fno + 1 else: self.action = " " elif orientation == "right hand": #Thumb on the right = right hand! if fingers == [1, 1, 1, 1, 1]: self.action = "Follow" self.follow_hand(results) elif fingers == [1, 0, 0, 0, 0]: if self.fland >= 15: self.action = "Land" self.tello.land() self.action_done = True self.fland = self.fland + 1 else: self.action = " " def fingers_position(self, results, orientation): # [thumb, index, middle finger, ring finger, pinky] # 0 for closed, 1 for open fingers = [0, 0, 0, 0, 0] if (results.multi_hand_landmarks[0].landmark[4].x > results.multi_hand_landmarks[0].landmark[3].x ) and orientation == "right hand": fingers[0] = 0 if (results.multi_hand_landmarks[0].landmark[4].x < results.multi_hand_landmarks[0].landmark[3].x ) and orientation == "right hand": fingers[0] = 1 if (results.multi_hand_landmarks[0].landmark[4].x > results.multi_hand_landmarks[0].landmark[3].x ) and orientation == "left hand": fingers[0] = 1 if (results.multi_hand_landmarks[0].landmark[4].x < results.multi_hand_landmarks[0].landmark[3].x ) and orientation == "left hand": fingers[0] = 0 fingermarkList = [8, 12, 16, 20] i = 1 for k in fingermarkList: if results.multi_hand_landmarks[0].landmark[ k].y > results.multi_hand_landmarks[0].landmark[k - 2].y: fingers[i] = 0 else: fingers[i] = 1 i = i + 1 return fingers def detection_loop(self): with self.mp_hands.Hands(model_complexity=0, min_detection_confidence=0.75, min_tracking_confidence=0.5) as hands: self.tello.streamoff( ) # Ends the current stream, in case it's still opened self.tello.streamon() # Starts a new stream while True: frame_read = self.tello.get_frame_read( ) # Stores the current streamed frame image = frame_read.frame self.battery = self.tello.get_battery() # To improve performance, optionally mark the image as not writeable to # pass by reference. image.flags.writeable = True image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) results = hands.process(image) # Draw the hand annotations on the image. image.flags.writeable = True image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) if results.multi_hand_landmarks: action = " " for hand_landmarks in results.multi_hand_landmarks: self.mp_drawing.draw_landmarks( image, hand_landmarks, self.mp_hands.HAND_CONNECTIONS, self.mp_drawing_styles. get_default_hand_landmarks_style(), self.mp_drawing_styles. get_default_hand_connections_style()) orientation = self.define_orientation(results) fingers = self.fingers_position(results, orientation) #print(fingers) self.action_to_do(fingers, orientation, results) for event in pg.event.get(): if event.type == pg.KEYDOWN: if event.key == pg.K_l: self.tello.land() if event.key == pg.K_t: self.tello.takeoff() if event.key == pg.K_b: print("A bateria esta em ", self.tello.get_battery(), "%") if event.key == pg.K_m: return 0 cv2.imshow("image", image) if cv2.waitKey(5) & 0xFF == 27: break def key_control(self): self.tello.streamoff( ) # Ends the current stream, in case it's still opened self.tello.streamon() # Starts a new stream while True: frame_read = self.tello.get_frame_read( ) # Stores the current streamed frame image = frame_read.frame # To improve performance, optionally mark the image as not writeable to # pass by reference. image.flags.writeable = True image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) # Draw the hand annotations on the image. image.flags.writeable = True image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) for event in pg.event.get(): if event.type == pg.KEYDOWN: if event.key == pg.K_w: self.tello.move_forward(20) if event.key == pg.K_a: self.tello.move_left(20) if event.key == pg.K_s: self.tello.move_back(20) if event.key == pg.K_d: self.tello.move_right(20) if event.key == pg.K_q: self.tello.rotate_counter_clockwise(20) if event.key == pg.K_e: self.tello.rotate_clockwise(20) if event.key == pg.K_SPACE: self.tello.move_up(20) if event.key == pg.K_LCTRL: self.tello.move_down(20) if event.key == pg.K_b: print("A bateria esta em ", self.tello.get_battery(), "%") if event.key == pg.K_m: return 0 cv2.imshow("image", image) if cv2.waitKey(5) & 0xFF == 27: break def main_interface(self): telloMode = -1 self.tello_startup() pg.init() win = pg.display.set_mode((500, 500)) pg.display.set_caption("Test") #self.tello.takeoff() print("Para controlar pelo teclado, digite 1") print("Para controlar com a mao, digite 2") print("Para sair, digite 0") self.tello.streamoff( ) # Ends the current stream, in case it's still opened self.tello.streamon() # Starts a new stream while telloMode != 0: frame_read = self.tello.get_frame_read( ) # Stores the current streamed frame image = frame_read.frame # To improve performance, optionally mark the image as not writeable to # pass by reference. image.flags.writeable = True image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) # Draw the hand annotations on the image. image.flags.writeable = True image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) cv2.imshow("image", image) if cv2.waitKey(5) & 0xFF == 27: break if telloMode == 1: self.key_control() telloMode = -1 print("Para controlar pelo teclado, digite 1") print("Para controlar com a mao, digite 2") print("Para sair, digite 0") elif telloMode == 2: self.detection_loop() telloMode = -1 print("Para controlar pelo teclado, digite 1") print("Para controlar com a mao, digite 2") print("Para sair, digite 0") elif telloMode == 0: self.tello.land() telloMode = -1 print("Obrigado por voar hoje") elif telloMode != -1 and telloMode != 1 and telloMode != 2: print("valor invalido!") for event in pg.event.get(): if event.type == pg.KEYDOWN: if event.key == pg.K_1: telloMode = 1 if event.key == pg.K_2: telloMode = 2 if event.key == pg.K_0: telloMode = 0 if event.key == pg.K_l: self.tello.land() if event.key == pg.K_t: self.tello.takeoff() if event.key == pg.K_b: print("A bateria esta em ", self.tello.get_battery(), "%")
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 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
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 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()
me.streamon() while True: # GET THE IMAGE FROM TELLO frame_read = me.get_frame_read() myFrame = frame_read.frame img = cv2.resize(myFrame, (width, height)) # TO GO UP IN THE BEGINNING if startCounter == 0: me.takeoff() time.sleep(8) me.rotate_clockwise(90) time.sleep(3) me.move_left(25) time.sleep(3) me.land() startCounter = 1 # # SEND VELOCITY VALUES TO TELLO # if me.send_rc_control: # me.send_rc_control(me.left_right_velocity, me.for_back_velocity, me.up_down_velocity, me.yaw_velocity) # DISPLAY IMAGE cv2.imshow("MyResult", img) # WAIT FOR THE 'Q' BUTTON TO STOP if cv2.waitKey(1) & 0xFF == ord('g'): me.land() break
class DroneObject: #initialize the object #Set default state to IdleState() #Create Tello object from the API #set default values for coordinate, FPS, distance, and tilt def __init__(self): self.state = IdleState() self.tello = Tello() self.coordinate = (0, 0) self.FPS = 30 self.distance = 30 #pre defined, to be changed later self.tilt = 0 #Generates pass events to tellodrone class def on_event(self, event): self.state = self.state.on_event(event) #setter for member variables def set_parameter (self, x,y, dist, tilt): self.coordinate = (x,y) self.distance = dist self.tilt = tilt #for take off the drone, called when the drone is in takeoff state #when take off is complete, trigger event for track def take_off(self): self.tello.takeoff() for i in range (0,3): print("taking off %d /3" % (i+1)) time.sleep(1) self.on_event("track") #for landing the drone, called when the drone is in landingstate #when landing is complete, trigger event for idle def land(self): self.tello.land() for i in range (0,3): print("landing %d / 3" % (i+1)) time.sleep(1) self.on_event("idle") #for tracking the drone, called when the drone is in track state #when tilt is active, it will prioritize the turning motion over the other motions #controls shifting, moving up and down, forward backwards, and turning def track(self): if(self.tilt <= 0.95 and self.tilt != 0): self.tello.rotate_clockwise(int((1-self.tilt)*100)) time.sleep(0.05) elif(self.tilt >= 1.05): self.tello.rotate_counter_clockwise(int((self.tilt-1)*100)) time.sleep(0.05) else: if (self.distance > 60): forward = int((self.distance - 60)) if ((forward < 20)): self.tello.move_forward(20) else: self.tello.move_forward(forward) time.sleep(0.05) elif (self.distance < 50): backward = int(abs(self.distance - 50)) if ((backward < 20)): self.tello.move_back(20) else: self.tello.move_back(backward) time.sleep(0.05) if (self.coordinate[0] < 400 and self.coordinate[0] >= 0): self.tello.move_left(20) time.sleep(0.05) elif (self.coordinate[0] < 959 and self.coordinate[0] >= 559): self.tello.move_right(20) time.sleep(0.05) if (self.coordinate[1] > 0 and self.coordinate[1] <= 200): self.tello.move_up(20) time.sleep(0.05) elif (self.coordinate[1] >= 519 and self.coordinate[1] < 719): self.tello.move_down(20) time.sleep(0.05) #For setting up the drone #Establish connection #Initialize streamming #Display battery def setup(self): if not self.tello.connect(): print("Tello not connected") sys.exit() # 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") sys.exit() if not self.tello.streamon(): print("Could not start video stream") sys.exit() print("Current battery is " + self.tello.get_battery()) self.tello.streamon() frame_read = self.tello.get_frame_read() #For calling corresponding functions based on their state def action(self): print("current state is" , self.state) print(str(self.state)) if (str(self.state) == "TakeOffState"): print("take off!") self.take_off() elif (str(self.state) == "LandState"): print("land") self.land() if (str(self.state)== "TrackState"): self.track() else: return #idle state or undefined state do nothing return
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): 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)
from djitellopy import Tello tello = Tello() tello.connect() tello.takeoff() tello.move_left(100) tello.rotate_clockwise(90) tello.move_forward(100) tello.land()
def demo(): # Setup for arucodes aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000) parameters = aruco.DetectorParameters_create() # Initialise tello = Tello() tello.connect() # Wait for video stream ok = tello.streamon() frame_read = tello.get_frame_read() while not frame_read.grabbed: frame_read = tello.get_frame_read() img = frame_read.frame cv2.imshow("video", img) k = cv2.waitKey(10) & 0xff # Take off print( f"Take off. Height: {tello.get_height()}, Battery: {tello.get_battery()}, Temp: {tello.get_temperature()}" ) # Start the main loop keep_going = True left_right_velocity = 0 forward_back = 0 up_down = 0 rotate = 0 while keep_going: frame_read = tello.get_frame_read() if frame_read.grabbed: # Get frame img = frame_read.frame height, width, channels = img.shape # Check for arucodes corners, ids, rejectedImgPoints = aruco.detectMarkers( img, aruco_dict, parameters=parameters) arucodes = AruCode.parse_ids_and_corners(ids, corners) delta = 10 up_down = 0 rotate = 0 forward_back = 0 if 0 in arucodes.keys(): cv2.rectangle(img, (0, 0), (width, height), (255, 0, 0), 100) # Color us BGR keep_going = False elif 1 in arucodes.keys(): cv2.rectangle(img, (0, 0), (width, height), (0, 0, 0), 100) # Color us BGR followme = arucodes[1] x_percentile = int(100 * followme.x / width) y_percentile = int(100 * followme.y / height) if y_percentile < 40: cv2.rectangle(img, (0, 0), (width, 0), (0, 255, 0), 100) # Color us BGR print("move up", time.time()) up_down = 30 elif y_percentile > 60: cv2.rectangle(img, (0, height), (width, height), (0, 255, 0), 100) # Color us BGR print("move down", time.time()) up_down = -30 if x_percentile < 40: cv2.rectangle(img, (0, 0), (0, height), (0, 255, 0), 100) # Color us BGR print("rotate ccw", time.time()) rotate = -40 elif x_percentile > 60: cv2.rectangle(img, (width, 0), (width, height), (0, 255, 0), 100) # Color us BGR print("rotate cw", time.time()) rotate = 40 if followme.diagonal < 150: cv2.circle(img, (int(width / 2), int(height / 2)), 50, (0, 255, 0), -1) print("forward", followme.w, time.time()) forward_back = 40 else: print("stop", time.time()) forward_back = 0 else: # No valid arucode found cv2.rectangle(img, (0, 0), (width, height), (0, 0, 255), 100) # Color us BGR # Preview the image on screen cv2.imshow("video", img) # Wait 30 milliseconds for a keyboard press k = cv2.waitKey(10) & 0xff # If ESC key pressed, close if k == 27: keep_going = False elif k == ord('0'): keep_going = False elif k == ord('1'): tello.takeoff() elif k == ord('+') or k == ord('='): up_down = 50 elif k == ord('-'): up_down = -50 elif k == ord('s'): forward_back = 0 left_right_velocity = 0 rotate = 0 up_down = 0 elif k == ord('a'): rotate = -90 elif k == ord('d'): rotate = 90 elif k == ord('w'): forward_back = 100 elif k == ord('x'): forward_back = -100 elif k == ord('q'): left_right_velocity = -100 elif k == ord('e'): left_right_velocity = 100 # Send to RC print( f"tello: forward: {forward_back}, up: {up_down}, yaw: {rotate}" ) tello.send_rc_control(left_right_velocity=left_right_velocity, forward_backward_velocity=forward_back, up_down_velocity=up_down, yaw_velocity=rotate) print( f"Landing. Height: {tello.get_height()}, Battery: {tello.get_battery()}, Temp: {tello.get_temperature()}" ) try: tello.land() tello.end() except: print("Unable to land without error") print( f"Power off. Battery: {tello.get_battery()}, Temp: {tello.get_temperature()}" ) cv2.destroyAllWindows()