print(cascade) cascade_dir = "cascades/" + ci.usr_choice(cas_lst) font = cv2.FONT_HERSHEY_SIMPLEX bottom_left_corner = (10, 710) cascade = cv2.CascadeClassifier(cascade_dir) # initialize Tello and video stream t = Tello() time.sleep(1) initialize() # loop controls once drone is connected while running: # detect face and find coordinates if exists frame_read = t.get_frame_read() frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) frameRet = frame_read.frame face_center_coords = facedetect(frameRet, cascade, MANUAL_MODE) # velocity printout if S == 30: text = "Speed: Low" elif S == 65: text = "Speed: Normal" elif S == 100: text = "Speed: Fast" k = cv2.waitKey(30) & 0xFF # controls as given in intro() function
class Interface: def __init__(self): self.tello = Tello() self.tello.sendCmd("streamon") #스트리밍 모드 pygame.init() pygame.display.set_caption("KNUE Drone Stream") self.screen = pygame.display.set_mode([960, 720]) pygame.time.set_timer(USEREVENT + 1, 50) self.font = pygame.font.Font("freesansbold.ttf", 14) def make_text(self, font, text, color, bgcolor, top, left, position=0): surf = font.render(text, False, color, bgcolor) rect = surf.get_rect() if position: rect.center = (left, top) else: rect.topleft = (left, top) self.screen.blit(surf, rect) return rect def run(self): ''' 프레임을 업데이트 하고 이벤트를 처리한다. ''' frame_read = self.tello.get_frame_read() time.sleep(1) exitEvent = False previousCounter = 0 while not exitEvent: #이벤트 검사 for event in pygame.event.get(): if event.type == USEREVENT + 1: pass elif event.type == KEYDOWN: keys = pygame.key.get_pressed() self.keyDown(keys) elif event.type == KEYUP: self.keyUp(keys) if frame_read.stopped: frame_read.stop() break nowCounter = frame_read.frameCounter if nowCounter > previousCounter: self.screen.fill([0, 0, 0]) # 스크린 칠하기(검정) frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB) # 읽어온프레임의 색 형식 바꾸기 frame = np.rot90(frame) # 90도 회전 frame = np.flipud(frame) # 위아래 전환 face = pygame.surfarray.make_surface(frame) # 화면 형식에 맞게 전환하고 self.screen.blit(face, (0, 0)) # 탐색필요 self.frame_rect = self.make_text( self.font, "frameCount : " + str(nowCounter), blue, None, 20, 10) self.bat_rect = self.make_text( self.font, "batteryState : " + str(self.tello.battery), green, None, 40, 10) self.wifi_rect = self.make_text( self.font, "wifiState : " + str(self.tello.wifi), green, None, 60, 10) pygame.display.update() previousCounter = frame_read.frameCounter # 화면갱신 pygame.display.update() time.sleep(1 / FPS) def keyDown(self, key): if key[pygame.K_HOME]: self.tello.state = "takeoff" if key[pygame.K_END]: self.tello.sendCmd("land") self.tello.state = "land" if key[pygame.K_UP]: self.tello.speed_ud = 50 print("key up") if key[pygame.K_DOWN]: self.tello.speed_ud = -50 print("key down") if key[pygame.K_LEFT]: self.tello.speed_yaw = -50 print("key left") if key[pygame.K_RIGHT]: self.tello.speed_yaw = 50 print("key right") if key[pygame.K_w]: self.tello.speed_fb = 50 print("key foward") if key[pygame.K_s]: self.tello.speed_fb = -50 print("key back") if key[pygame.K_a]: self.tello.speed_lr = -50 if key[pygame.K_d]: self.tello.speed_lr = 50 def keyUp(self, key): if key[pygame.K_UP]: self.tello.speed_ud = 0 if key[pygame.K_DOWN]: self.tello.speed_ud = 0 if key[pygame.K_LEFT]: self.tello.speed_yaw = 0 if key[pygame.K_RIGHT]: self.tello.speed_yaw = 0 if key[pygame.K_w]: self.tello.speed_fb = 0 if key[pygame.K_s]: self.tello.speed_fb = 0 if key[pygame.K_a]: self.tello.speed_lr = 0 if key[pygame.K_d]: self.tello.speed_lr = 0 def speedNormalize(self, value, cmd, multiply): """ 이 함수는 키 눌림에 따라 하여 반환 반영되는 속도는 최고 속도를 초과하지 않음 """ if abs(value + cmd * multiply) < self.tello.max_speed: value = value + cmd * multiply else: value = self.tello.max_speed * (abs(value) / value) return value
import time, cv2 from threading import Thread from tello import Tello tello = Tello() tello.connect() keepRecording = True tello.streamon() frame_read = tello.get_frame_read() def videoRecorder(): # create a VideoWrite object, recoring to ./video.avi height, width, _ = frame_read.frame.shape video = cv2.VideoWriter('video.avi', cv2.VideoWriter_fourcc(*'XVID'), 30, (width, height)) while keepRecording: video.write(frame_read.frame) time.sleep(1 / 30) video.release() # we need to run the recorder in a seperate thread, otherwise blocking options # would prevent frames from getting added to the video recorder = Thread(target=videoRecorder) recorder.start()
class Follower: def __init__(self): pygame.init() pygame.display.set_caption("Tello video stream") self.screen = pygame.display.set_mode([960, 720]) # 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.tello = Tello() self.send_rc_control = False pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS) self.personTracker = PersonTracker() self.Px, self.Ix, self.Dx = 0.10, 0, -0 #D gain should be negative. self.Py, self.Iy, self.Dy = 0.1, 0, -0 self.Pz, self.Iz, self.Dz = 0.25, 0, -0.001 self.prev_err_x, self.prev_err_y, self.prev_err_z = None, None, None self.accum_err_x, self.accum_err_y, self.accum_err_z = 0, 0, 0 self.found_person = False self.manual = True self.iter = 0 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() frame_read = self.tello.get_frame_read() should_stop = False imprinted = False emergency_counter = 0 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 if self.iter > 240: humanBox, imprinted = self.personTracker.findMyHooman(frame) if humanBox is None and self.send_rc_control and not self.manual: emergency_counter += 1 if emergency_counter >= 120: #missed human for 120 frames; 1 second print("ENGAGING EMERGENCY HOVER.") cv2.putText(frame, "EMERGENCY HOVER", (700, 130), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2) self.emergencyHover() elif humanBox is not None: if self.found_person == False: self.found_person = True emergency_counter = 0 #PID LOOP: desired_pos = (frame.shape[1] // 2, frame.shape[0] // 2 ) #format x,y if humanBox is not None: xmin = int(humanBox[1] * frame.shape[1]) ymin = int(humanBox[0] * frame.shape[0]) xmax = int(humanBox[3] * frame.shape[1]) ymax = int(humanBox[2] * frame.shape[0]) centerHumanPosition = (np.mean( (xmax, xmin)), np.mean((ymax, ymin))) #format x,y #draw bounding box cv2.rectangle(frame, (xmin, ymin), (xmax, ymax), (255, 0, 0), 2) #blue #draw target coord cv2.circle(frame, (int( centerHumanPosition[0]), int(centerHumanPosition[1])), 10, (255, 0, 0), 1) #blue # print("z width: {}".format(np.abs(xmax-xmin))) #draw desired coord cv2.circle(frame, desired_pos, 10, (0, 0, 255), 1) #red if self.send_rc_control and not self.manual: self.update_control(centerHumanPosition, desired_pos, xmax, xmin) text = "Battery: {}%".format(self.tello.get_battery()) cv2.putText(frame, text, (5, 720 - 5), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) heightText = "Height:{}".format(self.tello.get_height()) cv2.putText(frame, heightText, (720 - 5, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) manualText = "Manual: {}".format(self.manual) if self.manual: cv2.putText(frame, manualText, (720 - 5, 70), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) else: cv2.putText(frame, manualText, (720 - 5, 70), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) imprintedTxt = "Imprinted: {}".format(imprinted) if imprinted: cv2.putText(frame, imprintedTxt, (720 - 5, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2) else: cv2.putText(frame, imprintedTxt, (720 - 5, 100), 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() if self.iter <= 240: self.iter += 1 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_m: # set yaw clockwise velocity self.manual = not self.manual self.yaw_velocity = 0 self.up_down_velocity = 0 self.left_right_velocity = 0 print("MANUAL MODE IS NOW: {}".format(self.manual)) 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.iter = 0 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 update_control(self, curr_lateral_pos, desired_later_pos, xmax, xmin): #Three error directions. Two lateral, one forward/backward. How to calc forward/back error? err_x = curr_lateral_pos[0] - desired_later_pos[ 0] #if positive, we're to the left of where we want to be, so want positive control. (CHECK THIS) err_y = desired_later_pos[1] - curr_lateral_pos[ 1] #if positive, we're below where we want to be. (CHECK THIS) #hardcode desired box width. Must test!! desired_width = 350 curr_width = np.abs(xmax - xmin) #check. is this actually the width dim? err_z = desired_width - curr_width #if negative, too close; want backwards--> positive gain # print("Err z: {}".format(err_z)) if self.prev_err_x == None: derivative_x_input = 0 derivative_y_input = 0 derivative_z_input = 0 else: derivative_x_input = (err_x - self.prev_err_x) / (1 / FPS) derivative_y_input = (err_y - self.prev_err_y) / (1 / FPS) derivative_z_input = (err_z - self.prev_err_z) / (1 / FPS) #clip derivative errors to avoid noise derivative_x_input = np.clip(derivative_x_input, -11000, 11000) derivative_y_input = np.clip(derivative_y_input, -11000, 11000) derivative_z_input = np.clip(derivative_z_input, -11000, 11000) self.accum_err_x += err_x self.accum_err_y += err_y self.accum_err_z += err_z self.prev_err_x = err_x self.prev_err_y = err_y self.prev_err_z = err_z # print("derr_z: {}".format(derivative_z_input)) # self.left_right_velocity = self.Px*err_x+self.Dx*derivative_x_input+self.Ix*self.accum_err_x self.yaw_velocity = self.Px * err_x + self.Dx * derivative_x_input + self.Ix * self.accum_err_x self.up_down_velocity = self.Py * err_y + self.Dy * derivative_y_input + self.Iy * self.accum_err_y self.for_back_velocity = self.Pz * err_z + self.Dy * derivative_z_input + self.Iz * self.accum_err_z #limit velocity to 2*S. # self.left_right_velocity = np.clip(self.left_right_velocity, -S*2, S*2) self.yaw_velocity = int(np.clip(self.yaw_velocity, -S, S)) self.up_down_velocity = int(np.clip(self.up_down_velocity, -S, S)) self.for_back_velocity = int(np.clip(self.for_back_velocity, -S, S)) #Send new velocities to robot. self.update() def emergencyHover(self): print("Cannot find hooman. I am lonely doggo. Hovering and rotating.") self.found_person = False self.for_back_velocity = 0 self.left_right_velocity = 0 self.up_down_velocity = 0 self.yaw_velocity = int(S // 2) self.update()
class FrontEnd(object): def __init__(self): # Init pygame pygame.init() # 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.faceFound = False self.send_rc_control = True 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() if self.tello.takeoff(): print('Takeoff successfull') self.tello.connect() star = time.time() while True: # if time.time() - star > 10: # self.tello.connect() # star = time.time() pygame.event.pump() frame = frame_read.frame gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, 1.1, 10, minSize=(65, 65)) for (x, y, w, h) in faces: self.faceFound = True cv2.rectangle(frame, (x, y), (x+w, y+h), (255, 0, 0), 2) cx = (2*x+w)/2 cy = (2*y+h)/2 faceSize = (w+h)/2 if self.faceFound: yaw_dif = abs(cx - 480) updo_dif = abs(cy - 360) face_dif = abs(faceSize - 140) sign_yaw = (cx - 480)/yaw_dif sign_updo = (360 - cy)/updo_dif sign_face = (140 - faceSize)/face_dif if yaw_dif > 65: self.yaw_velocity = sign_yaw*S#*(yaw_dif/65) else: self.yaw_velocity = 0 if updo_dif > 45: self.up_down_velocity = sign_updo*S#*(updo_dif/45) else: self.up_down_velocity = 0 if face_dif > 35: self.for_back_velocity = sign_face*S#*(face_dif/35) else: self.for_back_velocity = 0 else: self.for_back_velocity = 0 self.yaw_velocity = 0 self.up_down_velocity = 0 self.update() cv2.imshow('img', frame) frame_read.out.write(frame) self.faceFound = False k = cv2.waitKey(30) & 0xff if k==27: break self.tello.end() frame_read.out.release() frame_read.cap.release() cv2.destroyAllWindows() return 0 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)