def main(): isrec = 0 # muitmap=0 tello = Tello() pydisplay = Pydisplay() keyuser = Keyuser() # 键盘命令 ui = UID() mapui = Mapui() # if muitmap==0: # mapui=Mapui() # else: # mapstack,mapsd = Pipe() # #stack= Manager().list() # mapshowing = Process(target=mapread, args=(mapstack,)) # mapshowing.start() pose = Pose() com = Com() mapcom = Mapcom() frame_skip = 300 # pidtuning # if mapcom.tpid==1: # pidimg=np.zeros((500, 512, 3), np.uint8) # cv2.namedWindow('pidyaw') # cv2.namedWindow('pidthro') # cv2.namedWindow('pidpith') # cv2.namedWindow('pidroll') # cv2.createTrackbar('p', 'pidyaw', 0, 100, nothing) # cv2.createTrackbar('i', 'pidyaw', 0, 100, nothing) # cv2.createTrackbar('d', 'pidyaw', 0, 100, nothing) # cv2.createTrackbar('down', 'pidyaw', 0, 100, nothing) # cv2.createTrackbar('up', 'pidyaw', 0,100, nothing) # cv2.createTrackbar('p', 'pidthro', 0, 100, nothing) # cv2.createTrackbar('i', 'pidthro', 0, 100, nothing) # cv2.createTrackbar('d', 'pidthro', 0, 100, nothing) # cv2.createTrackbar('down', 'pidthro', 0, 100, nothing) # cv2.createTrackbar('up', 'pidthro', 0,100, nothing) # cv2.createTrackbar('p', 'pidpith', 0, 100, nothing) # cv2.createTrackbar('i', 'pidpith', 0, 100, nothing) # cv2.createTrackbar('d', 'pidpith', 0, 100, nothing) # cv2.createTrackbar('down', 'pidpith', 0, 100, nothing) # cv2.createTrackbar('up', 'pidpith', 0,100, nothing) # cv2.createTrackbar('p', 'pidroll', 0, 100, nothing) # cv2.createTrackbar('i', 'pidroll', 0, 100, nothing) # cv2.createTrackbar('d', 'pidroll', 0, 100, nothing) # cv2.createTrackbar('down', 'pidroll', 0, 100, nothing) # cv2.createTrackbar('up', 'pidroll', 0,100, nothing) # 录像功能 if isrec: stack, sd = Pipe() #stack= Manager().list() pr = Process(target=read, args=(stack, )) pr.start() # try: for frame in tello.container.decode(video=0): # 一定要用这个循环来获取才不会产生delay if 0 < frame_skip: frame_skip = frame_skip - 1 continue start_time = time.time() image2surface = numpy.array(frame.to_image()) # 做个拷贝给pygame image = cv2.cvtColor(image2surface, cv2.COLOR_RGB2BGR) key_list = pygame.key.get_pressed() imageraw = image image = cv2.resize(image, (640, 480)) # 这个太大会爆显存 userc = keyuser.usec(key_list) # 来自用户输入的命令 # userc[0 1 2 3 4 5 ] # 是否使用openpose 四个通道 模式 if userc[4] == 0 or userc[4] == 1: if userc[4] == 1: # 判断使用跟踪 kp, out = pose.get_kp(image) else: # 不使用 kp = [[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0]] comd = com.get_comd(kp, userc) # 接受两个数组进行判断 tello.send_comd(comd) flight = tello.send_data() # 飞行数据 com.read_tello_data(flight) # 飞控获取数据用于判断指令 flightstate = com.get_state() # 命令状态 if userc[4] == 1: # 使用 rec = ui.show(out, kp, flightstate) # 显示并负责播放声音 else: # 不用 rec = ui.show(imageraw, 0, flightstate) if isrec: write(sd, rec) # print(userc[4]) elif userc[4] == 2 or userc[4] == 3: # if mapcom.tpid==1: # cv2.imshow('pidyaw', pidimg) # cv2.imshow('pidthro', pidimg) # cv2.imshow('pidpith', pidimg) # cv2.imshow('pidroll', pidimg) # pid=[[cv2.getTrackbarPos('p', 'pidyaw')/10,cv2.getTrackbarPos('i', 'pidyaw')/100,cv2.getTrackbarPos('d', 'pidyaw')/10,-cv2.getTrackbarPos('down', 'pidyaw'),cv2.getTrackbarPos('up', 'pidyaw')], # [cv2.getTrackbarPos('p', 'pidthro')/10,cv2.getTrackbarPos('i', 'pidthro')/100,cv2.getTrackbarPos('d', 'pidthro')/10,-cv2.getTrackbarPos('down', 'pidthro'),cv2.getTrackbarPos('up', 'pidthro')], # [cv2.getTrackbarPos('p', 'pidpith')/10,cv2.getTrackbarPos('i', 'pidpith')/100,cv2.getTrackbarPos('d', 'pidpith')/10,-cv2.getTrackbarPos('down', 'pidpith'),cv2.getTrackbarPos('up', 'pidpith')], # [cv2.getTrackbarPos('p', 'pidroll')/10,cv2.getTrackbarPos('i', 'pidroll')/100,cv2.getTrackbarPos('d', 'pidroll')/10,-cv2.getTrackbarPos('down', 'pidroll'),cv2.getTrackbarPos('up', 'pidroll')]] data = tello.send_data() mapcom.readflightdata(data) # if mapcom.tpid==1: # comd=mapcom.com(userc,pid) # else: comd = mapcom.com(userc) flightstate = mapcom.send_flightdata() tello.send_comd(comd) checkoutmap = mapcom.checkalldone() if checkoutmap == 1: userc[4] = 0 keyuser.us[4] = 0 mapcom.checkdone = None if isrec: write(sd, imageraw) mapui.mapshow(flightstate) # if muitmap==0: # mapui.mapshow(flightstate) # else: # mapsand(mapsd,flightstate) pydisplay.display(image2surface, flightstate) # pygame飞行界面 # 目前对丢帧策略的理解,只要分母不要小于飞机发送回来的最大帧速率则不会产生延迟同时保证帧率 # 例子里的60是不合理的,会多丢弃一半的帧,浪费辽 if frame.time_base < 1.0 / 35: if userc[4] == 1: time_base = 1.0 / 35 # 使用pose稍微保守一点 else: time_base = 1.0 / 35 else: time_base = frame.time_base frame_skip = int((time.time() - start_time) / time_base) k = cv2.waitKey(1) & 0xff # 与pygame的键盘存在未知冲突 if k == 27: pygame.display.quit() tello.drone.quit() # 退出 break # print(time.time()-start_time) # except: # print('连接超时或发生错误退出辽') cv2.destroyAllWindows() # 关掉飞机直接退出程序 tello.drone.quit() pygame.display.quit()
import Com p = Com('https://habr.com/post/127584/')