[스마트팜 드론 자율주행] Tello 조종 및 CPP 알고리즘 코딩, 그리고 이미지 전송

이전 포스팅에서 언급한 프로젝트의 연장입니다.

저번 포스팅에서 언급한 프로젝트의 방향이 약간 변경되어 다시 간략히 설명하자면,

노지에 있는 포도 농가를 대상으로 '포도알 솎아주기' 작업을 수월하게 할 수 있도록 포도 알 개수, 솎아줘야할 불량 포도 알 개수, 포도 등급을 리포트해주는 서비스입니다. 이 서비스로 일일이 포도 알 개수를 세지 않고 솎아줘야할 포도가 있는 구역으로 바로 가서 작업할 수 있도록 하는 것이 목표입니다.

저는 여기서 드론 자율주행 파트를 맡았습니다.

 

제가 방학동안 정말 부족한 드론 코딩에 대한 정보로 인해 고통받으며 여기저기 인터넷을 뒤져가며 혼자 코딩했던 것을 정리해보겠습니다

 

이 포스팅에서 "내가 원하는대로 드론 조종하고 이미지 저장하기"를 뽀개봅시다!!

<텔로 패키지로 텔로 드론을 조종하고, 포도밭에서의 자율주행을 위한 Coverage Path Planning 알고리즘을 따로 구현해 드론 이동 경로를 정해주고, 그리고 드론이 비행하면서 이미지를 서버에 올리는 것> 까지 설명해보겠습니다.

---

Tello Package

pycharm으로 코딩하시면 파이썬 패키지에 tello가 있어서 바로 다운로드해서 사용하시면 됩니다.

간단히 텔로 패키지에 대해 설명하자면, 교육용 코딩 드론인 tello, 상위 버전인 robomaster사에서 만든 robomaster tello talent 등 텔로 계열 드론들을 조종할 수 있는 패키지입니다.

어디에도 명확하게 나와있지 않아서 제가 힘들었던 부분이, 저는 robomaster tello talent를 사용했는데 이 드론은 robomaster SDK(이 회사의 드론, RC카 등등을 조종할 수 있는 SDK)를 사용해야한다고만 나와있고 tello package도 되는건지 언급이 없었습니다. 근데 조종이 됩니다..! 설치나 간단한 조종은 tello 패키지가 쉬우니 이걸 사용하시는 게 더 편할 것 같습니다.

 

패키지 임포트

import djitellopy import tello

텔로 조종

# 텔로 연결
tello.connect()

# 텔로 띄우기 (이륙)
tello.takeoff()

# 텔로 이동
tello.sand_rc_control(0, 0, 100, 0)

간단하죠? 아, 텔로 연결은 와이파이로 하게 됩니다. 

그리고 텔로 이동에서

sand_rc_control(x, y, z, angle)

이 함수의 parameter는 차례대로 x 방향으로 몇 움직일 것인지, y 방향으로 몇 움직일 것인지, z(높이)는 몇으로 할건지, 각도를 얼만큼 틀 것인지 입니다.

그런데 텔로가 각도를 트는데 시간이 걸려서 방향전환으로 움직이는 것을 최소화하는 것이 좋을 것 같습니다. (저는 작물을 찍어야해서 어쩔 수 없이 끝 지점에서 한번씩 방향전환을 했지만요..)

 

이동경로계획 알고리즘

이제 텔로 연결하고 기본이 되는 함수를 배웠으니 자율주행을 코딩해봅시다

제가 프로젝트를 하면서 드론 자율비행을 한 방식은

드론의 trajectory(경로)를 생성하고  sand_rc_control(x, y, z, angle) 함수에 x 이동할 값, y 이동할 값을 줘 가면서 드론을 움직이는 방식입니다.

프로젝트가 밭을 일일이 스캔해야하는 것이어서, 저는 지그재그로 움직이게 코딩했습니다 

아래는 그 알고리즘대로 움직이는 경로를 확인할 수 있게 MATLAB으로 코딩한 것입니다. (하나씩 경로 확인하면 느려서 프레임 중간중간만 이미지로 저장해서 만들었습니다,, 그래서 작물 끝까지 가는게 보이지는 않지만 제대로 경로는 저장됩니다..!)

플로우 차트로 그린 알고리즘은 다음과 같습니다

작물의 양 옆 면을 모두 스캔하며 지나가게 했습니다. 그리고 방향을 틀 때는 작물을 계속 촬영하면서 지나가게 해야해서 드론 방향을 90도씩 틀도록 했습니다. 

 

드론 경로 코딩은 2차원 배열에 한 row 씩 경로를 저장하도록 파이썬 코딩했습니다. 

자유롭게.. 그냥 sand_rc_control(x, y, z, angle) 이 함수에 들어갈 x, y, angle 값을 따로 저장하는 아주 쉬운 코테 코딩을 한다고 생각하시면,, 됩니다..

def cpp_algo_turn(width, grape_height, height, interval, sight_range, init_point, speed):
  # widht: 포도밭 넓이 / grape_height: 포도 작물 폭 / height: 포도밭 전체 높이 / interval: 밭 사이 간격
  # sight_range: 비행시 띄워야할 드론과 포도 사이 간격 / init_point: 시작 위치 / speed: 드론 속도
  print("Do Coverage Path Planning algorithm")

  # parameters
  width, grape_height, height, interval = width, grape_height, height, interval
  sight_range = sight_range
  speed = speed
  init_x = init_point[0]
  init_y = init_point[1] - sight_range

  # traj[0] = move only right side
  # trak[2] = turen left or right
  # traj[3] = x coordinate
  # traj[4] = y coordinate

  idx = 0
  traj = np.zeros((1, 5))
  traj[idx][3] = init_x
  traj[idx][4] = init_y

  count_y = 0
  max_y_range = 2 * sight_range + grape_height
  interval_y_range = interval - 2 * sight_range
  flag = -1

  while 1:
    if ((traj[idx][4] >= (height + 2 * sight_range + init_y)) & (traj[idx][3] >= init_x)):
      break

    if (traj[idx][3] == init_x):
      if (count_y == 0):
        traj = np.append(traj, np.array([[speed, 0, 0, traj[idx][3] + speed, traj[idx][4]]]), axis=0)
        # traj[idx+1][3] = traj[idx][3] + speed ##
        # traj[idx+1][4] = traj[idx][4] ##
        # traj[idx + 1][0] = speed
        # traj[idx + 1][1] = 0
        # traj[idx + 1][2] = 0
      elif (count_y > 0):
        traj = np.append(traj, np.array([[speed, 0, 0, traj[idx][3], traj[idx][4] + speed]]), axis=0)
        # traj[idx+1][3] = traj[idx][3] ##
        # traj[idx+1][4] = traj[idx][4] + speed ##
        # traj[idx+1][0] = speed
        # traj[idx+1][1] = 0
        # traj[idx+1][2] = 0
        count_y = count_y + speed
        if flag == 1:
          traj[idx + 1][2] = 90
          flag = -1
        if (count_y >= max_y_range + interval_y_range):
          count_y = 0
          traj[idx + 1][2] = 90

    elif (traj[idx][3] == init_x + width + sight_range):
      if ((count_y >= 0) & (count_y < max_y_range)):
        traj = np.append(traj, np.array([[speed, 0, 0, traj[idx][3], traj[idx][4] + speed]]), axis=0)
        # traj[idx+1][3] = traj[idx][3] ##
        # traj[idx+1][4] = traj[idx][4] + speed ##
        # traj[idx + 1][0] = speed
        # traj[idx + 1][1] = 0
        # traj[idx + 1][2] = 0
        count_y = count_y + speed
        if (count_y == 0):
          traj[idx + 1][2] = -90

      elif (count_y >= max_y_range):
        flag = 1
        traj = np.append(traj, np.array([[speed, 0, -90, traj[idx][3] - speed, traj[idx][4]]]), axis=0)
        # traj[idx+1][3] = traj[idx][3] - speed ##
        # traj[idx+1][4] = traj[idx][4] ##
        # traj[idx+1][0] = speed
        # traj[idx+1][1] = 0
        # traj[idx+1][2] = -90

    else:
      if (count_y == 0):
        traj = np.append(traj, np.array([[speed, 0, 0, traj[idx][3] + speed, traj[idx][4]]]), axis=0)
        # traj[idx+1][3] = traj[idx][3] + speed ##
        # traj[idx+1][4] = traj[idx][4] ##
        # traj[idx+1][0] = speed
        # traj[idx+1][1] = 0
        # traj[idx+1][2] = 0
      elif (count_y >= max_y_range):
        traj = np.append(traj, np.array([[speed, 0, 0, traj[idx][3] - speed, traj[idx][4]]]), axis=0)
        # traj[idx+1][3] = traj[idx][3] - speed ##
        # traj[idx+1][4] = traj[idx][4] ##
        # traj[idx+1][0] = speed
        # traj[idx+1][1] = 0
        # traj[idx+1][2] = 0

    idx = idx + 1
    print('[' + str(idx) + ']: x:' + str(traj[idx][3]) + ' y:' + str(traj[idx][4]))

  return traj

 

그리고 배열 traj에 저장한 경로를 하나씩 불러서 sand_rc_control(x, y, z, angle) 이 함수에 차례로 넣으시면 됩니다

저는 드론 베터리도 고려해서 베터리가 일정 범위 이하로 떨어지면 초기 지점에서 충전하고 돌아와서 다시 비행하도록 하는 코드까지 추가해서 코딩했습니다 (부끄러우니 참고,, 만.. 해주세요)

def fly_drone(traj_list, charge_x, charge_y, index, drone_id):
  # charge_x, charge_y: 충전기 x좌표, y좌표 
  # index: global 변수로 0으로 초기화해놨음. 경로 traj에 필요한 index
  # drone_id: 비행할 드론 번호 
  # global index ##
  traj = traj_list # 이동경로 
  
  # go to last location
  # basically, last location is initial point
  # index = last_traj_idx

  if index != 0:
    dist_x = traj[index][3] - charge_x
    dist_y = traj[index][4] - charge_y
    tello.send_rc_control(dist_x, dist_y, 0, 0)

  last_req = 0
  for i in range(index, len(traj)):
    print("fly idx:", i)

    if (tello.get_battery() < 10):
      # 마지막 위치 저장 후
      index = i
      # index = last_traj_idx
      # 충전하러 감
      dist_x = charge_x - traj[i][3]
      dist_y = charge_y - traj[i][4]
      tello.send_rc_control(dist_x, dist_y, 0, 0)
      flag = -1
      return flag  # or break

    else:
      tello.send_rc_control(traj[i][0], traj[i][1], 0, traj[i][2])
      request_func.load_img()

    now = datetime.datetime.now()
    if (now - last_req) > 1000:
      is_fly, start_time, end_time = request_func.req_droneControl(drone_id)
      last_req = datetime.datetime.now()
      if (last_req > end_time):
        break


  # 충전하러 감
  dist_x = charge_x - traj[i][3]
  dist_y = charge_y - traj[i][4]
  # tello.send_rc_control(dist_x, dist_y, 0, 0)
  flag = 0
  index = 0
  return flag

여기서 req_droneControl은 제가 다른데에 선언한 함수인데, 서버에 저장되어 있는 드론 정보들을 받아오도록 한 것입니다

두 번째 첨부한 코드는 그냥 경로를 차례로 넣는걸 저는 이런식으로 코딩했다~~의 개념으로 보여드리고자 첨부했습니다. 

 

드론에서 촬영한 이미지 전송

저희는 웹서버를 사용해서, 웹서버랑 connection 맺고 이미지 전송하는 방법을 다뤄보겠습니다.

 

먼저, 로컬에 이미지를 띄우는 것 먼저 살펴보겠습니다. 

이미지를 확인할 때는 cv가 필요하기 때문에 cv2를 임포트 해줍시다

import cv2

tello.streamon()
while True:
    img = tello.get_frame_read().frame
    img = cv2.resize(img, (360, 240))
    cv2.imshow("Image", img)
    cv2.waitKey(1)

streamon() 함수는 텔로에서 찍는 영상을 받아오게 해줍니다.

그리고 get_frame_read()으로 프레임 단위로 while문으로 반복해서 이미지로 저장합니다.

그리고 사진 크기 조절하고(resize), imshow로 화면을 띄웁니다. waitKey는 키 입력하면 화면이 닫히게 하는 함수로, 이 코드가 있어야 화면이 그대로 떠 있습니다.

 

이제, 웹서버와 통신해서 이미지를 보내볼까요?

위에 코드에 추가해서 넣어줄 부분(이미지 전송)만 보겠습니다. 

통신을 위해 requests를 임포트 해줍니다

import requests

save_num = 0
url = "http://xxxxxx"
tello.streamon()
files = []

while True:
    img = tello.get_frame_read().frame
    img = cv2.resize(img, (360, 240))
    str = 'img' + str(save_num) + '.png'
    save_num = save_num + 1
    cv2.imshow(str, img)
    
    files.append(('file', (str, img, 'image/png')))
    
payload = {}
headers = {}
response = requests.request("POST", url, headers=headers, data=payload, files=files)

이미지를 files에 받아서, (저장되는 이미지 이름을 바꾸기 위해 str로 지정해 주었습니다) 파일 자체를 보내주는 것입니다

payload, header도 선언하고, files에 이미지 파일들을 실어서 request()에 "POST"로 보내주면 됩니다.

 

 

여기까지 "드론 조종하고 이미지 전송하기" 입니다!

 

 

이렇게 해서 학교에서 드론을 날려보았습니다

잘 날아가는거 보니까 신기하고 뿌듯하네요-!