MAVSDKでSITLのドローンからテレメトリーを取得してCesiumJSにアニメーション表示してみた 〜Part 2 技術トピック編〜
前回に引き続き、今回は技術的なポイントについてお話していきます。
特に数学の知識が求められるところもあり、学生時代の勉強をサボっていたツケがここで回ってきました。
まぁこれから勉強していけばいいでしょう。
それでは、行きましょう。
CZML
CesiumJSでは、ライブラリのAPIをプログラマブルに呼び出して、各種機能を実行していきます。
まぁ一般的なサードパーティライブラリと使い方は一緒ですね。
CesiumJSには、API以外にも「CZML」という独自のデータ記述言語があり、JSONの形式でマップ上に表示するオブジェクトなどの定義ができます。
CZMLについては、非常に情報が少なく、英語のサイトですらあまり細かい情報は出てきません。
特に今回実装している細かいPacketに分割する方法は、公式Wikiに概要の紹介がありますが、他ではあまり見かけませんでした。
一応、公式のWikiを貼り付けておきます。
CZML Guide · AnalyticalGraphicsInc/czml-writer Wiki · GitHub
今回は、CZMLをブラウザとmavsdk_tryで起動したサーバとの間の通信のメッセージに使用しました。
といっても、形式はJSONと変わりませんし、難しいことはありません。通信はSSE（Server Sent Events）を使用しています。
CZMLは以下のようなデータです。
[{ "id" : "document", "name" : "Document Name", "version" : "1.0", "clock": { "interval": "2019-08-04T10:00:00Z/2019-08-04T15:00:00Z", "currentTime": "2019-08-04T10:00:00Z" } }, { "id" : "path", "name" : "path with GPS flight data", "availability" : "2019-08-04T10:00:00Z/2019-08-04T15:00:00Z", "path" : { "material" : { "polylineOutline" : { "color" : { "rgba" : [255, 0, 255, 255] }, "outlineColor" : { "rgba" : [0, 255, 255, 255] }, "outlineWidth" : 5 } }, "width" : 8, "leadTime" : 10, "trailTime" : 1000, "resolution" : 5 }, "billboard" : { "image" : "※画像のパス", "scale" : 1.5, "eyeOffset": { "cartesian": [ 0.0, 0.0, -10.0 ] } }, "position" : { "epoch" : "2019-08-04T10:00:00Z", "cartographicDegrees" : [ 0,-122.93797,39.50935,1776, 10,-122.93822,39.50918,1773, 20,-122.9385,39.50883,1772, 30,-122.93855,39.50842,1770, 40,-122.93868,39.50792,1770, 50,-122.93877,39.50743,1767 ] } }]
CesiumJSのサンドキャッスルからサンプルを拝借して、少し直したCZMLです。
JSON配列になっていて、その中にCesiumJSで表示するシーンをPacketという単位のJSONドキュメントにまとめて、配列に含めるそうです。
上の例だと、documentとpathという2つのPacketが一つのCZMLになっています。
単一のCZMLを読み込むやり方だと、このJSON配列のCZMLをCesiumJSに読み込ませてマップ上に表示させますが、
JSON配列の中の一つ一つのJSONドキュメントを順次読み込ませてマップ表示させると、部分的に処理できるものから処理していくストリーム処理を実現できます。
上記例のpositionのような位置を表すデータも、一つひとつ分割することで、リアルタイムな位置の処理が可能になります。
以下の例は、上記JSONを手で直したものです。正しくない構造ですが、イメージはつかめると思います。
[{ "id" : "document", "name" : "Document Name", "version" : "1.0", "clock": { "interval": "2019-08-04T10:00:00Z/2019-08-04T15:00:00Z", "currentTime": "2019-08-04T10:00:00Z" } }, { "id" : "path", "name" : "path with GPS flight data", "availability" : "2019-08-04T10:00:00Z/2019-08-04T15:00:00Z", "path" : { "material" : { "polylineOutline" : { "color" : { "rgba" : [255, 0, 255, 255] }, "outlineColor" : { "rgba" : [0, 255, 255, 255] }, "outlineWidth" : 5 } }, "width" : 8, "leadTime" : 10, "trailTime" : 1000, "resolution" : 5 }, "billboard" : { "image" : "※画像のパス", "scale" : 1.5, "eyeOffset": { "cartesian": [ 0.0, 0.0, -10.0 ] } }, "position" : { "epoch" : "2019-08-04T10:00:00Z", "cartographicDegrees" : [ 0,-122.93797,39.50935,1776 ] } }, { "id" : "path", "position" : { "epoch" : "2019-08-04T10:00:00Z", "cartographicDegrees" : [ 10,-122.93822,39.50918,1773 ] } }, { "id" : "path", "position" : { "epoch" : "2019-08-04T10:00:00Z", "cartographicDegrees" : [ 20,-122.9385,39.50883,1772 ] } }, { "id" : "path", "position" : { "epoch" : "2019-08-04T10:00:00Z", "cartographicDegrees" : [ 30,-122.93855,39.50842,1770 ] } }, { "id" : "path", "position" : { "epoch" : "2019-08-04T10:00:00Z", "cartographicDegrees" : [ 40,-122.93868,39.50792,1770 ] } }, { "id" : "path", "position" : { "epoch" : "2019-08-04T10:00:00Z", "cartographicDegrees" : [ 50,-122.93877,39.50743,176 ] } }]
ドキュメント→オブジェクトの初期化→Pathの最新化（順次）
というような、ストリームの流れになります。
（今回作成したmavsdk_tryは上記の流れになります。ドキュメント以降は好きなようにストリームを作れますが、私は上記の考え方がしっくりきたので、そうしています）
このJSONをサーバで順次生成してブラウザにSSEで送信することで、リアルタイムなドローンの表示を実現しています。
なお、idで指定しているdocumentは固定ですが、それ以降のオブジェクトのidは任意の名前が付けられます。特にドローンが複数機となった場合、このidを変更していくことで、複数のドローンをマップに表示できます（試してはいませんが）。
CesiumJSアニメーション
さて、次はアニメーションです。
CesiumJSでは、アニメーションを使用しなくても、オブジェクトのpositionを更新すればマップ上での位置も更新することができます。
ただ、その場合、滑らかな描画にはならないので、ある程度遠い場所に移動すると、ワープしたように見えます。
それに、3Dモデルにアニメーションが仕込んであっても、それが描画されません（ドローンで言う、プロペラが回る演出が起きません）。
そこで、CesiumJSの機能の一つであるtimeline（時間軸）の設定をして、オブジェクトの運動をシミュレーションして動きを付けてみたいと思います。
timelineで登場する時間軸のトピックで重要なものは以下のとおりです。
- Clock_Interval
- Clock_CurrentTime
- Epoch
- Sample_TIme
まず、CesiumJSのアニメーションを有効にします。
CesiumJSのClockオブジェクトのshouldAnimateをtrueにします。
// mavsdk_try/static/App.js #54 viewer.clock.shouldAnimate = true;
ClockオブジェクトのIntervalとCurrentTimeを設定します。
Intervalは、シミュレーションを行う時間の開始と終了の時間です。「2019-08-04T10:00:00Z/2019-08-04T15:00:00Z」のようにスラッシュ区切りで表現します。
シミュレーションはこの時間内を範囲として、現在日時をtick(時間を進める）します。現在日時はCurrentTimeです。
時間の進め方は倍速も出来ますし、変化率を数値で指定出来たりします。今回は普通に等倍です。（デフォルトは等倍（x1））
CZMLのdocumentにClock要素があり、シミュレーションの初期設定としてClockを設定するイメージです。
次にSampleのTimeです。
Packetの中のPositionとOrientationは、それぞれ位置と姿勢を表します。
位置も姿勢も、時間とともに変化していく要素のため、それぞれのサンプルには、そのデータに時間を含めることが出来ます。
この位置と姿勢のサンプルは、MAVSDKから取得したテレメトリーデータを加工して当てはめることが出来ます。
テレメトリーはMAVSDKからは大体10Hzくらいで取得できるため、そのテレメトリーから１秒間隔で取り出して、その時間を付与します。
すると、１秒間隔でサンプリングされたテレメトリーデータが生成出来ます。
上記CZMLのcartographicDegreesの要素は配列になっていますが、先頭から、
- サンプル時間
- 経度
- 緯度
- 高さ
の順序で１つのポジションを表現しています。
サンプル時間はある基準からの差分時間（秒）で表現しており（絶対時間形式でもできるらしいが）、今回のmavsdk_tryでは１秒間隔でテレメトリーデータを処理しますので、1ずつ加算されていきます。（上記の例では、10秒間隔のポジションデータとなっている）
CZMLのepochが、その基準時間となります。あまりこの辺は細かく検証出来ていませんが、基本的にシミュレーションを開始する時間（Intervalの開始時間）＝現在日時（CurrentTime）＝基準時間（Epoch）としておくことで、問題ないかなと思います。
上記の時間軸のトピックを図示すると以下のようなイメージになります。
リアルタイムシミュレーション
CesiumJSの例とか見てて気づいたのは、サンプルデータが最初から揃った例しかなかったことです。
今回みたいに、リアルタイムでドローンから取得したテレメトリーデータを順次CZMLで追加していくような場合、大変困ったことがありました。
CurrentTimeを現実の時間に近づけすぎると、ドローンの3Dモデルがうまく表示されないということです。点滅するように表示され、滑らかな描画になりませんでした。
上図のサンプリングデータは、その時間の断面的なデータなので、その時間を超えたデータが存在しない場合、シミュレーションを終了してしまいます。
つまり、A→Bのシミュレーション中は滑らかにアニメーションを描画しますが、Bに到達した時点で、次のB→Cのデータが存在しないと、アニメーションが終了してしまうのです。
これは、動画処理と考え方は同じだと思いますが、ある程度バッファーしてから動画の描画を始めないと、つっかえたり止まったりして、きれいな動画表示が出来ないことと同じだと思われます。
この辺、ちょっと前に動画のライブ配信の勉強をしようと思ってgstreamerを触ったりhlsの勉強をしていたおかげでピンときたので良かったです。
よって、今回のmavsdk_tryでもバッファーを持って描画しています。
goのCZMLのPacketを作成する処理の辺りの時刻処理に、以下のように差分計算処理をかましているのですが、-3秒の遅延をあえて持たせています。
（というか、すべての時刻処理に差分計算のメソッドをかましているのが、残っちゃってますね。試行錯誤の跡が見えて恥ずかしいですwww）
// mavsdk_try/mavsdk.go #65-76 d := time.Now().Add(-9 * time.Hour) docdata := Document{ Id: "document", Version: "1.0", Clock: &Clock{ Interval: d.Add(-3*time.Second).Format("2006-01-02T15:04:05Z") + "/" + d.Add(5*time.Hour).Format("2006-01-02T15:04:05Z"), CurrentTime: d.Add(-3 * time.Second).Format("2006-01-02T15:04:05Z"), Multiplier: 1.0, Range: "LOOP_STOP", Step: "SYSTEM_CLOCK_MULTIPLIER", }, }
上図でいうところの、Intervalの開始時間とCurrentTimeをあえて3秒前から実施することで、サンプルデータを3秒分バッファーした状態でアニメーションを開始出来ます。（ただし、サンプルデータの基準時間は変更しない！）
クライアントの性能次第だとは思いますが、3秒分あれば滑らかな描画には十分なバッファーだと思われます。
もちろん、3秒の遅延があるので、その差を大きいと見るか小さいと見るかは、ドローンをどのシーンで利用するかによると思います。
シビアな飛行制御のUIで使うなら、アニメーションを犠牲にしてでも、テレメトリーデータの最新位置にドローンの3Dモデルを表示すべきですし、3秒の遅延が容認されるシーンで利用されるのであれば、問題無いでしょう。その辺は、そのプロジェクト固有の判断となります。
クォータニオン
最後のトピックです。ドローンの姿勢を表すのに、私はよくオイラー角（厳密にはオイラー角ではないらしいですが、ピッチ、ロール、ヨーのことを、私はそう呼んでいます）を使用しますが、今回始めてクォータニオンという表現方法を知りました。
クォータニオンも、ピッチ、ロール、ヨーと同じく、物体の姿勢・回転を表現するものらしいですが、正直詳しいことは理解できていません。3Dゲームプログラミングの書籍などを本屋で立ち読みしたのですが、そう簡単には理解できるものではなかったです。特に、行列が絡んでいて、その辺を学生時代に匠に回避していたせいで、全然わからんのです。
ひとまず私が現時点で理解しているのは、
- 姿勢を表現するのと、回転を表現するのは、ほとんど同じ（回転の結果の姿勢という意味）
- クォータニオンは、軸（x、y、zのベクトル）と、回転（wで表すが、角度というより成分？）の4値がセットとなったもの
- オイラー角より直感的ではないが、計算の容易さ、ジンバルロック等の諸問題を回避できる
ということです。
もっと勉強が必要ですね。今回はとりあえず、MAVSDKで取得できるクォータニオンの扱いを勉強して、CesiumJSでも表示に使ってみるのが目的になります。
ということで、実際にやってみたところ、色々な問題が起きました。
MAVSDKから取得できるクォータニオンが正しくない？
MAVSDKから取得したテレメトリーデータのクォータニオンを、そのままCesiumJSの表示に使用してみたところ、以下のような表示となりました。
（以下の例は、まっすぐの姿勢：クォータニオン(w, x, y, z)=(1.0, 0.0, 0.0, 0.0)で表示した例）
ドローンの3Dモデルが変な傾き方をしているのです。
最初は3Dモデル自体が最初から傾いているのかとか色々疑って検証していたのですが、いくつかのパターンで表示させてみたところ、なんとなく法則が見えてきました。
同じクォータニオンでも、3Dモデルを表示するPositionを変更すると、傾き方が変わるのです。
試しに、上記クォータニオンで、経度:0.0、緯度:90.0の位置に表示してみたところ、まっすぐ表示されました。
（画面がバグっているのは、CesiumJSの北極点付近の処理がおかしい？からです。基本的に利用しないエリアなので、処理を粗めにしているんですかね？）
これはつまり、MAVSDKから取得できるクォータニオンには含まれていない、別のパラメータが存在するということです。
これをヒントにググってみると、以下のStack Overflowのスレッドが見つかりました。
quaternions - CESIUM : How to animate an aircraft from pitch, roll, heading? - Stack Overflow
これを読む限り、どうもクォータニオンには「ローカル軸」と「地球固定軸」という異なる基準からのクォータニオンという表現があるようです。MAVSDKはローカル軸のクォータニオン、CesiumJSは地球固定軸のクォータニオン、ということですね。
数学的知識が乏しいのですが、そういうものなんですかね？
とまぁそういうことで、上記Stack Overflowのスレッドに記載されている方法で軸の変換を行ってみたところ、どうやら正しく表示されたようです。
ソースコードで言うところの、以下の処理がその変換処理です。CZMLを一部クライアント側で変換することになるため、非常に気持ち悪い処理となりましたが、仕方ないみたいです。
// mavsdk_try/static/App.js #31-47 var quatlocal = new Cesium.Quaternion( czml.orientation.unitQuaternion[1], czml.orientation.unitQuaternion[2], czml.orientation.unitQuaternion[3], czml.orientation.unitQuaternion[4]) var pos = Cesium.Cartesian3.fromDegrees( czml.position.cartographicDegrees[1], czml.position.cartographicDegrees[2], czml.position.cartographicDegrees[3]); var mtx4 = Cesium.Transforms.eastNorthUpToFixedFrame(pos); var mtx3 = Cesium.Matrix4.getMatrix3(mtx4, new Cesium.Matrix3()); var base = Cesium.Quaternion.fromRotationMatrix(mtx3) var quat = Cesium.Quaternion.multiply(base, quatlocal, new Cesium.Quaternion()) czml.orientation.unitQuaternion[1] = quat.x; czml.orientation.unitQuaternion[2] = quat.y; czml.orientation.unitQuaternion[3] = quat.z; czml.orientation.unitQuaternion[4] = quat.w;
こういうのがあると、CZMLをクライアント-サーバ間のメッセージに直接使うのはあまりメリットが無いですね。
とまぁ、一応解決出来ましたが、これで本当に正しい姿勢を表現できているのか、判断できる数学的見識がないので、不安というか不満です。
微妙にドローンの向いている方位が違うようにも思いますが、なんとなく、計算の仕方をミスっているようにも感じるのですが、どうなんですかね？
この辺は、もっと勉強です。
2019/11/3
以下の記事にて、座標系とCesiumJSについて整理しました。
ということで、長くなりましたが、技術トピック編も終了です。
シミュレータでも、テレメトリーデータが現実に則したデータなので、マップ上に表示すると楽しいもんです。特にCesiumJSは3D表現なので、マップをグリグリ動かしているだけでも飽きません。
この調子で、ドローン界隈で盛り上がりそうな技術トピックをどんどん吸収して、その界隈で活躍できるエンジニアを目指していきたいです。
何かツッコミどころや、疑問点などがあれば、コメントください。
では、また。
◆Previous Part
tomofiles.hatenablog.com
◆Next Part
tomofiles.hatenablog.com
