Python으로 조작하는 드론 CoDroneII - 센서 데이터 읽기 편~

얼마전에 로보링크의 드론 제품인 코드론II를 소개했던 적이 있습니다. 이 제품은 파이썬으로 접근할 수 있도록 되어 있습니다. 그래서 이번에는 드론의 데이터를 받아오는 부분을 한 번 파이썬으로 연습해보려고 합니다~~^^

로보링크의 코드론 판매페이지입니다. 이 페이지 하단에 보면

이렇게 파이썬 라이브러리와 사용 설명서로 갈 수 있는 링크가 있습니다.

여기는 코드론용 파이썬 모듈인 e-drone의 소개 페이지로 설치는 음.. 그냥 pip로 하면 됩니다.^^

저는 conda에서 codrone이라는 환경을 만들어서 거기다 설치했습니다.

codrone에 이미 만들어서 설치한 모듈은

정말 몇개 없죠. 대부분은 알아서 다 설치가 됩니다.^^

그리고, 파이썬용 라이브러리 설명 페이지에 가보면, 안내되는 라이브러리 e 매뉴얼 페이지에 이미 더 이상 설명할 필요가 없도록 많은 코드가 예제로 제공되고 있습니다^^

해당 코드를 테스트해보기 위해, 저렇게 연결합니다. 조종기를 PC에 연결해서 시리얼 통신으로 데이터를 주고 받는 거죠.

이번에는 VSCode로 코드를 실행해 보려구요~

먼저 Select Interpreter를 선택해서,

아까 만들어둔 codrone이라는 conda 환경을 선택해 둡니다. 이러면 VSCode가 다양한 conda 환경에서도 길을 잃지 않고 잘 환경을 잡아 줍니다.^^

매뉴얼에 있는 고도, 온도, 압력 등을 확인하는 예제입니다.

실행하면 저렇게 나오네요^^

ㅎㅎㅎ. 실험 들어갑니다^^

ㅎㅎ 괜찮네요~~ 뭐 저정도 오차는 애교죠.. 들고 있는 제 손도 정확하지 않을 거구요^^ 이제 얼마전에 이야기한 센서 데이터를 받아서 실시간 그래프 그리던 코드를 조금 응용해 보겠습니다.

from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
from e_drone.drone import *
from e_drone.protocol import *

roll = 0.
pitch = 0.
yaw = 0.
height = 0.

def eventAttitude(attitude):
    global roll, pitch, yaw
    print("eventAttitude() / {0:0.1f}, {1:0.1f}, {2:0.1f}".format(attitude.roll, attitude.pitch, attitude.yaw))
    roll = attitude.roll
    pitch = attitude.pitch
    yaw = attitude.yaw

def eventAltitude(altitude):
    global height
    print("Height : ", str(altitude.rangeHeight))
    height = altitude.rangeHeight*100

def init():
    return (line1, line2, line3, line4)

def animate(i):
    drone.sendRequest(DeviceType.Drone, DataType.Attitude)
    drone.sendRequest(DeviceType.Drone, DataType.Altitude)

    old_roll = line1.get_ydata()
    new_roll = np.r_[old_roll[1:], roll]
    line1.set_ydata(new_roll)

    old_pitch = line2.get_ydata()
    new_pitch = np.r_[old_pitch[1:], pitch]
    line2.set_ydata(new_pitch)

    old_yaw = line3.get_ydata()
    new_yaw = np.r_[old_yaw[1:], yaw]
    line3.set_ydata(new_yaw)

    old_height = line4.get_ydata()
    new_height = np.r_[old_height[1:], height]
    line4.set_ydata(new_height)

    return (line1, line2, line3, line4)

drone = Drone()
drone.open()

fig = plt.figure(figsize=(12,6))
ax = plt.axes(xlim=(0, 100), ylim=(-90, 90))
plt.legend()
line1, = ax.plot([], [], lw=2)
line2, = ax.plot([], [], lw=2)
line3, = ax.plot([], [], lw=2)
line4, = ax.plot([], [], lw=2)

max_points = 100
line1, = ax.plot(np.arange(max_points), np.ones(max_points, dtype=np.float)*np.nan, lw=2, label='roll')
line2, = ax.plot(np.arange(max_points), np.ones(max_points, dtype=np.float)*np.nan, lw=2, label='pitch')
line3, = ax.plot(np.arange(max_points), np.ones(max_points, dtype=np.float)*np.nan, lw=2, label='yaw')
line4, = ax.plot(np.arange(max_points), np.ones(max_points, dtype=np.float)*np.nan, lw=2, label='height')

drone.setEventHandler(DataType.Attitude, eventAttitude)
drone.setEventHandler(DataType.Altitude, eventAltitude)

anim = animation.FuncAnimation(
                                fig, 
                                animate, 
                                init_func=init, 
                                frames=200, 
                                interval=100, 
                                blit=False
                            )

plt.show()
drone.close()
print("All done")

코드가 조금 길게 느껴지거나 난잡(ㅠㅠ)하다고 느끼실 수 있는데, 넵.. 그렇습니다.ㅠㅠ. 클래스로 만들어서 쓰면 정말 깔끔할텐데, 빨리 블로그 글을 올리고 싶은 마음(이라고 변명하면서..ㅠㅠ)으로 이렇게 되어 버렸네요ㅠㅠ. 뭐 아무튼 저 아이를 실행하면 아래 동영상과 같아용~~~^^

다음에는 Python 코드로 드론을 날려보려고 합니다~~ 그것도 무쟈게 쉬워요 ㅎㅎㅎ