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])
