Go言語のフレームワークGobotでドローンを制御してみた。

LIFULL Advent Calendar 2019 13日目の記事になります。

仕事では主にSalesforceやRPAなどのアプリケーション開発に関わっている新卒1年目Webエンジニアです。
9月にメルペイ主催のGopher道場に参加したことをきっかけに、Goの技術中心に最近はキャッチアップしています。

そんな中、GoはIoTにも活用できると聞き、試してみたいと思っていたのでドローンtelloを購入しました。

今回はそんなドローンをGoのフレームワークGoBotで色々動かしてみた話をします。

Gobotとは

Gobotは以下の３５のプラットフォームに対応したGo言語のロボティクス・IoTフレームワークになります。

ARDrone/Arduino/Beaglebone/Black Bebop/Bluetooth LE/C.H.I.P./C.H.I.P. Pro /Digispark/DJI Tello/Dragonboard/ESP8266/GoPiGo3/HS-200/Intel Curie/Intel Edison/ Intel Joule/Joystick/Keyboard/Leap Motion/Mavlink/Microbit/Minidrone/MQTT/NATS/ Neurosky/OpenCV/Pebble/PocketBeagle/Raspberry Pi/Particle/Ollie/BB-8/Sphero/SPRK+ /Tinker Board

詳しくはこちらを参照
https://gobot.io/

今回自分はもともとドローンに興味があったこともあり、この中でもDJI Telloを選び実際に動かしてみました。

DJI Telloとは

Telloとは、中国のスタートアップ企業Ryze Techが開発・販売しているトイドローンです。ドローンで世界シェアNo.1のDJIとIntelが技術協力をしています。
サイズ・重量・価格
Telloのサイズは98mm×92.5mm×41mm、重量は80g。手のひらほどのコンパクトサイズで、航空法の範囲内である200g未満を大幅に下回る軽量ボディとなっています。

インストール

go get -d -u gobot.io/x/gobot/...

goの環境が入っている方はgo getを行うだけです。

参考：https://gobot.io/documentation/getting-started/

とりあえず飛ばしてみる

main.go

package main

import (
  "time"

  "gobot.io/x/gobot"
  "gobot.io/x/gobot/drivers/gpio"
  "gobot.io/x/gobot/platforms/firmata"
)

func main() {
  firmataAdaptor := firmata.NewAdaptor("/dev/ttyACM0")
  led := gpio.NewLedDriver(firmataAdaptor, "13")

  work := func() {
    gobot.Every(1*time.Second, func() {
      led.Toggle()
    })
  }

  robot := gobot.NewRobot("bot",
    []gobot.Connection{firmataAdaptor},
    []gobot.Device{led},
    work,
  )

  robot.Start()
}

実行

go run main.go

こんな感じで飛びます。（※音量注意）

tello takeoff pic.twitter.com/vnKgA4U2Pl

— 56🐶 (@tkbrhcp) December 12, 2019

着陸位置が安定しないのはご愛嬌で、、、

色々と動かしてみる

このフレームワークだけで色んな操作ができるようになっています。

例えば、バックフリップ

func (d *Driver) BackFlip() (err error)

Tello backflip pic.twitter.com/5V13VBymPW

— 56🐶 (@tkbrhcp) December 12, 2019

抜粋になりますが、下記のような操作がGo上でできるようになっています。

//時計回り回転
func (d *Driver) Clockwise(val int) error
//反時計回り回転
func (d *Driver) CounterClockwise(val int) error
//フロントフリップ
func (d *Driver) FrontFlip() (err error)
//ホバリング
func (d *Driver) Hover()
//手のひら着陸
func (d *Driver) PalmLand() (err error)

カメラで撮影してみる

ここでカメラ機能を使ってみます。

必要なインストール

#FFMPEGのインストール
brew install ffmpeg $(brew options ffmpeg | grep -v -e '\s' | grep -e '--with-\|--HEAD' | tr '\n' ' ')
export LDFLAGS="-L/usr/local/opt/libffi/lib"
export PKG_CONFIG_PATH="/usr/local/opt/libffi/lib/pkgconfig"

#opencvのインストール
brew install opencv
export LDFLAGS="-L/usr/local/opt/openblas/lib"
export CPPFLAGS="-I/usr/local/opt/openblas/include"
export PKG_CONFIG_PATH="/usr/local/opt/openblas/lib/pkgconfig"

それぞれFFMPEGはドローンからの動画を変換するため、OpenCvはドローンからのリアルタイムデータをPC上に表示するためのライブラリになっています。（結構インストールに時間かかります。）

gobotでtelloの動画撮影をするためには以下のfunctionを利用します。

//ビデオを起動
func (d *Driver) StartVideo() (err error)
//ビデオのエンコーダーのレートを設定
func (d *Driver) SetVideoEncoderRate(rate VideoBitRate) (err error)
//カメラのexposure levelを設定
func (d *Driver) SetExposure(level int) (err error)

こちらが実際のコードになります。

main.go

package main

import (
    "fmt"
    "io"
    "os/exec"
    "strconv"
    "time"

    "gobot.io/x/gobot"
    "gobot.io/x/gobot/platforms/dji/tello"
    "gocv.io/x/gocv"
)

const (
//デスクトップ上に表示するサイズ
    frameX    = 1024
    frameY    = 720
    frameSize = frameX * frameY * 3
)

func main() {
    drone := tello.NewDriver("8888")

    window := gocv.NewWindow("Tello")

    ffmpeg := exec.Command("ffmpeg", "-hwaccel", "auto", "-hwaccel_device", "opencl", "-i", "pipe:0",
        "-pix_fmt", "bgr24", "-s", strconv.Itoa(frameX)+"x"+strconv.Itoa(frameY), "-f", "rawvideo", "pipe:1")
    ffmpegIn, _ := ffmpeg.StdinPipe()
    ffmpegOut, _ := ffmpeg.StdoutPipe()

    work := func() {
        if err := ffmpeg.Start(); err != nil {
            fmt.Println(err)
            return
        }
        // telloに接続を確認して、Macのビデオを起動
        drone.On(tello.ConnectedEvent, func(data interface{}) {
            fmt.Println("Telloへ接続")
            drone.StartVideo()
            drone.SetVideoEncoderRate(tello.VideoBitRateAuto)
            drone.SetExposure(0)

            gobot.Every(100*time.Millisecond, func() {
                drone.StartVideo()
            })
        })

        //FFMPEG functionとvideoデータをつなぐ
        drone.On(tello.VideoFrameEvent, func(data interface{}) {
            pkt := data.([]byte)
            if _, err := ffmpegIn.Write(pkt); err != nil {
                fmt.Println(err)
            }
        })

        gobot.After(5*time.Second, func() {
            drone.TakeOff()
        })

        gobot.After(2*time.Second, func() {
            drone.CounterClockwise(30)
        })

        gobot.After(2*time.Second, func() {
            drone.Down(30)
        })
        gobot.After(2*time.Second, func() {
            drone.Backward(30)
        })

        gobot.After(2*time.Second, func() {
            drone.FrontFlip()
        })

        gobot.After(15*time.Second, func() {
            drone.Land()
            fmt.Println("着陸")
        })
    }

    robot := gobot.NewRobot("tello",
        []gobot.Connection{},
        []gobot.Device{drone},
        work,
    )

    robot.Start(false)

    // ffmpegの出力をMac向けに変換
    for {
        buf := make([]byte, frameSize)
        if _, err := io.ReadFull(ffmpegOut, buf); err != nil {
            fmt.Println(err)
            continue
        }
        img, _ := gocv.NewMatFromBytes(frameY, frameX, gocv.MatTypeCV8UC3, buf)
        if img.Empty() {
            continue
        }

        window.IMShow(img)
        if window.WaitKey(1) >= 0 {
            break
        }
    }
}

実際にMacの画面上にはドローンで撮影中の映像がこのように表示されました。（ただの離着陸を撮影しました。）

tello live pic.twitter.com/73Xi9YIrnc

— 56🐶 (@tkbrhcp) December 12, 2019

以上になります。最後までお読みいただきありがとうございました。

参考
https://gobot.io/documentation/platforms/tello/
https://godoc.org/gobot.io/x/gobot/platforms/dji/tello#Driver.Clockwise