class drawMov: def __init__(self): self.tx, self.ty, self.bx, self.by = None, None, None, None self.top = None self.bottom = None self.left = None self.right = None self.width = None self.height = None self.center = None self.mamboAddr, self.mamboName = None, None self.mambo = None self.droneCheck = False self.droneBattery = None self.inBox = (220, 300) #(130,300)->(250,300)->(260,380) width height self.outBox = (340, 370) #(200,380)->(330,380)->(330,450) width height self.inBoxPos = [] self.outBoxPos = [] # 드론의 상태를 업데이트 한다 def update(self): self.mambo.smart_sleep(0.01) # 드론의 배터리 상태를 갱신한다 self.droneBattery = int(self.mambo.sensors.battery) print("Battery:", self.droneBattery, "% State:", self.mambo.sensors.flying_state) # 타겟의 좌표를 갱신한다 def setTarget(self, target): self.tx, self.ty, self.bx, self.by = int(target[0]), int( target[1]), int(target[2]), int(target[3]) self.top = self.ty self.bottom = self.by self.left = self.tx self.right = self.bx self.width = self.right - self.left self.height = self.bottom - self.top self.center = self.getCenter(target) # 드론 연결 def droneConnect(self): # 일반 패키지의 경우에만 사용하며, 블루투스를 통해 드론을 찾는다 self.mamboAddr, self.mamboName = findMinidrone.getMamboAddr() # FPV의 경우 use_wifi=True, 일반 패키지의 경우 use_wifi=False self.mambo = Mambo(self.mamboAddr, use_wifi=False) self.droneCheck = self.mambo.connect(num_retries=3) print("Drone Connect: ", self.droneCheck) self.mambo.smart_sleep(2) # 드론의 상태를 받아온다 최초 1회만 하면 됨 self.mambo.ask_for_state_update() self.mambo.set_max_tilt(1) # 드론 이륙 def droneStart(self): print('take off') self.mambo.safe_takeoff(5) # 이미지의 크기를 기준으로 드론 움직임의 기준이 되는 InBOX, OutBOX 표현 def getInOutBoxPos(self, img): standardCenter = self.getStandardCenter(img) self.inBoxPos = [ int(standardCenter[0] - self.inBox[0] / 2), int(standardCenter[1] - self.inBox[1] / 2), int(standardCenter[0] + self.inBox[0] / 2), int(standardCenter[1] + self.inBox[1] / 2) ] self.outBoxPos = [ int(standardCenter[0] - self.outBox[0] / 2), int(standardCenter[1] - self.outBox[1] / 2), int(standardCenter[0] + self.outBox[0] / 2), int(standardCenter[1] + self.outBox[1] / 2) ] return standardCenter # 드론 착륙 및 연결 해제 def droneStop(self): if not self.mambo.sensors.flying_state == 'landed': self.mambo.safe_land(5) try: self.mambo.disconnect() except: print("No Ground Cam!!") print("Complete to Stop the Drone!") def getCenter(self, bbox): return [int((bbox[2] + bbox[0]) / 2), int((bbox[3] + bbox[1]) / 2)] def drawLine(self, img): #moveCenter=self.getCenter(bbox) moveCenter = self.getStandardCenter(img) cv2.line(img, (self.center[0], self.center[1]), (moveCenter[0], moveCenter[1]), (255, 0, 0), 2) # 타겟의 위치와 이미지의 중앙점을 선으로 잇고 -Y축 기준으로 드론의 회전 각을 계산한다 def getAngle(self, img): moveCenter = self.getStandardCenter(img) distance = math.sqrt((moveCenter[0] - self.center[0])**2 + (moveCenter[1] - self.center[1])**2) cTheta = (moveCenter[1] - self.center[1]) / (distance + 10e-5) angle = math.degrees(math.acos(cTheta)) return angle def drawCenter(self, img): cv2.circle(img, tuple(self.center), 2, (255, 0, 0), -1) # 드론의 수직이동 값 계산 def adjustVertical(self): vertical = 0 ih = self.inBoxPos[1] oh = self.outBoxPos[1] vt = self.top if vt < oh: vertical = 10 elif vt > ih: vertical = -10 return vertical # 드론의 수평이동, Yaw, Yaw 횟수 계산 def adjustCenter(self, img, stack, yawTime): # right + , front +, vertical roll, yaw = 0, 0 angle = 0 yawCount = yawTime stackLR = stack standardCenter = self.getStandardCenter(img) #cv2.putText(img,"center",tuple(standardCenter),cv2.FONT_HERSHEY_COMPLEX_SMALL,1,(0,0,255),2) roll = self.center[0] - standardCenter[0] if roll < -1: roll = -20 stackLR -= 1 elif roll > 1: roll = 20 stackLR += 1 if yawCount < -1: yaw = -50 yawCount += 1 elif yawCount > 1: yaw = 50 yawCount -= 1 if stackLR > 20: angle = self.getAngle(img) stackLR = 0 print('angle: ', angle) yawCount = int(angle / 7) elif stackLR < -20: angle = -(self.getAngle(img)) stackLR = 0 print('angle: ', angle) yawCount = int(angle / 7) return roll, yaw, stackLR, yawCount def getStandardCenter(self, img): return [int(img.shape[1] / 2), int(img.shape[0] / 2 + 120)] #80->150->0->80 def drawStandardBox(self, img): standardCenter = self.getInOutBoxPos(img) cv2.circle(img, tuple(standardCenter), 2, (0, 0, 255), -1) cv2.rectangle(img, (self.inBoxPos[0], self.inBoxPos[1]), (self.inBoxPos[2], self.inBoxPos[3]), (0, 0, 255), 1) cv2.rectangle(img, (self.outBoxPos[0], self.outBoxPos[1]), (self.outBoxPos[2], self.outBoxPos[3]), (0, 0, 255), 1) def adjustBox(self, img): pitch = 0 if self.width * self.height < self.inBox[0] * self.inBox[1]: pitch = 30 elif self.width * self.height > self.outBox[0] * self.outBox[1]: pitch = -30 return pitch # 타겟의 위치에 따른 드론의 직접적인 움직임 제어 def adjPos(self, img, target, angleStack, yawTime): roll, pitch, yaw, vertical, duration = 0, 0, 0, 0, 0.1 angle = 0 self.drawStandardBox(img) stack = angleStack cv2.putText(img, "Following The Target", (5, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) self.setTarget(target) vertical = self.adjustVertical() if vertical == 0: pitch = self.adjustBox(img) roll, yaw, stack, yawTime = self.adjustCenter(img, stack, yawTime) pos = [roll, pitch, yaw, vertical] if pos == [0, 0, 0, 0]: stack = 0 else: self.mambo.fly_direct(roll=roll, pitch=pitch, yaw=yaw, vertical_movement=vertical, duration=duration) print('Roll:', roll, ' Pitch:', pitch, ' Yaw:', yaw, ' Vertical:', vertical) return stack, yawTime
def main(): tc = tracker.createTrackerByName("KCF") # 일반모드에서 블루투스를 사용하여 드론을 찾는다, 드론의 연결 주소와 이름이 반환된다 # addr,name = findMinidrone.getMamboAddr() addr = None # 드론 객체 생성 FPV의 경우는 WIFI를 사용하며, use_wifi = True가 된다 mambo = Mambo(addr, use_wifi=True) # 드론을 연결한다 success = mambo.connect(3) print("Connect: %s" % success) mambo.smart_sleep(0.01) # 드론의 상태를 업데이트 한다 mambo.ask_for_state_update() # 드론의 움직임 기울기를 1로 설정한다 mambo.set_max_tilt(1) # 드론의 배터리를 확인 할 수 있다 battery = mambo.sensors.battery # FPV 연결 mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=30) userVision = UserVision(mamboVision) mamboVision.set_user_callback_function(userVision.get_image, user_callback_args=None) cv2.namedWindow('Vision') cv2.setMouseCallback('Vision', draw_bbox) success = mamboVision.open_video() img = cv2.imread(mamboVision.filename, cv2.IMREAD_COLOR) mask = np.ones_like(img, dtype=np.uint8) tc = tracker.createTrackerByName("KCF") # Tracking Mode mode = False bbox = [] angleStack, yawTime = 0, 0 mov = drawMov() mov.mambo = mambo while (True): # OpenCV를 바탕으로 드론을 제어 하기위해 마스킹이미지를 생성한다 # mask = np.ones((480,600,3),dtype=np.uint8) img = cv2.imread(mamboVision.filename, cv2.IMREAD_COLOR) print(img.shape) # 드론과 연결이 끊기지 않기 위해 매 프레임마다 상태 확인 신호를 보낸다 mambo.smart_sleep(0.01) # 드론의 배터리를 확인 할 수 있다 battery = mambo.sensors.battery print("Battery: %s" % battery) #img=cv2.add(img,mask) #if mode==True: # bbox=tracker.updateTracker(img,tc) # angleStack,yawTime=mov.adjPos(mask,bbox,angleStack,yawTime) cv2.imshow("Vision", mask) key = cv2.waitKey(10) # 'q' 키를 누르면 드론을 착륙하고 프로세스를 종료한다 if ord('q') == key: mambo.safe_land(5) mamboVision.close_video() exit(0) # 'p' 키를 누르면 드론이 이륙한다 elif ord('p') == key: mambo.safe_takeoff(5) # 'w' 키를 누르면 드론에 앞으로 100 만큼 움직이라는 신호를 0.1초간 보낸다 elif ord('w') == key: mambo.fly_direct(roll=0, pitch=100, yaw=0, vertical_movement=0, duration=0.1) # 's' 키를 누르면 드론에 뒤로 100 만큼 움직이라는 신호를 0.1초간 보낸다 elif ord('s') == key: mambo.fly_direct(roll=0, pitch=-100, yaw=0, vertical_movement=0, duration=0.1) # 'a' 키를 누르면 드론에 왼쪽으로 50 만큼 움직이라는 신호를 0.1초간 보낸다 elif ord('a') == key: mambo.fly_direct(roll=-50, pitch=0, yaw=0, vertical_movement=0, duration=0.1) # 'd' 키를 누르면 드론에 오른쪽으로 50 만큼 움직이라는 신호를 0.1초간 보낸다 elif ord('d') == key: mambo.fly_direct(roll=50, pitch=0, yaw=0, vertical_movement=0, duration=0.1) elif ord('i') == key: mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=50, duration=0.1) elif ord('k') == key: mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-50, duration=0.1) elif ord('j') == key: mambo.fly_direct(roll=0, pitch=0, yaw=-50, vertical_movement=0, duration=0.1) elif ord('l') == key: mambo.fly_direct(roll=0, pitch=0, yaw=50, vertical_movement=0, duration=0.1) elif ord('c') == key: mask = np.ones((480, 600, 3), dtype=np.uint8) elif ord('v') == key: print("tracker start") bbox = [tx, ty, bx - tx, by - ty] mode = True ok = tc.init(img, bbox) mov.setBox([tx, ty, bx, by])