はじめに
このページは，
の１ページです．
全体を見たい場合は上記ページへお戻りください．
概要
これまでの記事で，
ドローンの情報をMQTTで受け取って，地図上に表示する
事ができました．
次は「MQTTでドローンから情報を受け取る」のではなくて，
「MQTTでドローンに操作コマンドを送る」をやってみたいと思います．
最終的にはWebブラウザからフルコントロールできる様にしたいわけですが，
今回は，PythonのGUIプログラムで簡易的に操作できるようにしてみます．
準備するもの
・これまで使用してきたLinux PC
dronekit，dronekit-sitl,paho-mqttなどのPythonライブラリが入っている
システム構成
今回開発するプログラムは，
- MQTTでドローンを操作するプログラム
- MQTTを受けて動くドローンシミュレータ
の２つです．
上の図で示すところの右上の操作プログラムと左のシミュレータを作ります．
Webブラウザでの画面表示は変更なしです．
ところで，MQTTは送信者(Publisher)から受信者(Subscriber)への一方向の通信です．
今まで使ってきたドローン情報用のトピック名drone/001ですが，これをまた使うことはできません．
（同じトピック名で中身のデータ構造が違うメッセージが来ると，受信プログラムが処理できなくなるので）
したがって，別にドローン操縦用のトピック名を用意してあげる必要があります．
今回はctrl/001としました．
送受信の関係は下図の様になります．
ドローンを操作するGUIプログラム
まずはMQTTを使ってドローンを操作するプログラムです．
これまでは，以前の記事
「dronekitの情報をMQTTで送信してみる」
で作ったシミュレータを使っていました．
システム構成は下の図の様になります．前項の図と比較して見てください．
これまでは，ドローンの操作はシミュレータプログラム本体でやっていました．
キーボード入力を受け取るライブラリkbhit.pyを使い，
gキーでGUIDEDモード，aキーでARM，tキーで離陸...
などとやっていました．
しかし，これを実機に置き換えてみると，
「ドローンを動かす 実機・Raspberry Pi編」
でやったように，
わざわざSSHでリモートログインしてキー入力する必要があります．
少々，無駄・面倒ですね．
というわけで，
ドローンを操作するコマンドをMQTTでPublishするプログラム
を作ります．
ただし，またkbhit.pyを使ってキー入力で操縦するのは能がないので，
ボタンを押して操作するGUIプログラム
にしたいと思います．
今回はPython用GUIライブラリとしてTkinterを選択しました．
dronekitのサンプルプログラム
サンプル：microgcs.py
がTkinterなので，自分の勉強も兼ねて使ってみました．
※筆者は普段はPyQt4,PyQt5を使っています．
Pythonライブラリのインストール
aptでTkinterのパッケージをインストールする必要があります．
Tkinterライブラリのインストール
$sudo apt install python-tk
Pythonプログラム本体
ここ を右クリック-[名前を付けて保存]するか，
以下のコードをコピー＆ペーストしてファイルを作成してください．
gui_mqtt_send.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import Tkinter
import paho.mqtt.client as mqtt
import json
# MQTTブローカーの情報，パブリッシュするトピック
mqtt_server = 'localhost'
mqtt_port = 1883
mqtt_topic = 'ctrl/001' # このトピック名を受信するドローンが動く
# ドローンに投げるコマンドのベースになる辞書
drone_command = {
"command":"None",
"d_lat":"0",
"d_lon":"0",
"d_alt":"0"
}
# メイン関数
def main(args):
# Tkinterのウィンドウを作る
root = Tkinter.Tk()
root.title(u'MQTT publisher for Drone Control') # ウィンドウタイトルバー
root.geometry('400x520') # ウィンドウサイズ
# ボタンが押された時のコールバック関数
def Button_pushed(event):
# コマンド(実際はボタン上のテキスト)を取得
drone_command["command"] = event.widget["text"]
# GOTOボタンのときは，緯度/経度/高度の情報も取得する
if drone_command["command"] == "GOTO":
# mainの子なので，main内で定義されているEditBoxクラスにアクセスできる
drone_command["d_lat"] = EditBox_lat.get()
drone_command["d_lon"] = EditBox_lon.get()
drone_command["d_alt"] = EditBox_alt.get()
# MQTTのサーバー、ポート番号、トピック名を取る
mqtt_server = EditBox_Host.get()
mqtt_port = int(EditBox_Port.get() )
mqtt_topic = EditBox_topic.get()
# ブローカーへ接続
client = mqtt.Client()
client.connect( mqtt_server, mqtt_port, 60 )
client.loop_start()
# データをJSONで作ってPub
json_command = json.dumps( drone_command )
client.publish( mqtt_topic, json_command )
client.loop_stop()
#--------------------------------------------
# 以降はウィンドウのデザインだけ
# MQTTブローカーのアドレス、ポートを入力する部分
frame_top = Tkinter.Frame(root,bd=2,relief='ridge')
frame_top.pack(fill="x")
Static_Host = Tkinter.Label(frame_top,font=("",11),text=u'Broker address: ')
Static_Host.pack(anchor='n',side='left')
EditBox_Host = Tkinter.Entry(frame_top,font=("",11),width=28)
EditBox_Host.insert(Tkinter.END,'localhost')
EditBox_Host.pack(anchor='n',side='left')
EditBox_Port = Tkinter.Entry(frame_top,font=("",11),width=5)
EditBox_Port.insert(Tkinter.END,'1883')
EditBox_Port.pack(anchor='n',side='left')
# トピック名の入力部分
frame1 = Tkinter.Frame(root,pady=10)
frame1.pack()
Label_topic = Tkinter.Label(frame1,font=("",12),text="Topic name:")
Label_topic.pack(side="left")
EditBox_topic = Tkinter.Entry(frame1,font=("",12),justify="center",width=15)
EditBox_topic.insert(Tkinter.END,'ctrl/001')
EditBox_topic.pack(side="left")
# フライトモード部分
frame2 = Tkinter.Frame(root,pady=10)
frame2.pack()
Label_mode = Tkinter.Label(frame2,font=("",11),text="Flight mode:")
Label_mode.pack(side="left")
Button_mode_guided = Tkinter.Button(frame2,font=("",11),text=u'GUIDED')
Button_mode_guided.bind("<Button-1>",Button_pushed )
Button_mode_guided.pack(side="left")
Button_mode_rtl = Tkinter.Button(frame2,font=("",11),text=u'RTL')
Button_mode_rtl.bind("<Button-1>",Button_pushed )
Button_mode_rtl.pack(side="left")
# ARM/DISARM部分
frame3 = Tkinter.Frame(root,pady=10)
frame3.pack()
Label_ada = Tkinter.Label(frame3,font=("",11),text="ARM/DISARM:")
Label_ada.pack(side="left")
Button_ada_arm = Tkinter.Button(frame3,font=("",11),text=u'ARM')
Button_ada_arm.bind("<Button-1>",Button_pushed )
Button_ada_arm.pack(side="left")
Button_ada_disarm = Tkinter.Button(frame3,font=("",11),text=u'DISARM')
Button_ada_disarm.bind("<Button-1>",Button_pushed )
Button_ada_disarm.pack(side="left")
# 離着陸部分
frame4 = Tkinter.Frame(root,pady=10)
frame4.pack()
Label_ada = Tkinter.Label(frame4,font=("",11),text="Takeoff/Landing:")
Label_ada.pack(side="left")
Button_Takeoff = Tkinter.Button(frame4,font=("",11),text=u'TAKEOFF')
Button_Takeoff.bind("<Button-1>",Button_pushed )
Button_Takeoff.pack(side="left")
Button_mode_land = Tkinter.Button(frame4,font=("",11),text=u'LAND')
Button_mode_land.bind("<Button-1>",Button_pushed )
Button_mode_land.pack(side="left")
#空白
Label_blank = Tkinter.Label(root,pady=10,font=("",12),text=" ")
Label_blank.pack()
# 緯度入力
frame5 = Tkinter.Frame(root,pady=10)
frame5.pack()
Label_lat = Tkinter.Label(frame5,font=("",11),text="Latitude:")
Label_lat.pack(side="left")
EditBox_lat = Tkinter.Entry(frame5,font=("",11),justify="center",width=15)
EditBox_lat.insert(Tkinter.END,'35.893246')
EditBox_lat.pack(side="left")
# 経度入力
frame6 = Tkinter.Frame(root,pady=10)
frame6.pack()
Label_lon = Tkinter.Label(frame6,font=("",11),text="Longitude:")
Label_lon.pack(side="left")
EditBox_lon = Tkinter.Entry(frame6,font=("",11),justify="center",width=15)
EditBox_lon.insert(Tkinter.END,'139.954909')
EditBox_lon.pack(side="left")
# 高度入力
frame7 = Tkinter.Frame(root,pady=10)
frame7.pack()
Label_alt = Tkinter.Label(frame7,font=("",11),text="Altitude:")
Label_alt.pack(side="left")
EditBox_alt = Tkinter.Entry(frame7,font=("",11),justify="center",width=15)
EditBox_alt.insert(Tkinter.END,'30')
EditBox_alt.pack(side="left")
# GOTOボタン
frame8 = Tkinter.Frame(root,pady=10)
frame8.pack()
Button_goto = Tkinter.Button(frame8,font=("",11),text=u'GOTO', width=20)
Button_goto.bind("<Button-1>",Button_pushed )
Button_goto.pack(side="left")
# メインループ
root.mainloop()
return 0
# このpyファイルがscriptとして呼ばれた時はmainを実行．importされたときは何もしない
if __name__ == '__main__':
sys.exit(main(sys.argv)) # ここでmain関数を呼ぶ．argvとはＣ言語と同じコマンドライン引数のこと
実行結果
いつも通り，プログラムを実行します．
$python gui_mqtt_send.py
成功すれば，こんな画面が現れます．
フライトモードの変更や，ARM/DISARM，離着陸のボタンがあります．
また，緯度/高度/経度を入力するボックスと，それを送信するGOTOボタンがあります．
プログラム解説
MQTTに関する定義
Publishするトピック名は上述した様にmqtt_topic = 'ctrl/001'になっています．
Pubするデータの土台になる辞書型は，簡単に作りました．
ドローンに投げるコマンド
drone_command = {
"command":"None",
"d_lat":"0",
"d_lon":"0",
"d_alt":"0"
}
・command：ドローンにやらせる作業の種類．
フライトモード,ARM/DISARM,離着陸,ウェイポイント移動など
・d_lat：ウェイポイント移動の際の目標緯度．desired latitudeの略．
・d_lat：目標経度．desired longitudeの略．
・d_lat：目標高度．desired altitudeの略．
ドローンの機体の向く方向を変えることはできません．
※dronekitのsimple_goto関数(MavlinkのMAV_CMD_NAV_WAYPOINTコマンドに該当)では，
機体の向く方位を操作する引数がないからです．
くわしくは「Mission Commands」を参照．
MAV_CMD_CONDITION_YAWというコマンドを使えば可能ですが，それはまた別の機会に紹介します．
Tkinterでウィンドウを作る
メイン関数の中では，まず最初にTkinterを使ってGUIウィンドウを作っています．
まずはGUIウィンドウを作る
# メイン関数
def main(args):
# Tkinterのウィンドウを作る
root = Tkinter.Tk() # ウィンドウ本体作成
root.title(u'MQTT publisher for Drone Control') # ウィンドウタイトルバー
root.geometry('400x520') # ウィンドウサイズ
Linuxのデスクトップ環境によっては，フォントサイズが異なって見切れてしまうことがあるので，
root.geometry('400x520')のサイズは適宜変更してください．
ボタンを押したときに呼ばれる関数
WindowsでもLinuxでもMacでも，GUIウィンドウのプログラムの考え方は皆同じで，
イベントドリブン型プログラムになります．
すなわち「ボタンが押された」「ウィンドウが閉じられた」などのイベントが発生した際に
あらかじめ登録しておいた関数を呼び出して(コールバックして)くれます．
これらの関数は，main関数の中の子関数としてdef文で定義されています．
子の関数は，main関数内で定義されている変数にアクセスできるので便利です．
もしmainと同じ深さのインデントで定義して作った場合，mainの中にあるEditBox_latなどのクラスにアクセスすることができなくなります．
この様に，pythonではどこでも関数が作れ，親子関係を踏襲できるるのが便利な点ですが，
読む人をよく惑わすこともあるので注意が必要です．
ボタンを押した際に呼ばれる関数
# ボタンが押された時のコールバック関数
def Button_pushed(event):
# コマンド(実際はボタン上のテキスト)を取得
drone_command["command"] = event.widget["text"]
# GOTOボタンのときは，緯度/経度/高度の情報も取得する
if drone_command["command"] == "GOTO":
# mainの子なので，main内で定義されているEditBoxクラスにアクセスできる
drone_command["d_lat"] = EditBox_lat.get()
drone_command["d_lon"] = EditBox_lon.get()
drone_command["d_alt"] = EditBox_alt.get()
# MQTTのサーバー、ポート番号、トピック名を取る
mqtt_server = EditBox_Host.get()
mqtt_port = int(EditBox_Port.get() )
mqtt_topic = EditBox_topic.get()
# ブローカーへ接続
client = mqtt.Client()
client.connect( mqtt_server, mqtt_port, 60 )
client.loop_start()
# データをJSONで作ってPub
json_command = json.dumps( drone_command )
client.publish( mqtt_topic, json_command )
client.loop_stop()
登録したコールバックはeventという引数を持ちます．
このeventの中身に，"押したボタンの表面に書いてある文字列"の情報があるので，
event.widget["text"]で取り出しています．
すなわち，drone_command["command"]には
"GUIDED","RTL","ARM","DISARM","TAKEOFF","LAND","GOTO"のいずれかが入ります．
GOTOコマンドは，緯度/経度/高度の情報が必要です．
EditBox_lat.get()の様に，エディットボックスから文字列を取得する関数を使って取り出しています．
GOTO以外のボタンが押されたときは，
drone_command["d_lat"]などの緯度経度情報は不要なので，何も書き込みません．
その後，MQTTのブローカーへ接続し，JSON型にしたdrone_commandを送信しています．
ウィンドウをデザインする
以降は，ウィンドウのデザインをする部分です．
長いので省略しますが，
基本構造はFrameの中にLabel(Staticテキスト)やEntry(エディットボックス)やButton(ボタン)を作っています．
ウィンドウを作るプログラムの一部
# フライトモード部分
frame2 = Tkinter.Frame(root,pady=10)
frame2.pack()
Label_mode = Tkinter.Label(frame2,font=("",11),text="Flight mode:")
Label_mode.pack(side="left")
Button_mode_guided = Tkinter.Button(frame2,font=("",11),text=u'GUIDED')
Button_mode_guided.bind("<Button-1>",Button_pushed )
Button_mode_guided.pack(side="left")
Button_mode_rtl = Tkinter.Button(frame2,font=("",11),text=u'RTL')
Button_mode_rtl.bind("<Button-1>",Button_pushed )
Button_mode_rtl.pack(side="left")
コールバック関数登録のポイントを例で示すと
Button_mode_guided.bind("<Button-1>",Button_pushed )
です．
bindという関数を使って，"<Button-1>"すなわち左クリックされた時の挙動を，自作したButton_pushed関数へ紐付けています．
Windowsプログラミングで言うところの,WM_LBUTTONDOWNイベント時のコールバック登録です．
画面デザインに関しては，言葉で説明すると分かりにくいので，下の図を見てください．
ウィンドウ本体であるrootの下にぶら下がるように，frame0〜frame8があります．
Frame関数は，呼び出された順に上から並んでいくので，9段の棚があるような配置になります．
それぞれの棚に，説明文のLabel，データ入力欄のEntry，コマンドのButtonを左から順に並べています．
．．．デザイン効率が悪いですね(-_-;)
Pygubu1やPAGE2などのTkinter用の画面デザインエディタを使えば，
もっと効率よくGUIデザインすることができるようになるはずです．
メインループ
画面デザインが終わったので，Tkinterのメインループを回します．
このループ内で，クリックやキー入力のイベントを処理して，適宜コールバック関数を呼び出してくれませす．
Tkinterのメインループ
# メインループ
root.mainloop()
return 0
このroot.mainloopは，ウィンドウのXボタンが押されて終了するまで永久に回り続けます．
Xボタンが押されるとreturn 0にたどり着きます．
これは，WindowsのGUIプログラミング(Win32API)で言うところのメッセージループですね．
ウィンドウメッセージに応じてウィンドウプロシージャ(コールバック)を呼び出してくれます．
Pythonのお約束
以前も書きましたが，Pythonではこのように書くことでimportで呼ばれた時とそうでない時を判定します．
スクリプト実行かimport呼び出しかの判定文
# このpyファイルがscriptとして呼ばれた時はmainを実行．importされたときは何もしない
if __name__ == '__main__':
sys.exit(main(sys.argv)) # ここでmain関数を呼ぶ．argvとはＣ言語と同じコマンドライン引数のこと
ドローン側プログラム(dronekit-sitl)
- 操作コマンドをSubscribeして動くドローンシミュレータ
Pythonプログラム本体
ここ を右クリック-[名前を付けて保存]するか，
以下のコードをコピー＆ペーストしてファイルを作成してください．
gui_sitl_pubsub.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import time # ウェイト関数time.sleepを使うために必要
from subprocess import Popen # subprocessの中から、Popenをインポート
from signal import signal, SIGINT # Ctrl+C(SIGINT)の送出のために必要
from dronekit import connect # connectを使いたいのでインポート
from dronekit import VehicleMode # VehicleModeも使いたいのでインポート
from dronekit import LocationGlobal, LocationGlobalRelative # ウェイポイント移動に使いたいのでインポート
import Tkinter # GUIを作るライブラリ
import paho.mqtt.client as mqtt # MQTT送受信
import json # JSON<-->辞書型の変換用
#==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)]
#connection_stringの生成
connection_string = 'tcp:localhost:' + str(5760 + int(sitl_instance_num) * 10 ) # インスタンスが増えるとポート番号が10増える
#== MQTTの情報，Pub/Subするトピック =======================
mqtt_server = 'localhost'
mqtt_port = 1883
mqtt_pub_topic = 'drone/001' # Publish用のトピック名を作成
mqtt_sub_topic = 'ctrl/001' # Subscribe用のトピック名を作成
#== MQTTに送信するJSONのベースになる辞書 ===================
drone_info = {
"status":{
"isArmable":"false",
"Arm":"false",
"FlightMode":"false"
},
"position":{
"latitude":"35.0000",
"longitude":"135.0000",
"altitude":"20",
"heading":"0"
}
}
#== MQTTで受信するコマンドのベースになる辞書 ================
drone_command = {
"IsChanged":"false",
"command":"None",
"d_lat":"0",
"d_lon":"0",
"d_alt":"0"
}
#== メイン関数 ============================================
def main(args):
#==Tkinterのウィンドウを作る===============================
root = Tkinter.Tk() # ウィンドウ本体の作成
root.title(u'Dronekit-SITL Monitor') # ウィンドウタイトルバー
root.geometry('400x550') # ウィンドウサイズ
# Status
frame0 = Tkinter.Frame(root,pady=10)
frame0.pack()
Label_status = Tkinter.Label(frame0,font=("",11),text="Status:")
Label_status.pack(side="left")
EditBox_status = Tkinter.Entry(frame0,font=("",11),justify="center",width=15)
EditBox_status.pack(side="left")
# IsArmable
frame1 = Tkinter.Frame(root,pady=10)
frame1.pack()
Label_armable = Tkinter.Label(frame1,font=("",11),text="IsArmable:")
Label_armable.pack(side="left")
EditBox_armable = Tkinter.Entry(frame1,font=("",11),justify="center",width=15)
EditBox_armable.pack(side="left")
# ARM/DISARM
frame2 = Tkinter.Frame(root,pady=10)
frame2.pack()
Label_arm = Tkinter.Label(frame2,font=("",11),text="Armed:")
Label_arm.pack(side="left")
EditBox_arm = Tkinter.Entry(frame2,font=("",11),justify="center",width=15)
EditBox_arm.pack(side="left")
# フライトモード
frame3 = Tkinter.Frame(root,pady=10)
frame3.pack()
Label_flightmode = Tkinter.Label(frame3,font=("",11),text="Flight mode:")
Label_flightmode.pack(side="left")
EditBox_flightmode = Tkinter.Entry(frame3,font=("",11),justify="center",width=15)
EditBox_flightmode.pack(side="left")
# 緯度
frame5 = Tkinter.Frame(root,pady=10)
frame5.pack()
Label_lat = Tkinter.Label(frame5,font=("",11),text="Latitude:")
Label_lat.pack(side="left")
EditBox_lat = Tkinter.Entry(frame5,font=("",11),justify="center",width=15)
EditBox_lat.pack(side="left")
# 経度
frame6 = Tkinter.Frame(root,pady=10)
frame6.pack()
Label_lon = Tkinter.Label(frame6,font=("",11),text="Longitude:")
Label_lon.pack(side="left")
EditBox_lon = Tkinter.Entry(frame6,font=("",11),justify="center",width=15)
EditBox_lon.pack(side="left")
# 高度
frame7 = Tkinter.Frame(root,pady=10)
frame7.pack()
Label_alt = Tkinter.Label(frame7,font=("",11),text="Altitude:")
Label_alt.pack(side="left")
EditBox_alt = Tkinter.Entry(frame7,font=("",11),justify="center",width=15)
EditBox_alt.pack(side="left")
# 方位
frame8 = Tkinter.Frame(root,pady=10)
frame8.pack()
Label_dir = Tkinter.Label(frame8,font=("",11),text="Bearing:")
Label_dir.pack(side="left")
EditBox_dir = Tkinter.Entry(frame8,font=("",11),justify="center",width=15)
EditBox_dir.pack(side="left")
# InstanceNumber
frame4 = Tkinter.Frame(root,pady=10)
frame4.pack()
Label_num = Tkinter.Label(frame4,font=("",11),text="Instance Number:")
Label_num.pack(side="left")
EditBox_number = Tkinter.Entry(frame4,font=("",11),justify="center",width=15)
EditBox_number.pack(side="left")
# Pubトピック名
frame9 = Tkinter.Frame(root,pady=10)
frame9.pack()
Label_pubtopic = Tkinter.Label(frame9,font=("",11),text="Publish Topic:")
Label_pubtopic.pack(side="left")
EditBox_pubtopic = Tkinter.Entry(frame9,font=("",11),justify="center",width=30)
EditBox_pubtopic.pack(side="left")
# Subトピック名
frame10 = Tkinter.Frame(root,pady=10)
frame10.pack()
Label_subtopic = Tkinter.Label(frame10,font=("",11),text="Subscribe Topic:")
Label_subtopic.pack(side="left")
EditBox_subtopic = Tkinter.Entry(frame10,font=("",11),justify="center",width=30)
EditBox_subtopic.pack(side="left")
#==dronekit-sitlの起動====================================
p = Popen(sitl_boot_list) # サブプロセスの起動
time.sleep(1) # 起動完了のために1秒待つ
#==フライトコントローラ(FC)へ接続==========================
vehicle = connect( connection_string, wait_ready=True ) # 接続
#==MQTTのSubscribe関数====================================
def on_message(client, userdata, msg):
recv_command = json.loads(msg.payload)
# 受信メッセージをコマンド辞書にコピー、その際に変更フラグを付加
drone_command["IsChanged"] = "true" # 届いた際にtrueにし，コマンドを処理したらfalseにする
drone_command["command"] = recv_command["command"]
if drone_command["command"] == "GOTO":
drone_command["d_lat"] = recv_command["d_lat"]
drone_command["d_lon"] = recv_command["d_lon"]
drone_command["d_alt"] = recv_command["d_alt"]
#==MQTTの初期化===========================================
client = mqtt.Client() # クラスのインスタンス(実体)の作成
client.connect( mqtt_server, mqtt_port, 60 ) # 接続先は自分自身
client.subscribe( mqtt_sub_topic )
client.on_message = on_message
client.loop_start() # 通信処理スタート
#==1秒おきに画面表示を更新する関数=========================
def redraw():
# ステータス、Arming関連、フライトモード情報の更新
EditBox_status.delete(0,Tkinter.END) # 前の文字列を削除
EditBox_status.insert(Tkinter.END, str(vehicle.system_status.state) ) # 新しい文字列を書き込む
EditBox_armable.delete(0,Tkinter.END)
EditBox_armable.insert(Tkinter.END, str(vehicle.is_armable) )
EditBox_arm.delete(0,Tkinter.END)
EditBox_arm.insert(Tkinter.END, str(vehicle.armed) )
EditBox_flightmode.delete(0,Tkinter.END)
EditBox_flightmode.insert(Tkinter.END, str(vehicle.mode.name) )
# 緯度/経度/高度/方位の更新
EditBox_lat.delete(0,Tkinter.END)
EditBox_lat.insert(Tkinter.END, str(vehicle.location.global_frame.lat) )
EditBox_lon.delete(0,Tkinter.END)
EditBox_lon.insert(Tkinter.END, str(vehicle.location.global_frame.lon) )
EditBox_alt.delete(0,Tkinter.END)
EditBox_alt.insert(Tkinter.END, str(vehicle.location.global_frame.alt) )
EditBox_dir.delete(0,Tkinter.END)
EditBox_dir.insert(Tkinter.END, str(vehicle.heading) )
# 起動インスタンス番号、MQTTのトピック名の更新
EditBox_number.delete(0,Tkinter.END)
EditBox_number.insert(Tkinter.END, str(sitl_instance_num) )
EditBox_pubtopic.delete(0,Tkinter.END)
EditBox_pubtopic.insert(Tkinter.END, mqtt_pub_topic )
EditBox_subtopic.delete(0,Tkinter.END)
EditBox_subtopic.insert(Tkinter.END, mqtt_sub_topic )
#==Publishするデータを作る===============================
drone_info["status"]["isArmable"] = str(vehicle.is_armable) # ARM可能か？
drone_info["status"]["Arm"] = str(vehicle.armed) # ARM状態
drone_info["status"]["FlightMode"] = str(vehicle.mode.name) # フライトモード
drone_info["position"]["latitude"] = str(vehicle.location.global_frame.lat) # 緯度
drone_info["position"]["longitude"] = str(vehicle.location.global_frame.lon)# 経度
drone_info["position"]["altitude"] = str(vehicle.location.global_frame.alt) # 高度
drone_info["position"]["heading"] = str(vehicle.heading) # 方位
#==MQTTの送信===========================================
json_message = json.dumps( drone_info ) # 辞書型をJSON型に変換
client.publish("drone/001", json_message ) # トピック名は以前と同じ"drone/001"
# コマンドに対する処理
if drone_command["IsChanged"] == "true":
# GUIDEDコマンド
if drone_command["command"] == "GUIDED":
print("# Set GUIDED mode")
vehicle.mode = VehicleMode("GUIDED")
# RTLコマンド
if drone_command["command"] == "RTL":
print("# Set RTL mode")
vehicle.mode = VehicleMode("RTL")
# ARMコマンド
if drone_command["command"] == "ARM":
print("# Arming motors")
vehicle.armed = True
# DISARMコマンド
if drone_command["command"] == "DISARM":
print("# Disarming motors")
vehicle.armed = False
# TAKEOFFコマンド
if drone_command["command"] == "TAKEOFF":
print("# Take off!")
aTargetAltitude = 20.0
vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude
# LANDコマンド
if drone_command["command"] == "LAND":
print("# Set LAND mode...")
vehicle.mode = VehicleMode("LAND")
# GOTOコマンド
if drone_command["command"] == "GOTO":
print("# Set target position.")
if drone_command["d_lat"] == "0":
drone_command["d_lat"] = str( vehicle.location.global_frame.lat ) # 緯度
if drone_command["d_lon"] == "0":
drone_command["d_lon"] = str( vehicle.location.global_frame.lon ) # 経度
if drone_command["d_alt"] == "0":
drone_command["d_alt"] = str( vehicle.location.global_frame.alt ) # 高度
point = LocationGlobalRelative(float(drone_command["d_lat"]), float(drone_command["d_lon"]), float(drone_command["d_alt"]) )
vehicle.simple_goto(point, groundspeed=5)
# コマンドは読み終えたので、フラグを倒す
drone_command["IsChanged"] = "false"
drone_command["command"] = "None"
root.after(1000, redraw ) # 1秒後に自分自身を呼び出す
#==Tkinterの時間実行の機能after関数を使う===================
root.after(1000, redraw ) # 最初に1回だけは本文で呼び出す必要がある．
#==Tkinterメインループ====================================
# Xボタンを押すまでこの関数がブロックする
root.mainloop()
#==ここから終了処理========================================
# MQTT終了
client.loop_stop()
# フライトコントローラとの接続を閉じる
vehicle.close()
# サブプロセスにもSIGINT送信
p.send_signal(SIGINT)
p.communicate()
time.sleep(1) # 終了完了のために1秒待つ
return 0
# このpyファイルがimportされたのではなく，scriptとして実行された時
if __name__ == '__main__':
sys.exit(main(sys.argv)) # ここでmain関数を呼ぶ．argvはC言語と同様にコマンドライン引数
実行結果
端末から実行します．
$python gui_sitl_pubsub.py
成功するとSITLの起動に時間がかかりますが，最終的に以下の様な画面が表示されます．
プログラム解説
グローバル変数の定義
まずはmain関数の外で定義されているグローバルな変数の解説です．
dronekit-sitlはPopenで別のプロセスとして起動させるので，
そのための起動情報を前もって作っています．
また，dronekitでconnectするための，connection_stringもここで作ってしまいます．
dronekit-sitlをコマンドラインで起動させるための情報
#==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)]
#connection_stringの生成
connection_string = 'tcp:localhost:' + str(5760 + int(sitl_instance_num) * 10 ) # インスタンスが増えるとポート番号が10増える
次は，MQTTのブローカーの情報や，Publish/Subscribeするトピック名を設定しています．
MQTTの接続情報や，トピック名
#== MQTTの情報，Pub/Subするトピック =======================
mqtt_server = 'localhost'
mqtt_port = 1883
mqtt_pub_topic = 'drone/001' # Publish用のトピック名を作成
mqtt_sub_topic = 'ctrl/001' # Subscribe用のトピック名を作成
MQTTでPub/SubするJSON型データを作るために，ベースになる辞書型もグローバルで定義しています．
Pub/Subするデータの素になる辞書
#== MQTTに送信するJSONのベースになる辞書 ===================
drone_info = {
"status":{
"isArmable":"false",
"Arm":"false",
"FlightMode":"false"
},
"position":{
"latitude":"35.0000",
"longitude":"135.0000",
"altitude":"20",
"heading":"0"
}
}
#== MQTTで受信するコマンドのベースになる辞書 ================
drone_command = {
"IsChanged":"false",
"command":"None",
"d_lat":"0",
"d_lon":"0",
"d_alt":"0"
}
main関数では，最初にTkinterでウィンドウを作ります．
rootの下にFrameを棚のように重ねていき，Frameの中にEntryやButtonを配置しています．
ウィンドウデザイン(メイン関数)
#== メイン関数 ============================================
def main(args):
#==Tkinterのウィンドウを作る===============================
root = Tkinter.Tk() # ウィンドウ本体の作成
root.title(u'Dronekit-SITL Monitor') # ウィンドウタイトルバー
root.geometry('400x550') # ウィンドウサイズ
以下省略
ウインドウのデザインが終わったら，
dronekit-sitlを起動し，dronekitのconnectで接続します．
dronekit-sitlのプロセス起動とdronekitで接続
#==dronekit-sitlの起動====================================
p = Popen(sitl_boot_list) # サブプロセスの起動
time.sleep(1) # 起動完了のために1秒待つ
#==フライトコントローラ(FC)へ接続==========================
vehicle = connect( connection_string, wait_ready=True ) # 接続
次に，MQTTでSubscribeしたときのコールバック関数の定義を行います．
main関数の子となるようにインデントを取っています．
ここでのポイントは，
drone_command["IsChanged"]という要素を追加していることです．
このIsChangedは，コマンドを受信した際にtrueになり，
後述するredraw関数でコマンドを理解して処理した際にfalseになるように使っています．
つまり，受信データを1回だけ処理するためのフラグなのです．
このフラグがないと，ARM->ARM->ARM->.....のように同じコマンドを何度も送ってしまうからです．
トピック名'ctrl/001'のコマンドを受信した際の処理
#==MQTTのSubscribe関数====================================
def on_message(client, userdata, msg):
recv_command = json.loads(msg.payload)
# 受信メッセージをコマンド辞書にコピー、その際に変更フラグを付加
drone_command["IsChanged"] = "true" # 届いた際にtrueにし，コマンドを処理したらfalseにする
drone_command["command"] = recv_command["command"]
if drone_command["command"] == "GOTO":
drone_command["d_lat"] = recv_command["d_lat"]
drone_command["d_lon"] = recv_command["d_lon"]
drone_command["d_alt"] = recv_command["d_alt"]
次は，MQTTの初期設定です．必要最低限にしました．
Subするトピック名drone/001の登録，
Subした際に実行するコールバック関数on_messageの登録をしています．
ここにclient.loop_start()が書いてあるので，Tkinterのメインループroot.mainloop()とは別にループが周り，
MQTTメッセージをSubした際に自動的にon_messageを呼び出してくれます．
MQTTの初期化
#==MQTTの初期化===========================================
client = mqtt.Client() # クラスのインスタンス(実体)の作成
client.connect( mqtt_server, mqtt_port, 60 ) # 接続先は自分自身
client.subscribe( mqtt_sub_topic )
client.on_message = on_message
client.loop_start() # 通信処理スタート
今回は，このredraw関数がキモになります．
Tkinterのafter関数を使ってroot.after(1000, redraw )と書くことで1000ミリ秒後に呼び出されます．
こうすることで，1秒おきに
・Entryボックスの中身の情報を更新
・ドローンの位置情報をPublish
・コマンドの内容を読んで，該当する処理を実行
の3つの作業をしています．
Tkinterのafterを使って1000ミリ秒おきに呼ばれる関数
#==1秒おきに画面表示を更新する関数=========================
def redraw():
# ステータス、Arming関連、フライトモード情報の更新
EditBox_status.delete(0,Tkinter.END) # 前の文字列を削除
EditBox_status.insert(Tkinter.END, str(vehicle.system_status.state) ) # 新しい文字列を書き込む
EditBox_armable.delete(0,Tkinter.END)
EditBox_armable.insert(Tkinter.END, str(vehicle.is_armable) )
EditBox_arm.delete(0,Tkinter.END)
EditBox_arm.insert(Tkinter.END, str(vehicle.armed) )
EditBox_flightmode.delete(0,Tkinter.END)
EditBox_flightmode.insert(Tkinter.END, str(vehicle.mode.name) )
# 緯度/経度/高度/方位の更新
EditBox_lat.delete(0,Tkinter.END)
EditBox_lat.insert(Tkinter.END, str(vehicle.location.global_frame.lat) )
EditBox_lon.delete(0,Tkinter.END)
EditBox_lon.insert(Tkinter.END, str(vehicle.location.global_frame.lon) )
EditBox_alt.delete(0,Tkinter.END)
EditBox_alt.insert(Tkinter.END, str(vehicle.location.global_frame.alt) )
EditBox_dir.delete(0,Tkinter.END)
EditBox_dir.insert(Tkinter.END, str(vehicle.heading) )
# 起動インスタンス番号、MQTTのトピック名の更新
EditBox_number.delete(0,Tkinter.END)
EditBox_number.insert(Tkinter.END, str(sitl_instance_num) )
EditBox_pubtopic.delete(0,Tkinter.END)
EditBox_pubtopic.insert(Tkinter.END, mqtt_pub_topic )
EditBox_subtopic.delete(0,Tkinter.END)
EditBox_subtopic.insert(Tkinter.END, mqtt_sub_topic )
#==Publishするデータを作る===============================
drone_info["status"]["isArmable"] = str(vehicle.is_armable) # ARM可能か？
drone_info["status"]["Arm"] = str(vehicle.armed) # ARM状態
drone_info["status"]["FlightMode"] = str(vehicle.mode.name) # フライトモード
drone_info["position"]["latitude"] = str(vehicle.location.global_frame.lat) # 緯度
drone_info["position"]["longitude"] = str(vehicle.location.global_frame.lon)# 経度
drone_info["position"]["altitude"] = str(vehicle.location.global_frame.alt) # 高度
drone_info["position"]["heading"] = str(vehicle.heading) # 方位
#==MQTTの送信===========================================
json_message = json.dumps( drone_info ) # 辞書型をJSON型に変換
client.publish("drone/001", json_message ) # トピック名は以前と同じ"drone/001"
# コマンドに対する処理
if drone_command["IsChanged"] == "true":
# GUIDEDコマンド
if drone_command["command"] == "GUIDED":
print("# Set GUIDED mode")
vehicle.mode = VehicleMode("GUIDED")
# RTLコマンド
if drone_command["command"] == "RTL":
print("# Set RTL mode")
vehicle.mode = VehicleMode("RTL")
# ARMコマンド
if drone_command["command"] == "ARM":
print("# Arming motors")
vehicle.armed = True
# DISARMコマンド
if drone_command["command"] == "DISARM":
print("# Disarming motors")
vehicle.armed = False
# TAKEOFFコマンド
if drone_command["command"] == "TAKEOFF":
print("# Take off!")
aTargetAltitude = 20.0
vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude
# LANDコマンド
if drone_command["command"] == "LAND":
print("# Set LAND mode...")
vehicle.mode = VehicleMode("LAND")
# GOTOコマンド
if drone_command["command"] == "GOTO":
print("# Set target position.")
if drone_command["d_lat"] == "0":
drone_command["d_lat"] = str( vehicle.location.global_frame.lat ) # 緯度
if drone_command["d_lon"] == "0":
drone_command["d_lon"] = str( vehicle.location.global_frame.lon ) # 経度
if drone_command["d_alt"] == "0":
drone_command["d_alt"] = str( vehicle.location.global_frame.alt ) # 高度
point = LocationGlobalRelative(float(drone_command["d_lat"]), float(drone_command["d_lon"]), float(drone_command["d_alt"]) )
vehicle.simple_goto(point, groundspeed=5)
# コマンドは読み終えたので、フラグを倒す
drone_command["IsChanged"] = "false"
drone_command["command"] = "None"
root.after(1000, redraw ) # 1秒後に自分自身を呼び出す
#==Tkinterの時間実行の機能after関数を使う===================
root.after(1000, redraw ) # 最初に1回だけは本文で呼び出す必要がある．
if drone_command["IsChanged"] == "true":と
drone_command["IsChanged"] = "false"を対にして使うことで，同じコマンドを何度も実行しないようにしています．
「コマンドはもう読み終わったから，次回のループでは読まなくていいからね」という操作です．
後は，Tkinterのメインループが半永久的に周ります．
Xボタンで終了した後は，MQTTのループを止め，dronekitを切断し，dronekit-sitlのプロセスを殺して終了です．
Tkinterのメインループ->最後まで
#==Tkinterメインループ====================================
# Xボタンを押すまでこの関数がブロックする
root.mainloop()
#==ここから終了処理========================================
# MQTT終了
client.loop_stop()
# フライトコントローラとの接続を閉じる
vehicle.close()
# サブプロセスにもSIGINT送信
p.send_signal(SIGINT)
p.communicate()
time.sleep(1) # 終了完了のために1秒待つ
return 0
# このpyファイルがimportされたのではなく，scriptとして実行された時
if __name__ == '__main__':
sys.exit(main(sys.argv)) # ここでmain関数を呼ぶ．argvはC言語と同様にコマンドライン引数
操作実験
それぞれ別の端末で，操作プログラムとドローン側シミュレータを立ち上げてください．
端末1
$python gui_mqtt_send.py
端末2
$python gui_sitl_pubsub.py
操作方法
これまでコマンドラインのキー入力でしていたのと同じ操作を，ボタンでやります．
フライトモードをGUIDEDにする -> ARM -> 離陸する -> 移動させる
緯度/経度/高度の数値は変更できるので，別の座標を入れることができます．
※入力された数値の整合性チェックはしていませんので，
文字やありえない数値を入力した際にはエラーになるので注意してください．
本来のアプリケーション開発では，こういうユーザーの入力ミスをチェックする関数を作る必要があります．
Webブラウザで
http://localhost/marker_moving_rotated.html
を開いて，動きを見てみましょう．
おわりに
今回は新たにトピック名ctrl/001としてメッセージを作り，
ドローンを操作するコマンドを送るプログラムを作りました．
また，コマンドを受け取る新しいdronekit-sitlのシミュレータを作り，
動作確認をしました．
MQTTを使って，ドローン情報の取得(受信)とドローンへのコマンド(送信)
という双方向の通信ができるようになりました．
しかし，地図表示されたWebブラウザとは別にアプリを起動させておくというのは，少々面倒です．
次回は下図の様に，Webブラウザを用いて操作できるようにします．
