파이썬으로 드론 조종하기 (얼굴인식)

얼굴인식으로 이륙한 뒤 키보드로 드론 조종하는 코드입니다.

 

얼굴인식 이륙, 키보드 조종

 

from time import sleep
from e_drone.drone import *
from e_drone.protocol import *
import cv2 as cv
import keyboard

is_quit = False
is_takeOff = False
ROI = 100
frame_name = "face_detect"
face_detection = 1 

capture = cv.VideoCapture(0)
face_detector = cv.CascadeClassifier()
face_detector.load(r'C:\opencv\haarcascade_frontalface_default.xml')

 

필요한 패키지들을 불러오고, 필요한 변수들을 만듭니다. is_quit은 영상 관련 변수이고, is_takeOff는 드론 관련 변수입니다. ROI는 범위입니다. cv.VideoCapture(0)로 비디오 객체를 생성하고, cv.CascadeClassifier()로 분류기 객체 생성한 뒤, face_detector.load로 경로를 입력합니다.

 

def eventTrim(trim):
    print("{0}, {1}, {2}, {3}".format(trim.roll, trim.pitch, trim.yaw, trim.throttle))

if __name__ == '__main__':
    drone = Drone()
    drone.open("com5")
    drone.setEventHandler(DataType.Trim, eventTrim)
    drone.sendTrim(-16, 5, 0, 0)
    sleep(1)
    drone.sendRequest(DeviceType.Drone, DataType.Trim)

 

미세 조종(Trim) 함수와 if __name__ == '__main__': 밑은 드론 관련 함수입니다. drone = Drone()으로 드론 객체를 만들고, drone.open("com5")로 드론을 연결합니다. drone.setEventHandler로 Trim정보를 받고, drone.sendTrim(-16, 5, 0, 0)으로 미세 설정한 뒤, drone.sendRequest로 미세 조종한 Trim값을 드론으로 보냅니다.

 

def face(image, ROI, W, H):
    roi = image[H - ROI :H + ROI, W - ROI:W + ROI]
    faces = face_detector.detectMultiScale(roi, 1.3, 5, 10)
    if(len(faces) == 0):
        return False
    else:
        return True

 

• face함수는 얼굴을 인식했는지 확인하는 함수입니다.
• face에는 매개변수 image, ROI, W, H를 넣습니다. ROI에는 100을 할당했습니다.
• roi는 image크기를 설정합니다. [H - ROI :H + ROI, W - ROI:W + ROI] 코드는 정사각형이 만들어집니다.
• face_detect.detectMultiScale로 face_detect함수의 얼굴을 조금 더 잘 인식하게 만듭니다. roi의 범위 안에 인식한 얼굴이 faces에 저장이 되는데 그 값이 0이라면 얼굴인식이 안된거고 0이 아니라면 얼굴인식이 된 거라서 True를 리턴합니다.

 

def face_detect(frame): 
    h, w, channel = frame.shape   
    w_half = int(w / 2)
    h_half = int(h / 2)
    font = cv.FONT_HERSHEY_DUPLEX
    if (h_half < ROI or w_half < ROI):
        print("ROI오류")
    while(not face(frame, ROI, w_half, h_half)):
        cv.rectangle(frame, (w_half - ROI, h_half - ROI), (w_half + ROI, h_half + ROI), (0, 0, 200), 8)
        cv.imshow(frame_name, frame)
        cv.waitKey(1)
        ret, frame = capture.read()
        frame = cv.flip(frame, 1)
    cv.putText(frame, "START ", (w_half - 135, h_half + 20) , font , 3, (255, 255, 255), 3)
    cv.imshow(frame_name, frame)
    cv.waitKey(1)

 

• face_detect함수는 사진을 찍었을 때 실행되는 함수입니다.
• face_detect에 frame을 넣습니다. frame은 웹캠 영상을 말합니다.
• shape은 영상의 높이와 너비와 채널을 알려줍니다.
• w_half은 영상 너비의 반을, h_half은 영상 높이의 반을 표시합니다.
• w_half, h_half에서 ROI를 뺄 건데 ROI가 너무 크면 -값이 나오면 오류가 뜨니 if를 사용해서 만약에 ROI가 너무 크면 ROI오류라는 문구가 나오게 설정합니다.
• while문을 사용해서 얼굴을 인식할 때까지 반복합니다. 아까 만든 face함수에 frame, ROI, w_half, h_half를 넣습니다.
• cv.rectangle을 사용해서 사각형을 그립니다. 크기는(w_half - ROI, h_half - ROI), (w_half + ROI, h_half + ROI), 색상은 레드, 두께는 8로 표시합니다.
• cv.imshow로  영상을 보여주고, cv.waitKey(1)로 0.001초를 기다립니다. cv.waitKey(1)가 없다면 너무 빨라서 오류가 발생할 수 있습니다.
• capture.read()로 영상을 읽고 frame에 저장한 뒤 frame은 cv.flip(frame, 1)로 좌우반전을 해줍니다.(거울 모드) 얼굴을 안 읽었다면 while문을 반복하고 얼굴을 읽으면 cv.putText으로 넘어갑니다.
• cv.putText(영상에서 "START를 표시하는데 좌표는 (w_half - 135, h_half + 20), 폰트를 설정하고 크기는 3 색상은 하얀색, 두께는 3으로 표시합니다.)
• cv.imshow로 글자를 보여줍니다.

 

while (not is_quit):
    if(capture.isOpened()):
        ret, frame = capture.read()
        frame = cv.flip(frame, 1)
        print("준비완료")
        if (face_detection == 1):
            face_detect(frame)
    else:   #비디오가안열렸다면
        print("비디오안열림")

 

• 이제 나오는 while문은 실행하는 함수입니다.
• not is_quit을 해서 True가 될 때까지 반복합니다.
• capture=웹캠이 isOpened()=열렸는지 확인합니다. 열렸다면 밑으로 가고 아니라면 else로 갑니다.
• capture.read()로 영상을 읽고, cv.flip(frame, 1)로 좌우반전(거울 모드)을 합니다. 여기까지 했다면 print("준비완료")로 준비완료를 출력합니다.
• if (face_detection == 1): 만약에 face_detection이 1이라면 face_detect(frame) 함수를 실행합니다.
• 웹캠이 안 열렸다면 비디오 안 열림을 출력합니다.

 

    while True:
        cv.waitKey(1)
        print(keyboard.read_key())       
        if not is_takeOff:
            if keyboard.is_pressed('1'):
                print("이륙")
                drone.sendTakeOff()
                sleep(1)
                drone.sendControlWhile(0, 0, 0, 0, 2000) 
                is_takeOff = True

 

• 두 번째 while문은 키보드로 조종하는 실행문입니다.
• 0.001초를 기다리고 keyboard.read_key()로 무슨 키를 눌렀는지 출력합니다.
• if not is_takeOff: is_takeOff가 True가 될 때까지 반복합니다.
• if keyboard.is_pressed('1'): 1번 키를 누르면 실행됩니다.
• print("이륙")으로 이륙을 출력합니다.
• drone.sendTakeOff()으로 드론을 이륙합니다.
• drone.sendControlWhile(0, 0, 0, 0, 2000) 2초간 호버링 합니다. 그 뒤 is_takeOff를 True로 만듭니다.

 

        if is_takeOff:
            if keyboard.is_pressed("w"):
                print("위로")
                drone.sendControl(0, 0, 0, 50)
            elif keyboard.is_pressed("s"):
                print("아래로")
                drone.sendControl(0, 0, 0, -50)
            elif keyboard.is_pressed("a"):
                print("왼쪽회전")
                drone.sendControl(0, 0, 50, 0)
            elif keyboard.is_pressed("d"):
                print("오른쪽회전")
                drone.sendControl(0, 0, -50, 0)
            elif keyboard.is_pressed("up"):
                print("직진")
                drone.sendControl(0, 50, 0, 0)          
            elif keyboard.is_pressed("down"):
                print("뒤로")
                drone.sendControl(0, -50, 0, 0)              
            elif keyboard.is_pressed("right"):
                print("오른쪽")
                drone.sendControl(50, 0, 0, 0)
            elif keyboard.is_pressed("left"):
                print("왼쪽")
                drone.sendControl(-50, 0, 0, 0)
            elif keyboard.is_pressed("2"):
                print("제자리")
                drone.sendControl(0, 0, 0, 0)
            elif keyboard.is_pressed("3"):
                print("착륙")
                drone.sendLanding()
                sleep(1)
                drone.sendLanding()
                sleep(1)
                is_quit = True
                break
        if keyboard.is_pressed('i'):
            drone.sendStop()
            is_quit = True
            break

 

• if is_takeOff:는 is_takeOff가 True라면 실행하는 함수입니다.
• 각각 키보드로 조종하는 함수이고 맨 마지막에 elif keyboard.is_pressed("3"):부분은 착륙하고  is_quit = True로 첫 번째 while문을 나갑니다.
• if keyboard.is_pressed('i'):은 i키를 누르면 실행됩니다.
• drone.sendStop()은 드론 기능이 아예 멈춥니다. 공중에 떠있다면 급추락하기 때문에 사용하면 기계가 손상 날 수 있습니다.
• is_quit = True로 첫 번째 while문을 나갑니다.

 

capture.release()
cv.destroyAllWindows()
print("Done")

 

capture.release()는 웹캠 촬영을 멈추고, cv.destroyAllWindows()로 창을 닫고, Done이라고 출력합니다.

 

 

전체 코드

 

from time import sleep
from e_drone.drone import *
from e_drone.protocol import *
import cv2 as cv
import keyboard

is_quit = False #False를 저장,비디오관련
is_takeOff = False  #False를 저장,드론관련
ROI = 100 #범위 
frame_name = "face_detect" #창 이름
face_detection = 1 

capture = cv.VideoCapture(0)    #객체생성
face_detector = cv.CascadeClassifier()  #객체생성
face_detector.load(r'C:\opencv\haarcascade_frontalface_default.xml')    #경로입력

def eventTrim(trim):
    print("{0}, {1}, {2}, {3}".format(trim.roll, trim.pitch, trim.yaw, trim.throttle))

if __name__ == '__main__':
    drone = Drone()
    drone.open("com5")
    drone.setEventHandler(DataType.Trim, eventTrim)
    drone.sendTrim(-16, 5, 0, 0)
    sleep(1)
    drone.sendRequest(DeviceType.Drone, DataType.Trim)

def face(image, ROI, W, H):
    roi = image[H - ROI :H + ROI, W - ROI:W + ROI]  #정사각형만듬
    faces = face_detector.detectMultiScale(roi, 1.3, 5, 10) #얼굴을 더 선명하게 만듬
    if(len(faces) == 0):    #얼굴을 읽었으면 True 못읽으면 False
        return False
    else:
        return True
        
def face_detect(frame):
    #사진을 찍었을 때 크기 
    h, w, channel = frame.shape   
    w_half = int(w / 2) #너비반값
    h_half = int(h / 2) #높이반값
    font = cv.FONT_HERSHEY_DUPLEX
    if (h_half < ROI or w_half < ROI):  #ROI값이 너무클때 알려줍니다.
        print("ROI오류")
    while(not face(frame, ROI, w_half, h_half)):    #face읽을때까지 반복
        ###rectangle=사각형을만듬,(좌표에서ROI만큼 빼고더해서 정사각형만듬)
        cv.rectangle(frame, (w_half - ROI, h_half - ROI), (w_half + ROI, h_half + ROI), (0, 0, 200), 8)
        cv.imshow(frame_name, frame)    #창이름과영상을보여줌
        cv.waitKey(1) #1밀리초를 기다린다. 없으면 너무 빨라서 프로그램 오류가 난다.
        ret, frame = capture.read() #계속 영상을 촬영하기
        frame = cv.flip(frame, 1)   #좌우반전(거울모드)
    #얼굴읽으면START표시,좌표,폰트,크기,색상,굵기
    cv.putText(frame, "START ", (w_half - 135, h_half + 20) , font , 3, (255, 255, 255), 3)
    cv.imshow(frame_name, frame)    #글자를 보여줌
    cv.waitKey(1)

while (not is_quit):    #while문1,True될때까지반복
    if(capture.isOpened()): #캠이열림
        ret, frame = capture.read() #영상촬영
        frame = cv.flip(frame, 1)   #좌우반전(거울모드)
        print("준비완료")
        if (face_detection == 1):   #face_detection가1이라면
            face_detect(frame)  #face_detect(frame)실행
    else:   #비디오가안열렸다면
        print("비디오안열림")
    while True: #while문2
        cv.waitKey(1)
        print(keyboard.read_key())  #어떤키눌렀는지확인       
        if not is_takeOff:  #is_takeOff가 False라면 #
            if keyboard.is_pressed('1'):    #1번 누르면 이륙
                print("이륙")
                drone.sendTakeOff()
                sleep(1)
                drone.sendControlWhile(0, 0, 0, 0, 2000) 
                is_takeOff = True   #이륙하면is_takeOff는True가됩니다.
        if is_takeOff:  #is_takeOff가 True라면 키보드조종합니다.
            if keyboard.is_pressed("w"):
                print("위로")
                drone.sendControl(0, 0, 0, 50)
            elif keyboard.is_pressed("s"):
                print("아래로")
                drone.sendControl(0, 0, 0, -50)
            elif keyboard.is_pressed("a"):
                print("왼쪽회전")
                drone.sendControl(0, 0, 50, 0)
            elif keyboard.is_pressed("d"):
                print("오른쪽회전")
                drone.sendControl(0, 0, -50, 0)
            elif keyboard.is_pressed("up"):
                print("직진")
                drone.sendControl(0, 50, 0, 0)          
            elif keyboard.is_pressed("down"):
                print("뒤로")
                drone.sendControl(0, -50, 0, 0)              
            elif keyboard.is_pressed("right"):
                print("오른쪽")
                drone.sendControl(50, 0, 0, 0)
            elif keyboard.is_pressed("left"):
                print("왼쪽")
                drone.sendControl(-50, 0, 0, 0)
            elif keyboard.is_pressed("2"):
                print("제자리")
                drone.sendControl(0, 0, 0, 0)
            elif keyboard.is_pressed("3"):
                print("착륙")
                drone.sendLanding()
                sleep(1)
                drone.sendLanding()
                sleep(1)
                is_quit = True  #비디오가꺼지면 True가됨
                break   #while문2에서 나갑니다.while문1로이동is_quit = True라서 while문1도 나갑니다.
        if keyboard.is_pressed('i'):    #급추락합니다.
            drone.sendStop()	#드론멈춤
            is_quit = True
            break
            
capture.release()
cv.destroyAllWindows()
print("Done")