ドローンを動かす(SITL編)

#はじめに
このページは，

[ドローン操作システムを作ろう](https://qiita.com/hsgucci/items/86eedb5555b4234ee0e7)

の１ページです．
全体を見たい場合は上記ページへお戻りください．

#概要

ここでは，キーボード入力でdronekit-sitlのドローンを操作してみます．

#準備するもの

・Ubuntu Linuxの入ったIntel系ＰＣ
　（dronekit-sitlの記事で使ったもの）

#ベースとなるプログラム
これまでの記事を応用し，

・コマンドラインでホームポイント(緯度/経度/高度)を指定してSITL起動
・キー入力でフライトモード変更

ができるベースプログラムを用意しました．
※前回の記事「[dronekit-python を使ってみる 実機編(キーボード入力を取る方法)](https://qiita.com/hsgucci/items/e72ef6c172b9d2e62d72#%E3%82%AD%E3%83%BC%E3%83%9C%E3%83%BC%E3%83%89%E5%85%A5%E5%8A%9B%E3%82%92%E5%8F%96%E3%82%8B%E6%96%B9%E6%B3%95)」の中でも紹介した [kbhit.py](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter03/kbhit.py) を同じフォルダに置いておいてください．

以下をコピーするか，
あるいは　[ここ](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter04/skelton_sitl.py)　を右クリックして[名前を付けて保存]してください．
ファイル名は`skelton_sitl.py`としています．

```py:skelton_sitl.py
#!usr/bin/env python
# -*- coding: utf-8 -*-
print( "dronekitスタート" )    # 開始メッセージ

# 必要なライブラリをインポート
from kbhit import *                 # kbhitを使うために必要(同じフォルダにkbhit.pyを置くこと)
from subprocess import Popen        # subprocessの中から、Popenをインポート
from signal import signal, SIGINT   # Ctrl+C(SIGINT)の送出のために必要 
from dronekit import connect        # connectを使いたいのでインポート
from dronekit import VehicleMode    # VehicleModeも使いたいのでインポート
import time                         # ウェイト関数time.sleepを使うために必要

# kbhit()を使うための「おまじない」を最初に２つ書く
atexit.register(set_normal_term)
set_curses_term()

# dronekit SITL の起動情報
# example: 'dronekit-sitl copter --home=35.079624,136.905453,50.0,3.0 --instance 0'
sitl_frame          = 'copter'          # rover, plane, copterなどのビークルタイプ
sitl_home_latitude  = '35.894087'       # 緯度(度)  柏の葉キャンパス駅前ロータリー
sitl_home_longitude = '139.952447'      # 経度(度)
sitl_home_altitude  = '17.0'            # 高度(m)
sitl_home_direction = '0.0'             # 機首方位(度)
sitl_instance_num   = 0                 # 0〜

# コマンドライン入力したい文字列をリスト形式で作成
sitl_boot_list = ['dronekit-sitl',sitl_frame,
                  '--home=%s,%s,%s,%s' % (sitl_home_latitude,sitl_home_longitude,sitl_home_altitude,sitl_home_direction),
                  '--instance=%s'%(sitl_instance_num)]

print '# sitl command: ', sitl_boot_list        # 文字列を表示
p = Popen(sitl_boot_list)   # サブプロセスの起動
time.sleep(1)   # 起動完了のために1秒待つ

#connection_stringの生成
connection_string = 'tcp:localhost:' + str(5760 + int(sitl_instance_num) * 10 ) # インスタンスが増えるとポート番号が10増える

# フライトコントローラ(FC)へ接続
print( "FCへ接続: %s" % (connection_string) )    # 接続設定文字列を表示
vehicle = connect(connection_string, wait_ready=True)    # 接続

#Ctrl+cが押されるまでループ
try:
    while True:
        if kbhit():     # 何かキーが押されるのを待つ
            key = getch()   # 1文字取得

            # keyの中身に応じて分岐
            if key=='g':                # guided
                vehicle.mode = VehicleMode( 'GUIDED' )
            elif key=='l':              # land
                vehicle.mode = VehicleMode( 'LAND' )


        # ここはif文と同じインデントなので，キーに関係なく1秒に1回実行される
        # 現在の状態を表示
        print("--------------------------" )
        print(" System status: %s" % vehicle.system_status.state)
        print(" Is Armable?: %s" % vehicle.is_armable)
        print(" Armed: %s" % vehicle.armed) 
        print(" Mode: %s" % vehicle.mode.name )
        print(" Global Location: %s" % vehicle.location.global_frame)
        time.sleep(1)

except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
    print( "SIGINTを検知" )

# フライトコントローラとの接続を閉じる
vehicle.close()

 # サブプロセスにもSIGINT送信
p.send_signal(SIGINT)
p.communicate()
time.sleep(1)   # 終了完了のために1秒待つ

print("終了．")    # 終了メッセージ
```
フライトモードはGUIDED(ウェイポイント移動)とLAND(着陸)があれば十分です．




実行はいつも通りです．終了はCtrl+cです．

```bash
$python skelton_sitl.py
```

**実行結果**

以下の項目を１秒に１回表示しています．
・システムステータス
・PreArmチェックの状態
・ARM状態
・フライトモード
・現在の緯度・経度・高度

```bash
--------------------------
 System status: STANDBY
 Is Armable?: True
 Armed: False
 Mode: STABILIZE
 Global Location: LocationGlobal:lat=35.894087,lon=139.952447,alt=17.03
```


#キー入力でARM/DISARM
まずは，`skelton_sitl.py`にARMとDISARMの機能を追加しましょう．
ARMなので'a'キー，DISARMなので'd'キーで良いでしょう．

キー入力のif-elseif文の下に追加します．
Pythonなのでインデントの深さを揃える必要があります．

```python:skelton_sitl.pyに`a`と`d`のキーを追加する
            # keyの中身に応じて分岐
            if key=='g':                # guided
                vehicle.mode = VehicleMode( 'GUIDED' )
            elif key=='l':              # land
                vehicle.mode = VehicleMode( 'LAND' )
            elif key=='a':              # arm
                vehicle.armed = True
            elif key=='d':              # disarm
                vehicle.armed = False
```

実行して試してみましょう．

キーを押して試してみると分かりますが，
`Is Armable?`がTrueにならないとARMできません．
これは実機と同様です．PreArmのチェックが全て終わって準備OKになる必要があります．

'a'キーを押してから実際にARMが完了するまでは数秒のタイムラグがあります．
ARM時にも裏で様々な処理が行われる（ホーム地点の設定や，ロギングの開始）ので，しばらく時間がかかるのです．

また，**ARMした後10秒以内に離陸しないと，勝手にDISARMしてモータが止まります**．
これも実機と同じ仕様です．

-----
dronekit-pythonのサンプルの１つである**simple_goto.py**では，
このように書いてあります．

```python:simple_goto.pyの一部
    print("Basic pre-arm checks")
    # Don't try to arm until autopilot is ready
    while not vehicle.is_armable:     # Armableになるまで待ち続ける
        print(" Waiting for vehicle to initialise...")
        time.sleep(1)

    print("Arming motors")
    # Copter should arm in GUIDED mode
    vehicle.mode = VehicleMode("GUIDED")
    vehicle.armed = True

    # Confirm vehicle armed before attempting to take off
    while not vehicle.armed:     # ARM完了するまで待ち続ける
        print(" Waiting for arming...")
        time.sleep(1)
```
while文を使って，
・PreArmチェックが完了するまで１秒待ちのループ
・ARMが完了するまで1秒待ちのループ
待っていることがわかります．

本来はこのように書いたほうが良いでしょう．

---
実は，vehicleクラスには，`vehicle.arm()`や`vehicle.disarm()`といったメンバ関数も用意されています．
しかし，これらはブロッキング関数なので，１秒毎の表示処理が止まってしまいます．
**<font color="red">飛行中にはDISARMできないので，vehicle.disarm()を間違って飛行中に呼ぶと，永久に操作が効かなくなります（泣</font>**

したがって，`vehicle.armed = True` `vehicle.armed = False`と書くほうが安全です．

#キー入力で離陸/着陸
次は離陸と着陸を`skelton_sitl.py`に追加します．
離陸はTakeoffなので't'キーで良いでしょう．


```python:skelton_sitl.pyの変更
            # keyの中身に応じて分岐
            if key=='g':                # guided
                vehicle.mode = VehicleMode( 'GUIDED' )
            elif key=='l':              # land
                vehicle.mode = VehicleMode( 'LAND' )
            elif key=='a':              # arm
                vehicle.armed = True
            elif key=='d':              # disarm
                vehicle.armed = False
            elif key=='t':              # takeoff
                vehicle.simple_takeoff(alt=10)
```
離陸は`vehicle`クラスのメンバ関数である`simple_takeoff`を使いました．
離陸高度は10mにしています．
着陸用の関数は用意されていないので，'LAND'にすることで着陸させます．

実行して試してみましょう．
離陸後，`alt`の値が離陸地点の海抜17mから，+10mの27mまで増加することを確認します．
LANDすると17mまで降りてきます．

以下のような特徴を試しておきましょう．

・フライトモードがSTABILIZEのままだと，離陸できない
　　->モードをGUIDEDに変えてから('g'キー)，離陸させましょう．

・飛行中(ステータスACTIVE)の時に'd'キーを押しても，DISARMしない．
　　->いきなりモータが止まったら墜落するので，セーフティが働いている．

・フライトモードLANDで着陸すると，勝手にDISARMされモータが止まる．
　　->ARMのまま待ち続けることはない．

・LANDで着陸後，再離陸するにはGUIDEDに戻す必要がある．

#キー入力で移動

位置の移動には，`LocationGlobal`，`LocationGlobalRelative`というクラスを使いたいので，`skelton_sitl.py`の冒頭部分，色々なimport文が固まっている箇所に，新たにインポートを追加します．

```python:skelton_sitl.pyにインポート文を追加
from dronekit import LocationGlobal, LocationGlobalRelative
```

移動は'1'，'2'の数字キーを，ゴーホームするために'r'キーを使いましょう．

```python:`1`と`2`キーで目的地へ移動，`r`キーでゴーホーム
            elif key=='1':              # simple_goto
                # 柏の葉キャンパス交番上空30mへ
                point = LocationGlobalRelative( 35.893246, 139.954909 , 30 )
                vehicle.simple_goto(point)
            elif key=='2':              # simple_goto
                # 三井ガーデンホテル上空50mへ
                point = LocationGlobalRelative( 35.895236, 139.952468 , 50 )
                vehicle.simple_goto(point)
            elif key=='r':              # RTL
                vehicle.mode = VehicleMode( 'RTL' )

```
改変内容全体をまとめたプログラムを　[key_ctrl.py](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter04/key_ctrl_sitl.py)　として置いておきます．

**解説**
`LocationGlobal`と`LocationGlobalRelative`の２種類のクラスをインポートしました．
両者の違いは**絶対高度を使うか，相対高度を使うか**だけの違いです．
高度の絶対/相対なので注意してください．**緯度経度は常に絶対値**です．

離陸地点が海抜ゼロメートルならばどちらを使っても同じになりますが，
今回は海抜17メートルを離陸地点，と設定しました．
`LocationGlobalRelative`で30mや50mの高度を指定していますから，
実際は海抜47mと67mに移動することになります．



実行して試してみましょう．


現実世界と同じシミュレーションなので，目標地点に到達するまでは結構時間がかかります．
RTLすると，離陸地点まで戻って勝手に着陸します．

#おわりに

今回は，キーボード入力でsitlのシミュレーションドローンを動かしてみました．
これができれば，実機ドローンでも同じことが試せます．

次回は，プログラムからちょっと離れて，実機ドローンのシステム構成について書きたいと思います．