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

[LIFULL Advent Calendar 2019](https://qiita.com/advent-calendar/2019/lifull) 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未満を大幅に下回る軽量ボディとなっています。

# インストール
```bash
go get -d -u gobot.io/x/gobot/...
```
goの環境が入っている方はgo getを行うだけです。

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

# とりあえず飛ばしてみる
```go: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()
}
```
実行

```bash
go run main.go
```

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

<blockquote class="twitter-tweet"><p lang="en" dir="ltr">tello takeoff <a href="https://t.co/vnKgA4U2Pl">pic.twitter.com/vnKgA4U2Pl</a></p>&mdash; 56🐶 (@tkbrhcp) <a href="https://twitter.com/tkbrhcp/status/1205161484252114944?ref_src=twsrc%5Etfw">December 12, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
着陸位置が安定しないのはご愛嬌で、、、

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

例えば、バックフリップ


```go 

func (d *Driver) BackFlip() (err error)
```
<blockquote class="twitter-tweet"><p lang="en" dir="ltr">Tello backflip <a href="https://t.co/5V13VBymPW">pic.twitter.com/5V13VBymPW</a></p>&mdash; 56🐶 (@tkbrhcp) <a href="https://twitter.com/tkbrhcp/status/1205172572943351810?ref_src=twsrc%5Etfw">December 12, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

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

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

# カメラで撮影してみる

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

## 必要なインストール

```bash
#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を利用します。

```go
//ビデオを起動
func (d *Driver) StartVideo() (err error)
//ビデオのエンコーダーのレートを設定
func (d *Driver) SetVideoEncoderRate(rate VideoBitRate) (err error)
//カメラのexposure levelを設定
func (d *Driver) SetExposure(level int) (err error)
```
こちらが実際のコードになります。

```go: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の画面上にはドローンで撮影中の映像がこのように表示されました。（ただの離着陸を撮影しました。）
<blockquote class="twitter-tweet"><p lang="en" dir="ltr">tello live <a href="https://t.co/73Xi9YIrnc">pic.twitter.com/73Xi9YIrnc</a></p>&mdash; 56🐶 (@tkbrhcp) <a href="https://twitter.com/tkbrhcp/status/1205175862724685824?ref_src=twsrc%5Etfw">December 12, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

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

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