Autoware開発におけるrvizの活用ついて 

こんにちは、ティアフォーで自動運転システムを開発している高木です。今回はROS開発の心強い味方、「rviz」についてのちょっとしたお話を紹介していきたいと思います。主に開発者向けのノウハウについて書いていくので実装に踏み込んだ話が多くなりますが、そうでない方も画像だけ見れば概要は伝わると思いますので、こういった機能があることを知っていただき、ぜひ率直な感想をお聞かせください。

なお、ティアフォーでは「自動運転の民主化」をともに実現していく仲間を様々な分野から募集しています。下記ページから募集職種のリストをご覧いただき、興味を持った方はぜひコンタクトをお願いします。

tier4.jp

可視化について

Autowareは多数のプロセスが組み合わさって動作する複雑なシステムであり、最終的な出力となる車両の挙動のみを見て動作状況を把握するのは容易なことではありません。特にAutowareはオープンソースであるため、第三者が作成したプログラムとの連携が前提となります。そのため、それぞれの処理でどのような判断が行われており、どういった出力がなされているかを把握することは開発の効率や品質に大きな影響を与えます。

プログラムの動作を把握する方法としてはログ出力やデバッガなどが汎用的ですが、AutowareはROS上で動作するためrqtやrvizといった強力な周辺ツールを利用することができます。私達人間にとって視覚情報は極めて相性が高く、適切な可視化は、詳細な説明をせずとも直感的に動作を理解させるほどの効果を発揮します。というわけで、今回は可視化を行う際に大活躍するrvizについて語っていこうと思います。

rvizとは

ROSが提供しているデータの可視化を行うツールがrvizで、センサーから取得した映像や点群、各プログラムの出力結果を簡単に表示することができます。ティアフォーのデモ映像でもよく登場していて、例えばこの動画でもrvizを使っています。

www.youtube.com

それでは、ここから先はrvizの機能の細かいところについてお話していきます。

Marker/MarkerArray

恐らくrvizで何らかの表示を行いたいと考えた時、真っ先に思い浮かぶのがこれでしょう。ROS標準のメッセージ型(visualization_msg)の中で定義されており、typeを指定して様々な図形を描画することができます。

poseとpoints

知ってる方にとっては当たり前かもしれませんが、POINTSやLINE_LISTなどpointsを使用する図形ではposeとpointsが併用できます。挙動はpointsの全座標をpose.positionだけ平行移動してから、pose.positionを中心にしてpose.orientationの回転を行います。私はなぜかposeが無視されてpointsだけ使われると勘違いしていたのでpointsの時点で座標変換をしていました。同じ作業に煩わされている方がいるかもしれないのでメモとして残しておきます。

LINE_LISTとLINE_STRIP

Markerで線を描くにはLINE_LISTかLINE_STRIPを使います。LINE_LISTは頂点を2個ごとに使用して独立した直線を描き、LINE_STRIPは頂点を順番に繋いだ連続する直線を描きます。単に頂点の指定方法が違うだけかと思いきや、実は連続した直線を描く場合に微妙に挙動が異なります。下の画像は星形をそれぞれの方法で描いた結果なのですが、右のLINE_STRIPで描いた方は線の幅が均等になっていません。これは線と言いつつも内部的には四角形になっているからで、頂点で折り曲げるときに線の幅が縦に配置されてしまうためです。なので見た目を気にする場合はLINE_LISTを使ったほうが綺麗になります。

LINE_LISTとLINE_STRIPで描いた星形

透過度の罠

まずはこちらの画像をご覧ください。これは手前(Z=+1.0)に緑の三角形を、奥(Z=-1.0)に赤の三角形を配置したときの描画結果です。両方の図形で透明度は1.000に設定しています。

 続いて透過度を0.999にした結果がこちらです。再表示するたびにランダムで左右のどちらかの結果になります。赤の三角形が奥に配置されていて、Z座標は緑の三角形と2.0も離れているのにもかかわらず、手前に赤の三角形が表示されてしまう場合があります。

原因はMarker表示時に透過度に応じて以下のような設定が行われるためで、透過度を0.9998以上に設定した時にしか前後関係が正しく処理されません。

透過度 深度バッファ アルファブレンディング
0.9998以上 有効 (前後関係が正しく表示される) 無効 (上書きする)
0.9998未満 無効 (前後関係が正しく表示されない) 有効 (合成する)

両方有効にすれば良いのではないかと思ったかもしれませんが、そう簡単にはいかない事情があります。深度バッファは描画の順序によらず前後関係を正しく表示する方法であるのに対し、透過度を考慮するには背景色が必要なので奥の物体から処理しなければならないためです。残念なことに順序はMarkerで制御できないので、透過度をどちらが上になっても判別できるように調整するのが最も簡単な対応となるでしょう。

Display Plugin

Displayは主にrviz中央の3D画面に何かを表示する目的で使用します。特殊な用途でなければMarkerで十分表現可能だと思いますが、より高度な処理をしたければ自作のpluginを作成することになります。Markerに似た図形を描画するならogre_helpersにあるこれらのクラスが役立ちます。

  • Arrow
  • Axes
  • BillboardLine
  • Line
  • PointCloud
  • Shape

それでも満足できないという方はOgreという3D描画ライブラリを調べてみてください。ここから先はrvizというよりもOgreの領域になります。詳細は省きますが、大まかには次の手順で自由に図形を描画できるようになり、TRIANGLE_STRIPといったMarkerにはない描画方法の指定も行えるようになります。先ほどのogre_helpers内にあるline.cppの実装が分かりやすいです。

  1. 親となるSceneNodeから新しいSceneNodeを作成する
  2. ManualObjectを作成して新しいSceneNodeに関連付ける
  3. Materialを作成する
  4. Materialを使用してManualObjectの描画関数一式を実行する

 Panel Plugin

Panelはrvizの上下左右にGUIを追加できます。Qtというライブラリで実装されており、純粋なQtアプリケーションと似たような感覚で開発できるでしょう。固有の機能としてrvizが内部で管理しているデータにアクセスできるほか、設定をrviz本体と一緒に管理するための機能が用意されています。

個人的にはrvizと同じウィンドウでまとめて情報を表示できるのがメリットだと思っています。特段rviz固有の機能を使うわけではないのですが、使用頻度の高いデータを表示しておけば別途端末を立ち上げてrostopic echoする手間が省けます(rqtにもtopicを表示するpluginがありますが、自作する場合は最小限の情報を適切に配置できるのが強み)。このような感じでQGridLayoutとQLabelを組み合わせ、簡単に情報表示を行うpluginを作ってみたのですが、これだけでもかなり印象が変わってくるのではないでしょうか。

f:id:isamu-takagi:20210226030837p:plain

特に情報が常に視界に入っているというのが大きいです。能動的にデータを見に行く場合、気にすべきデータがどれなのかを把握し、適切なタイミングで確認しなければなりません。意外とこの作業の負担は大きく、確認し忘れなどヒューマンエラーが起こる頻度も高いです。しかし、このような表示があれば「何かエラーの部分が赤くなっているんだけど大丈夫なのだろうか」といった具合で疑問を抱かせ、迅速な対応を取ることが可能となります。

また、現地で実験を行っているスタッフを開発者が遠隔からサポートする際も役立ち、確認用のコマンドを細かく指示せずとも画面の表示を読み上げてもらうだけで大まかに状況を理解することができます。実際、スクリーンショットを一枚送ってもらうだけで問題を解決できたこともありました。今後は自動運転のサービス化を目指して車両の台数を増やしていかなければなりませんから、現地で活動するスタッフの規模も拡大していくと予想されます。そのとき、個人のスキルレベルに大きく依存せず、技術者と容易にコミュニケーションがとれるかどうかは、プログラムの重要な要素となっているはずです。

Tool Plugin

Toolはrviz上部に配置され、主に中央の3D画面を使った操作を作成できます。その性質上マウスとキーボードのイベントを処理するためのコールバック関数が提供されていて、素晴らしいことにマウスの座標と任意の平面の交点を計算する関数(getPointOnPlaneFromWindowXY)まで用意されています。もちろん、rvizにはInteractive Markerが用意されているので基本的にこちらを使えば良いのですが、例えば点群を任意の場所に出して障害物検知のテストを行いたい場合に便利だったりします。 

本来の使い方ではないような気もしますが(Tool内で処理を完結させた方が良いと思っています)、画面上でクリックされた座標をtopicとしてpublishするToolを作って利用しています。マウスがクリックされた時だけ処理を行ったり、ドラッグの始点と終点を取得したりと大抵は同じような操作が欲しくなるので該当部分をToolで実装しておき、利用したいプログラムでは結果をsubscribeすればお手軽にrvizとの連携が可能となるわけです。あくまでデバッグの範囲に限りますが、作っておくとそこそこ便利に使えます。

youtu.be

この動画では歩行者に見立てた点群をマウスで動かせるようにしてデバッグをしています。任意のタイミングで急に飛び出したり、車両に合わせて速度を調整するといった動きを用意するのはなかなか難しいのですが、マウスで歩行者の位置を操作すれば複雑なケースも自由自在にテストできます。突き詰めていくとやはり専用のツールには敵いませんが、基本的にrvizにはデバッグ用の情報を色々と出しているはずですので、同じ画面から空間を共有して操作できるというメリットは非常に大きいです。

まとめ

普段の開発では可視化まわりの処理にそこまで力を入れることはないと思います。しかし、ちょっとした工夫で効率が大きく変わったり、気づきにくいミスを防止できたりと大きな可能性を秘めている部分なので、余裕があればひと手間かけてみると良いかもしれません。