dronekit-pythonを使ってみる(SITL編)

(2024年7月27日更新)
pipでインストールするdronekit-sitlはまともに動かなくなりました(T_T

したがって、これからArdupilotのシミュレーションをしようとしている人には、以下の２つをおすすめします。

- MissionPlannerのシミュレーション機能を使う
- UbuntuLinux上で公式のArdupilot SITLの環境を構築する

前者は、後者のWindows版なのですが、少し試すだけなら十分です。
後者は、Virtual BoxやVM Ware、WSLなどの仮想PCで最初は試すとよいでしょう。

というわけで、dronekit-sitlを使うこの記事は役に立ちません。
部分的に抜粋する程度には使えるかもしれませんが。。。


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

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

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

# 概要

ドローン用フライトコントローラ『Ardupilot[^1]/PX4[^2]』をPythonプログラムで動かすには，
mavproxy.py[^3]あるいはpymavlink[^4]ライブラリを使うのがセオリーとされていますが，初心者には敷居が高いです．
そこで，3D Robotics[^5]が元気だった頃(笑)に開発したラッパーライブラリがdronekit[^6]です．
また，実際のドローンで実験できない人のために用意されているシミュレータがdronekit-sitl[^7]です．

今回は，dronekitとdronekit-sitlを使って，シミュレーションのドローンを動かしてみます．

原典はDronekit-Pythonの解説ページ
　　~~http://python.dronekit.io/~~
です．

※2019年4月15日時点では，Webサーバが落ちているようです．
　おい3DR！大丈夫かー！

※2019年5月追記：　上記サイトは閉鎖されたようです．
　ArdupilotプロジェクトとPX4プロジェクトが喧嘩別れして，
　3DRはPX4側に行ったため，Ardupilotを支援する必要がないのでしょうね．
　今後はDronecode SDK Python[^99]を使うようです．
　仕方ないので，Wayback Machineに保存されたページのリンクを貼っておきます．
　　[2018年7月のpython.dronekit.io](https://web.archive.org/web/20180705115544/http://python.dronekit.io/)

[^99]: Dronecode SDK Python https://github.com/Dronecode/DronecodeSDK-Python

# 目次

本ページは以下の内容で構成されています．

1. Linux環境にPythonやdronekit，dronekit-sitlを準備する
2. サンプルプログラムのダウンロード
3. vehicle_state.pyで動作確認
4. その他のサンプルプログラム
5. sitlを自分のプログラムで動かしてみる
6. dronekit-sitlの起動の仕方
7. コマンドライン起動のsitlにPythonプログラムを接続する
8. Pythonプログラム内からコマンドライン起動する

# 準備するもの

- Ubuntu Linuxの動くインテル系ＰＣ

　　Linux環境の構築と言われると，安価なRaspberry Piで済ませたくなってしまいますが，
　　　　~~http://python.dronekit.io/develop/sitl_setup.html~~
　　　　　[2018年7月の保存ページ](https://web.archive.org/web/20181007033316/http://python.dronekit.io/develop/sitl_setup.html)
　　によると「sitlにはx86系のみ，ARM系非サポート」と明記されています(泣)
　　というわけで，古いＰＣやラップトップをLinuxパソコンとして復活させると良いでしょう．

　　動作確認済みＯＳ：　Ubuntu-MATE 18.04.2(x64)

# Linux環境にPythonやdronekit，dronekit-sitlを準備する

まずはaptのリポジトリのリストを最新のものに更新(update)し，
更新のあるものをupgradeします．

```bash
sudo apt update
sudo apt upgrade -y
```

次に，Python開発やライブラリインストールに必要なパッケージをインストールします．
(既に最新のものが入っている場合もあります)

```bash
sudo apt install python3-dev python3-pip
```

Ardupilot/PX4フライトコントローラを操作するためのライブラリ『dronekit』と，
フライトコントローラ＆ドローン本体のシミュレータ『dronekit-sitl』を，
pipでインストールします．
(以前のpipの使い方によっては，sudoを付加しなければいけない場合もある)

```bash
pip install dronekit
pip install dronekit-sitl
```

PATHなどの環境の更新があるので，一旦システムを再起動します．

```bash
sudo reboot
```

インストールが完了していれば，次のコマンドでシミュレータが起動できます．

```bash
dronekit-sitl copter
```

シミュレータの終了はctrl+Cです．

# サンプルプログラムのダウンロード

gitがインストールされていなかったら，インストールします．

```bash
sudo apt install git
```

ホームフォルダにdronekitのgitをクローンします．

```bash
cd ~
git clone https://github.com/dronekit/dronekit-python.git
```

gitによって，ホームフォルダには"dronekit-python"というフォルダができます．(lsコマンドで確認)
このフォルダの中で今回必要なものはexamplesのフォルダだけです．
なので，examplesだけホームフォルダに移動し，不要なフォルダは削除してます．(-rfなのでフォルダ内も含めて強制削除)
※もちろん，削除せず運用してもかまいません．

```bash
$ls
$ls ~/dronekit-python/
$mv dronekit-python/examples ~/
$rm -rf dronekit-python
```

examplesフォルダの中をlsコマンドで見てみると，
~~http://python.dronekit.io/examples/index.html~~
　[2018年7月の保存ページ](https://web.archive.org/web/20181005211227/http://python.dronekit.io/examples/index.html)
このページと対応するようにフォルダがあることがわかります．
(しかし，Webページが更新されていないのでexamplesの方には4個の追加サンプルがあります)

```bash

$cd ~/examples
$ls
```

# vehicle_state.pyで動作確認
では，dronekitの動作確認をしてみましょう．
最初は「**<font color="red">vehicle_state.py</font>**」です．
vehicle_state.pyがエラーなく動けば，他のプログラムもまず問題なく動きます．

```bash
$cd ~/examples/vehicle_state
$python vehicle_state.py
```

vehicle_state.pyでは，dronkitの機体管理クラスであるvehicleが持っている様々なメンバ変数の内容が表示されます．
また，初期チェックやコールバック関数の設定，パラメータ変更などの方法も書かれています．
**<font color="red">が，vehicle_state.pyのプログラムの内容をガチで理解しようとすると大変なので，
今は「ただの動作確認」と割り切って，エラーなく動くかどうかだけを確認しましょう．</font>**
ソースは読まなくて良い，と．

特にエラーがなければ，

```python
Completed
```

と表示されてプログラムが自動終了します．

# その他のサンプルプログラム

ページ数が多くなったので，[ここ](https://qiita.com/hsgucci/items/5f16bd161d1fb9c5683f) へ移動しました．


# sitlを自分のプログラムで動かしてみる

サンプルを試すのは簡単ですが，大事なのは「自作プログラムで動かせるかどうか」ですね．
~~[Quick Start](http://python.dronekit.io/guide/quick_start.html)~~
[2018年7月に保存されたQucik Start](https://web.archive.org/web/20181004085314/http://python.dronekit.io/guide/quick_start.html)
にあるhello.pyを少し改良し，
日本語のコメントを付け，ctrl+cで終了するようにしてみました．
コピー＆ペーストするか，あるいは　[ここ](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter01/hello_jp.py)　を右クリック-[名前を付けて保存]してください．

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

# 必要なライブラリをインポート
import dronekit_sitl            # シミュレータをインポート
from dronekit import connect    # フライトコントローラやシミュレータへ接続するのがdronekit内にあるconnect
import time                     # ウェイト関数time.sleepを使うために必要

# SITLの起動
sitl = dronekit_sitl.start_default()            # sitlをデフォルト設定で起動
connection_string = sitl.connection_string()    # 起動したsitlから，接続設定用の文字列を入手する

# フライトコントローラ(FC)へ接続
# 実機ドローンだとconnection_stringは"/dev/ttyUSB0,57600"の様になる
print( "FCへ接続: %s" % (connection_string) )    # 接続設定文字列を表示
vehicle = connect(connection_string, wait_ready=True)    # 接続

#ctrl+cが押されるまでループ
try:
    while True:
        # vehicleオブジェクト内のステータスを表示
        print("--------------------------" )
        print(" GPS: %s" % vehicle.gps_0 )
        print(" Battery: %s" % vehicle.battery )
        print(" Last Heartbeat: %s" % vehicle.last_heartbeat )
        print(" Is Armable?: %s" % vehicle.is_armable )
        print(" System status: %s" % vehicle.system_status.state )
        print(" Mode: %s" % vehicle.mode.name )

        time.sleep(1)

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

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

# SITLを終了させる
sitl.stop()


print("終了．")    # 終了メッセージ
```

プログラムを実行します．
※このプログラムはctrl+cが押されるまで終了しません．


```bash
$python hello_jp.py
```


**動作説明**
sitlシミュレータを起動し，そこへdronekit(connect関数)で接続．
1秒おきにドローンの情報を表示します．

**解説**
マイコン等のプログラムと同様に，永久ループが回っています．
arduinoだと`loop()`，processingだと`draw()`に該当する部分ですね．
ループ１回ごとに`time.sleep()`でウェイトを入れてあげないと，CPU負荷が100%になるので気をつけましょう．

ctrl+cでプログラムを強制終了させると，dronekit-sitlがバックグラウンドプロセスとして残ってしまう(ゾンビ化する)可能性があるので，`try except`でctrl+cを引っ掛けて，`while`文を脱出し，終了処理である`vehicle.close`と`sitl.stop`へ移行する様にしました．

実行してみると，print文で表示しようとした情報以外の文字が出力されているのが分かります．
これはSITLが吐き出していたり，vehicleオブジェクトが勝手に書き出していたりするものです．
無害なので気にしなくて大丈夫です．

以下はそれぞれの情報の説明です．

- `vehicle.gps_0` は，GPS衛星の取得状態（位置がFixできているか，衛星を何個取れているか）を確認できます
- `vehicle.battery` は，バッテリーの電圧/電流/残量です．今回はシミュレータなので減りません．
- `vehicle.last_heartbeat` は，フライトコントローラから最後に受信したデータから何秒経過したかの時間です．
- `vehicle.is_armable` は，ARMが可能かどうかのチェックです．実機だとジャイロやコンパス，気圧センサの初期キャリブレーションが完了しないとARMできません．この値がTrueになったら，ARMして飛行して良い，という事になります．
- `vehicle.system_status.state` は，システムの状態です．BOOTやCALIBRATINGでは飛行できません．必ずSTANDBYになるのを待ちます．飛行中はACTIVEになります．
- `vehicle.mode.name` は，現在のフライトモードです．今回は特に設定していないので，STABILIZE(GPSなしの手動操縦)ですね．

#dronekit-sitlの起動の仕方

https://github.com/dronekit/dronekit-sitl
によれば，コマンドラインからdronekit-sitlを起動させることができる旨書いてあります．

Pythonプログラム中に`dronekit_sitl.start_default()`と書いた場合は，デフォルトのマルチコプターしか使うことができません．
コマンドラインからだと，機体の種類(飛行機・マルチコプター・地上ローバー)，スタート位置(緯度・経度・高度・方位)などが変更できます．


試しにやってみましょう．

```bash
$dronekit-sitl rover --home=35.360759,138.727326,3776,180
```
富士山山頂(北緯35.360759,東経138.727326)で、3776m地点から、180度(南向き)でローバーを起動させました。

このプログラムを実行してみると，以下のような待受状態で止まっています．
※プログラムの終了はctrl+cです．

```
Waiting for connection ....
```

これは，シミュレータが起動しただけの状態で、dronekit(connect)は接続していない状態です。
次の章で，dronekitを接続してみましょう．

余談ですが、dronekit以外にも、Mission Planner(windowsアプリ)やmavproxy.py(mavlink中継用のpythonプログラム)などもSITLに接続することができます。

# コマンドライン起動のsitlにPythonプログラムを接続する

まずは，ターミナル(端末)を1つ開いて，dronekit-sitlを起動させます．
今回は，柏の葉キャンパス駅前ロータリー(北緯35.894087，東経139.952447），高度17m，北向き(0度)にローバーを置きました．

```bash
$dronekit-sitl rover --home=35.894087,139.952447,17,0
```

次に，Pythonプログラムを作ります．
先程のhello_jp.pyからdronekit_sitlの起動・終了部分を削除したものになります．
コピー＆ペーストするか，あるいは　[ここ](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter01/hello_no_sitl.py)　を右クリック-[名前を付けて保存]してください．

```hello_no_sitl.py
#!usr/bin/env python
# -*- coding: utf-8 -*-
print( "SITLは前もって起動させておいてください" )    # 開始メッセージ

# 必要なライブラリをインポート
from dronekit import connect    # フライトコントローラやシミュレータへ接続するのがdronekit内にあるconnect
import time                     # ウェイト関数time.sleepを使うために必要

# 起動済みのSITLへの接続設定
connection_string = "tcp:localhost:5760"

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

#Ctrl+cが押されるまでループ
try:
    while True:
        # vehicleオブジェクト内のステータスを表示
        print("--------------------------" )
        print(" GPS: %s" % vehicle.gps_0 )
        print(" Battery: %s" % vehicle.battery )
        print(" Last Heartbeat: %s" % vehicle.last_heartbeat )
        print(" Is Armable?: %s" % vehicle.is_armable )
        print(" System status: %s" % vehicle.system_status.state )
        print(" Mode: %s" % vehicle.mode.name )

        time.sleep(1)

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

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

print("終了．")    # 終了メッセージ
```

プログラムを実行します．

```bash
$python hello_no_sitl.py
```

終了する際は，Pythonプログラムを先にctrl+cで終了させ，
次にdronekit-sitlにctrl+cしてください．


**動作説明**
hello_no_sitl.pyの動作は基本的にhello_jp.pyと同じです．
しかし，シミュレーションしているのは地上ローバーなので，細かな状態が違います．
バッテリーは0.0V，ステータスはACTIVE(走行中)，フライトモードはMANUAL(マニュアル走行)になっています．

**解説**
シミュレーション(dronekit-sitl)のプロセスと，dronekit(connect)のプロセスが別々に起動している様子がよく分かると思います．

dronekit-sitlはTCPプロトコルの5760番ポートで待ち受けているので，
`connection_string = "tcp:localhost:5760"`
と書くことで，接続する事ができます．
※localhost(127.0.0.1)とは，ＰＣの自分自身のIPアドレスを意味しています．

# Pythonプログラム内からコマンドライン起動する

ターミナル画面(端末)を毎回2つ開くのは面倒ですね．
なので，Pythonのプログラム内から起動してしまいましょう．
subprocessライブラリを使うと可能です．
今回は，`subprocess.call`ではなく，`subprocess.Popen`を使います．

コピー＆ペーストするか，あるいは　[ここ](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter01/hello_popen.py)　を右クリック-[名前を付けて保存]してください．

```hello_popen.py
#!usr/bin/env python
# -*- coding: utf-8 -*-
print( "SITLをsubprocessで起動します" )    # 開始メッセージ

# 必要なライブラリをインポート
from subprocess import Popen      # subprocessの中から、Popenをインポート
from signal import signal, SIGINT # Ctrl+C(SIGINT)の送出のために必要 
from dronekit import connect    # フライトコントローラやシミュレータへ接続するのがdronekit内にあるconnect
import time                     # ウェイト関数time.sleepを使うために必要

# 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.079624'		# 緯度(度)
sitl_home_longitude = '136.905453'		# 経度(度)
sitl_home_altitude	= '0.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秒待つ


# フライトコントローラ(FC)へ接続
connection_string = 'tcp:localhost:' + str(5760 + int(sitl_instance_num) * 10 )	# インスタンスが増えるとポート番号が10増える
print( "FCへ接続: %s" % (connection_string) )    # 接続設定文字列を表示
vehicle = connect(connection_string, wait_ready=True)    # 接続

#Ctrl+cが押されるまでループ
try:
    while True:
        # vehicleオブジェクト内のステータスを表示
        print("--------------------------" )
        print(" GPS: %s" % vehicle.gps_0 )
        print(" Battery: %s" % vehicle.battery )
        print(" Last Heartbeat: %s" % vehicle.last_heartbeat )
        print(" Is Armable?: %s" % vehicle.is_armable )
        print(" System status: %s" % vehicle.system_status.state )
        print(" Mode: %s" % vehicle.mode.name )

        time.sleep(1)

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

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


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

print("終了．")    # 終了メッセージ
```

プログラムを実行します．ctrl+cで終了です．

```bash
$python hello_popen.py
```

**動作説明**
hello_popen.pyの動作は基本的にはhello_no_sitl.pyと同じです．
が，今回はマルチコプターです．

**解説**
sitl_で始まる5つの変数を使って，コマンドライン入力を作っています．

- sitl_frame　　　　　　：rover, plane, copterなどのビークルタイプ
- sitl_home_latitude　　：緯度を文字列で表記
- sitl_home_longitude　：経度を文字列で
- sitl_home_altitude　　：高度を文字列で
- sitl_home_direction　：機首方位(度)を文字列で
- sitl_instance_num　　：インスタンス番号

特に注目する点は最後の`sitl_instance_num`です．
これは，起動するdronekit-sitlに番号を与えることで，複数のシミュレータの起動を可能にします．
つまり，2台同時とか，10台同時とか，100台同時とかのシミュレーションができることになります．
群知能の練習台に使えますね．

これらの変数を使って，コマンドライン起動用のリストと`connection_string`を作っています．
`connection_string`は，インスタンス番号によってポート番号が10ずつ増えていきます．


実は**<font color="red">dronekit_sitlライブラリを使って細かな指定をして起動することも可能</font>**です．

出典：https://github.com/abearman/sparrow-dev/wiki/How-to-use-the-DroneKit-SITL-simulator

```python
from dronekit_sitl import SITL
sitl = SITL()  
sitl.download('solo', '1.2.0', verbose=True)   # system (e.g. "copter", "solo"), version (e.g. "3.3"), verbose
sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
sitl.launch(sitl_args, verbose=False, await_ready=False, restart=True)
```

なぜPopenスタイルにしたかというと，
**<font color="blue">将来的にdronekit_sitlではなくて，ardupilot SITL[^8]を使いたいからです．</font>**

dronekit_sitlはpipで簡単インストールができる事からもわかるように，ardupilot SITLの軽量版です．
またdronekit_sitlはplane(飛行機)がまともに動かないなどのバグが残っています．

飛行機を飛ばしたければ，フル版のardupilot SITLのsim_vehicle.pyでやらなければなりません．

# おわりに
今回はdronekitを使ってシミュレータに接続するまで，で終わってしまいました．
次回は，キーボード入力でドローンを動かしてみたいと思います．



[^1]: Ardupilotのページ　http://ardupilot.org/
[^2]: PX4のページ　https://px4.io/
[^3]: MAVproxy　http://ardupilot.github.io/MAVProxy/html/index.html
[^4]: Pythonライブラリ「pymavlink」　https://pypi.org/project/pymavlink/
[^5]: 3D Roboticsのページ　https://3dr.com/
[^6]: Pythonライブラリ「dronekit」　https://pypi.org/project/dronekit/
[^7]: Pythonライブラリ「dronekit-sitl」　https://pypi.org/project/dronekit-sitl/
[^8]: Ardupilot SITL　http://ardupilot.org/dev/docs/sitl-simulator-software-in-the-loop.html