はじめに
このページは，
の１ページです．
全体を見たい場合は上記ページへお戻りください．
概要
これまでの記事で
- dronekitでドローンを動かす
- MQTTでテキストを送信(Pub)・受信(Sub)する
が出来ましたので，
次は，この２つの要素技術を組み合わせてみます．
なお，今回も使うのはdronekit-sitl(シミュレータ)です．
準備するもの
今回もdronekit，dronekit-sitlおよびMQTTのために準備した
Ubuntu Linuxの入ったＰＣを利用します．
MQTTブローカーはＰＣ自身(localhost)とします．
とりあえずPub/Subする
まずは，
・ドローンを動かす(SITL編)
・pythonでMQTT送受信
この２つの記事で使ったプログラムを１つにしてみます．
MQTTのPub側プログラムがドローンになり，
Sub側のプログラムが地上局，のようなイメージです．
Pub側プログラム
Pub側は新たにファイルを作ります．
以下をコピーするか，
あるいは ここ を右クリックして[名前を付けて保存]してください．
sitl_mqtt_pub.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も使いたいのでインポート
from dronekit import LocationGlobal, LocationGlobalRelative # ウェイポイント移動に使いたいのでインポート
import time # ウェイト関数time.sleepを使うために必要
import paho.mqtt.client as mqtt # MQTTのライブラリをインポート
#==MQTT関数の定義===========================================
# ブローカーに接続できたときの処理
def on_connect(client, userdata, flag, rc):
print("Connected with result code " + str(rc))
# ブローカーが切断したときの処理
def on_disconnect(client, userdata, flag, rc):
if rc != 0:
print("Unexpected disconnection.")
# publishが完了したときの処理
def on_publish(client, userdata, mid):
print("publish: {0}".format(mid))
#--ここからプログラムスタート----------------------------------
# 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) # 接続
#==MQTTの初期化===========================================
client = mqtt.Client() # クラスのインスタンス(実体)の作成
client.on_connect = on_connect # 接続時のコールバック関数を登録
client.on_disconnect = on_disconnect # 切断時のコールバックを登録
client.on_publish = on_publish # メッセージ送信時のコールバック
client.connect("localhost", 1883, 60) # 接続先は自分自身
client.loop_start() # 通信処理スタート
#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' )
elif key=='a': # arm
vehicle.armed = True
elif key=='d': # disarm
vehicle.armed = False
elif key=='t': # takeoff
vehicle.simple_takeoff(alt=10)
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' )
# ここは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)
#==MQTTの送信===========================================
# トピック名は以前と同じ"drone/001"
# 現在の緯度/経度/高度/方位を文字列化(str関数)して送信
client.publish("drone/001", str(vehicle.location.global_frame) )
time.sleep(1) # 1秒ウェイト
except( KeyboardInterrupt, SystemExit): # Ctrl+cが押されたら離脱
print( "SIGINTを検知" )
# フライトコントローラとの接続を閉じる
vehicle.close()
# サブプロセスにもSIGINT送信
p.send_signal(SIGINT)
p.communicate()
time.sleep(1) # 終了完了のために1秒待つ
print("終了．") # 終了メッセージ
プログラム解説
コードの中で，===========のコメントを付けてある部分が，MQTTに関する作業です．
MQTTライブラリのインポート
import paho.mqtt.client as mqtt
MQTT送受信の関数を定義
# ブローカーに接続できたときの処理
def on_connect(client, userdata, flag, rc):
print("Connected with result code " + str(rc))
# ブローカーが切断したときの処理
def on_disconnect(client, userdata, flag, rc):
if rc != 0:
print("Unexpected disconnection.")
# publishが完了したときの処理
def on_publish(client, userdata, mid):
print("publish: {0}".format(mid))
MQTTの初期化
client = mqtt.Client() # クラスのインスタンス(実体)の作成
client.on_connect = on_connect # 接続時のコールバック関数を登録
client.on_disconnect = on_disconnect # 切断時のコールバックを登録
client.on_publish = on_publish # メッセージ送信時のコールバック
client.connect("localhost", 1883, 60) # 接続先は自分自身
client.loop_start() # 通信処理スタート
この３箇所は，前回の記事と同じです．
肝心のPublish部分がこうなっています．
ドローンの位置情報をPubする
print(" Global Location: %s" % vehicle.location.global_frame)
#==MQTTの送信===========================================
# トピック名は以前と同じ"drone/001"
# 現在の緯度/経度/高度/方位を文字列化(str関数)して送信
client.publish("drone/001", str(vehicle.location.global_frame) )
今までは，print関数を使って，
端末画面上にvehicle.location.global_frame（ドローンの位置情報）を書き出していましたが，
同じものをMQTTでPublishする部分が追加されていることがわかります．
このvehicle.location.global_frameは文字列型ではないので，
str()を使って文字列にしています．
Sub側プログラム
次は，Sub側のプログラムです．
前回使ったファイルsample_sub.pyをそのまま使います．
一応，書いておきますが，すでにファイルがあるはずです．
（ここ を右クリックして[名前を付けて保存]）
sample_sub.py
#!usr/bin/env python
# -*- coding: utf-8 -*-
import paho.mqtt.client as mqtt # MQTTのライブラリをインポート
# ブローカーに接続できたときの処理
def on_connect(client, userdata, flag, rc):
print("Connected with result code " + str(rc)) # 接続できた旨表示
client.subscribe("drone/001") # subするトピックを設定
# ブローカーが切断したときの処理
def on_disconnect(client, userdata, flag, rc):
if rc != 0:
print("Unexpected disconnection.")
# メッセージが届いたときの処理
def on_message(client, userdata, msg):
# msg.topicにトピック名が，msg.payloadに届いたデータ本体が入っている
print("Received message '" + str(msg.payload) + "' on topic '" + msg.topic + "' with QoS " + str(msg.qos))
# MQTTの接続設定
client = mqtt.Client() # クラスのインスタンス(実体)の作成
client.on_connect = on_connect # 接続時のコールバック関数を登録
client.on_disconnect = on_disconnect # 切断時のコールバックを登録
client.on_message = on_message # メッセージ到着時のコールバック
client.connect("localhost", 1883, 60) # 接続先は自分自身
client.loop_forever() # 永久ループして待ち続ける
解説
ポイントとしては，「Pub/Subでトピック名を同じにすること」
ですね．
Pub側が"drone/001"のトピックを使っているので，
Sub側も同じトピック名にしないと受信できません．
例えるならば，
「シリアル通信はボーレートを同じにする」
「ソケット通信はポート番号を同じにする」
「MQTT通信はトピック名を同じにする」
ですね．
実行結果
以下に実行例を載せます．
まずPub側ですが，publish: 1publish: 2...と
Publishした際のメッセージが出ていることがわかります．
sitl_mqtt_pub.pyを実行した様子
$ python sitl_mqtt_pub.py
dronekitスタート
(sitlの初期化部分は省略)
Connected with result code 0
--------------------------
System status: STANDBY
Is Armable?: False
Armed: False
Mode: STABILIZE
Global Location: LocationGlobal:lat=35.894087,lon=139.952447,alt=None
publish: 1
--------------------------
System status: STANDBY
Is Armable?: False
Armed: False
Mode: STABILIZE
Global Location: LocationGlobal:lat=35.894087,lon=139.952447,alt=None
publish: 2
--------------------------
System status: STANDBY
Is Armable?: False
Armed: False
Mode: STABILIZE
Global Location: LocationGlobal:lat=35.894087,lon=139.952447,alt=None
publish: 3
次にSub側ですが，
受信したメッセージが
'LocationGlobal:lat=35.894087,lon=139.952447,alt=None'
だと出ていることがわかります．
最初は気圧センサーの初期化があるのでalt=Noneになっていますが，
5秒ぐらい経過(5回ぐらい受信)するとalt=17.0へ変わります．
sample_sub.pyの実行した様子
$ python sample_sub.py
Connected with result code 0
Received message 'LocationGlobal:lat=35.894087,lon=139.952447,alt=None' on topic 'drone/001' with QoS 0
Received message 'LocationGlobal:lat=35.894087,lon=139.952447,alt=None' on topic 'drone/001' with QoS 0
Received message 'LocationGlobal:lat=35.894087,lon=139.952447,alt=None' on topic 'drone/001' with QoS 0
IoT向きのメッセージにする(JSON)
上述のプログラムで，ドローンの位置情報をメッセージにして送ることができました．
これを応用すれば，現在のARM状態やフライトモードなども送ることができます．
まあPythonプログラム同士での通信であれば，str()で文字列化したデータを１文としてくっつけて送信するだけでも十分なのですが，
本企画で最終的にしたいことは，Webブラウザからの操縦です．
したがって，
Webブラウザで使われるプログラミング言語JavaScriptとの相性が良いデータフォーマットにしたい
ということです．
というわけで，IoTやクラウドでよく使われている，
JSON型で通信することにします．
JSONに関する詳細については割愛しますが，
今回の流れとしては以下のようになります．
- Pythonの辞書型をJSON型にしてPubする
- JSONでインターネットを流れる
- SubしたJSON型をまた辞書型に戻して表示する
将来的には，3.の部分が
「3. SubしたJSON型をJavaScriptの連想配列にして表示する」
へと代わりますが，それは今後の記事で解説します．
ドローンの情報を辞書型に
今回，ドローンの状態や位置の情報は辞書型に格納します．
具体的には，以下のように定義します．
辞書型
drone_info = { "status":{ "isArmable":"false",
"Arm":"false",
"FlightMode":"false"
},
"position":{ "latitude":"35.0000",
"longitude":"135.0000",
"altitude":"20",
"heading":"0"
}
}
Pythonの辞書型の詳細説明はここではしませんが，
イメージとしては以下のExcel表を考えてください．
drone_infoという器の中に，status，positionというカテゴリーがあり，
さらにその中身の変数名がisArmable,Arm,...とあるわけです．
辞書型に書き込むとき
辞書型にデータを書き込むときは，
以下のようにカギ括弧内にダブルクォート""で変数名を書くだけで，
どこに書き込むか指定できます．
カギ括弧に名前を書けばいい
drone_info["status"]["isArmable"] = str(vehicle.is_armable) # ARM可能か？
辞書型を読み込むとき
また，辞書型の値を取り出したいときは，
書き込むときと同様にカギ括弧内にダブルクォート""で変数名を書くでも良いですが，
全部printしたいときには，以下のように先頭の名前だけ書くこともできます．
一番外の名前だけでもprintが可能
print( drone_info )
実行結果
{'status': {'isArmable': 'False', 'Arm': 'False', 'FlightMode': 'STABILIZE'}, 'position': {'latitude': '35.894087', 'altitude': 'None', 'heading': '0', 'longitude': '139.952447'}}
実行結果を見ると分かりますが，longitudeのデータが後ろにあります．
作成した時の要素の順番は守られないのが辞書型の特徴です．
Ｃ言語の構造体と違って，メモリ構造が保たれるわけではないのです．
どちらかというとデータベースに近いですね．
作った時の順番が保たれるOrderedDictという型もありますが， 今回は特に使う必要はありません．
辞書型をJSON型に
JSONを取り扱うには，jsonライブラリをインポートする必要があります．
import json
Python辞書型をJSON型に変換するには，json.dumps関数を使います．
json.dump関数で辞書->JSON
json_message = json.dumps( drone_info ) # drone_infoが辞書型
逆に，JSON型からPython辞書型に戻すときは，json.loads関数を使います．
json.loads関数でJSON->辞書
dict_message = json.loads( msg.payload) # msg.payloadがJSON型
以上の予備知識を使って，プログラムを変更していきます．
Pub側プログラム
Pub側のプログラムです．
以下をコピーするか，
あるいは ここ を右クリックして[名前を付けて保存]してください．
sitl_mqtt_pub_json.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も使いたいのでインポート
from dronekit import LocationGlobal, LocationGlobalRelative # ウェイポイント移動に使いたいのでインポート
import time # ウェイト関数time.sleepを使うために必要
import paho.mqtt.client as mqtt # MQTTのライブラリをインポート
import json # json.dumps関数を使いたいのでインポート
#==MQTTでpubするJSONのベースになる辞書===========================================
drone_info = { "status":{ "isArmable":"false",
"Arm":"false",
"FlightMode":"false"
},
"position":{ "latitude":"35.0000",
"longitude":"135.0000",
"altitude":"20",
"heading":"0"
}
}
#==MQTT関数の定義===========================================
# ブローカーに接続できたときの処理
def on_connect(client, userdata, flag, rc):
print("Connected with result code " + str(rc))
# ブローカーが切断したときの処理
def on_disconnect(client, userdata, flag, rc):
if rc != 0:
print("Unexpected disconnection.")
# publishが完了したときの処理
def on_publish(client, userdata, mid):
print("publish: {0}".format(mid))
#==ここからプログラムスタート===========================================
# 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) # 接続
#==MQTTの初期化===========================================
client = mqtt.Client() # クラスのインスタンス(実体)の作成
client.on_connect = on_connect # 接続時のコールバック関数を登録
client.on_disconnect = on_disconnect # 切断時のコールバックを登録
client.on_publish = on_publish # メッセージ送信時のコールバック
client.connect("localhost", 1883, 60) # 接続先は自分自身
client.loop_start() # 通信処理スタート
#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' )
elif key=='a': # arm
vehicle.armed = True
elif key=='d': # disarm
vehicle.armed = False
elif key=='t': # takeoff
vehicle.simple_takeoff(alt=10)
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' )
# ここは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)
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) # 方位
print( drone_info ) # 作ったdrone_infoを表示
#==MQTTの送信===========================================
json_message = json.dumps( drone_info ) # 辞書型をJSON型に変換
client.publish("drone/001", json_message ) # トピック名は以前と同じ"drone/001"
time.sleep(1) # 1秒ウェイト
except( KeyboardInterrupt, SystemExit): # Ctrl+cが押されたら離脱
print( "SIGINTを検知" )
# フライトコントローラとの接続を閉じる
vehicle.close()
# サブプロセスにもSIGINT送信
p.send_signal(SIGINT)
p.communicate()
time.sleep(1) # 終了完了のために1秒待つ
print("終了．") # 終了メッセージ
解説
drone_infoという名の辞書型に，
vehicleクラスに格納されているそれぞれのデータを書き込んでいます．
vehicleの中身を辞書に書き込む
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) # 方位
その後，辞書型drone_infoをJSON型json_messageに変換し，
作ったjson_messageの方をpublishしています．
JSONにしてPub
json_message = json.dumps( drone_info )# 辞書型をJSON型に変換
client.publish("drone/001", json_message )
Sub側プログラム
Pub側のプログラムです．
以下をコピーするか，
あるいは ここ を右クリックして[名前を付けて保存]してください．
sample_sub_json.py
#!usr/bin/env python
# -*- coding: utf-8 -*-
import paho.mqtt.client as mqtt # MQTTのライブラリをインポート
import json
# ブローカーに接続できたときの処理
def on_connect(client, userdata, flag, rc):
print("Connected with result code " + str(rc)) # 接続できた旨表示
client.subscribe("drone/001") # subするトピックを設定
# ブローカーが切断したときの処理
def on_disconnect(client, userdata, flag, rc):
if rc != 0:
print("Unexpected disconnection.")
# メッセージが届いたときの処理
def on_message(client, userdata, msg):
# msg.topicにトピック名が，msg.payloadに届いたデータ本体が入っている
dict_message = json.loads(msg.payload) # payloadデータはJSONなので，辞書型に変換
print("---------------------------------------")
print("JSON message:" + str(msg.payload) ) # JSONのまま表示
print("Dict message:" + str(dict_message) ) # 辞書で表示
# MQTTの接続設定
client = mqtt.Client() # クラスのインスタンス(実体)の作成
client.on_connect = on_connect # 接続時のコールバック関数を登録
client.on_disconnect = on_disconnect # 切断時のコールバックを登録
client.on_message = on_message # メッセージ到着時のコールバック
client.connect("localhost", 1883, 60) # 接続先は自分自身
client.loop_forever() # 永久ループして待ち続ける
解説
ポイントは１箇所だけ，msg.payloadに届くデータはJSON型なので，
json.loads関数を使って辞書型に戻しています．
JSONを辞書に変換
dict_message = json.loads(msg.payload)
実行結果
$ python sample_sub_json.py
Connected with result code 0
---------------------------------------
JSON message:{"status": {"isArmable": "False", "Arm": "False", "FlightMode": "STABILIZE"}, "position": {"latitude": "35.894087", "altitude": "None", "heading": "0", "longitude": "139.952447"}}
Dict message:{u'status': {u'isArmable': u'False', u'Arm': u'False', u'FlightMode': u'STABILIZE'}, u'position': {u'latitude': u'35.894087', u'altitude': u'None', u'heading': u'0', u'longitude': u'139.952447'}}
JSONは中括弧{とダブルクォート"を使った純粋な文字列なので，綺麗な形で表示されます．
一方，辞書型は，文字コードがUTF-8であることを示すuが付いてu'status'の様に表示されています．
おわりに
これで，ドローンの位置情報をMQTTで送受信することができるようになりました．
次はいよいよ，Webブラウザでデータを表示するプログラムです．