ImuDisplay

概要

このチュートリアルではRViz用のシンプルなディスプレイプラグインの書き方について紹介します。

RVizには今のところ、sensor_msgs/Imu messagesを直接表示する方法がありません。このチュートリアルで扱うコードはsensor_msgs/Imu messagesを直接表示するrviz::Displayのサブクラスを実装します。

チュートリアル用のソースコードはrviz_plugin_tutorialsパッケージにあります。ソースのディレクトリを確認、または（もしUbuntuを使っているなら）コンパイル済みのパッケージを、以下のようにapt-getでインストールできます。

sudo apt-get install ros-hydro-visualization-tutorials

新しいImuDisplayの出力がどのようなものかという、テストスクリプトからのひと続きのsensor_msgs/Imu messagesの描画はこのようになります：

プラグインのコード

ImuDisplayのコードは以下のファイルで構成されます：src/imu_display.h,src/imu_display.cpp,src/imu_visual.h, src/imu_visual.cpp.

imu_display.h

imu_display.hの全内容はここにあります：src/imu_display.h

ここでは、rviz::Displayの新しいサブクラスを宣言します。“Displays” パネルにリストアップできる全てのディスプレイはrviz::Displayのサブクラスです。

ImuDisplayはIMU加速度ベクトルの方向と大きさを示す3Dの矢を表示します。矢のベースはImuメッセージのヘッダ内にリストされたフレームで、矢の方向はそのフレームの方向に関係しています。循環バッファにストアされた直近の加速度ベクトルの履歴が任意に表示されます。

ImuDisplay クラスそのものは、循環バッファ、編集可能なパラメータ、Displayサブクラスだけで実装されています。描画部自体は別のクラスのImuVisualとして実現されています。オブジェクトが存在する時に画面に表示し、オブジェクトが消去されたら表示を消すというのが描画の慣例です。

class ImuDisplay: public rviz::MessageFilterDisplay<sensor_msgs::Imu>
{
Q_OBJECT
public:

コンストラクタです。pluginlib::ClassLoaderはデフォルトコンストラクタを呼び出すことでインスタンスを生成するので、ここで確かに１つ生成されます.

ImuDisplay();
virtual ~ImuDisplay();

Displayからのprotectedな仮想関数のオーバーライドです。Displayの機能が有効でない時は、できる限り、Displayは来ているデータを購読するべきではなく、３DViewに何も表示すべきではありません。これらの関数はこの繋がりが作られたり破棄されたりする場所です。

protected:
  virtual void onInitialize();

ヘルパー関数で、このディスプレイをクリアして初期状態に戻します。

virtual void reset();

これらのQtスロットは変化を示すシグナルに接続しており、ユーザ編集可能なプロパティです。

private Q_SLOTS:
  void updateColorAndAlpha();
  void updateHistoryLength();

入ってくるROSメッセージを処理する関数です。

private:
  void processMessage( const sensor_msgs::Imu::ConstPtr& msg );

ビジュアルデータのリストのためのストレージ。これは循環バッファで、データはフロント（最も古い）から取り出し、バック（最新）にプッシュします。

boost::circular_buffer<boost::shared_ptr<ImuVisual> > visuals_;

ユーザ編集可能なプロパティ変数

  rviz::ColorProperty* color_property_;
  rviz::FloatProperty* alpha_property_;
  rviz::IntProperty* history_length_property_;

imu_display.cpp

imu_display.cppの全内容は：　src/imu_display.cpp

ImuDisplayのコンストラクタは引数を取ってはいけないので、コンストラクタが初期化に必要なパラメータは引数として渡すことはできません。

ImuDisplay::ImuDisplay()
{
  color_property_ = new rviz::ColorProperty( "Color", QColor( 204, 51, 204 ),
                                             "Color to draw the acceleration arrows.",
                                             this, SLOT( updateColorAndAlpha() ));

  alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
                                             "0 is fully transparent, 1.0 is fully opaque.",
                                             this, SLOT( updateColorAndAlpha() ));

  history_length_property_ = new rviz::IntProperty( "History Length", 1,
                                                    "Number of prior measurements to display.",
                                                    this, SLOT( updateHistoryLength() ));
  history_length_property_->setMin( 1 );
  history_length_property_->setMax( 100000 );
}

トップレベルのrviz::Display::initialize()で初期処理をした後、サブクラスのonInitialize()関数を呼び出します。ここで動作するクラスの全てのインスタンスを生成します。そして、メッセージフィルタのセットアップする重要な要素があるため、当面のスーパークラスのonInitialize()関数の呼び出しも、ここで確実に実行します。

“MFDClass” は MessageFilterDisplay<message type> の typedef で、スーパークラスを参照する際に毎回必要になる長いテンプレートクラス名を打ち込む手間を省きます。

void ImuDisplay::onInitialize()
{
  MFDClass::onInitialize();
  updateHistoryLength();
}

ImuDisplay::~ImuDisplay()
{
}

描画しているオブジェクトが消去される際にビジュアルデータをクリアします。

void ImuDisplay::reset()
{
  MFDClass::reset();
  visuals_.clear();
}

それぞれのビジュアルの現在のカラーとアルファ値をセットする。

void ImuDisplay::updateColorAndAlpha()
{
  float alpha = alpha_property_->getFloat();
  Ogre::ColourValue color = color_property_->getOgreColor();

  for( size_t i = 0; i < visuals_.size(); i++ )
  {
    visuals_[ i ]->setColor( color.r, color.g, color.b, alpha );
  }
}

表示できる過去の描画履歴の数をセットする

void ImuDisplay::updateHistoryLength()
{
  visuals_.rset_capacity(history_length_property_->getInt());
}

入ってくるメッセージを処理するためのコールバック関数

void ImuDisplay::processMessage( const sensor_msgs::Imu::ConstPtr& msg )
{

Imuメッセージのヘッダで、固定フレームからフレームに変換するために、ここでrviz::FrameManagerを呼び出す。もし処理が失敗した場合は、returnを返す以外は何もできません。

Ogre::Quaternion orientation;
Ogre::Vector3 position;
if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
                                                msg->header.stamp,
                                                position, orientation ))
{
  ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
             msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
  return;
}

ビジュアルポインタの循環バッファを保持し続けます。ここで次のポインタを取得、または生成し、バッファが満杯でなければポインタを保持します。

boost::shared_ptr<ImuVisual> visual;
if( visuals_.full() )
{
  visual = visuals_.front();
}
else
{
  visual.reset(new ImuVisual( context_->getSceneManager(), scene_node_ ));
}

ここは、選択したビジュアルのコンテンツをセットまたはアップデートします。

visual->setMessage( msg );
visual->setFramePosition( position );
visual->setFrameOrientation( orientation );

float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
visual->setColor( color.r, color.g, color.b, alpha );

循環バッファの末尾にビジュアルデータを送ります。

  visuals_.push_back(visual);
}

} // end namespace rviz_plugin_tutorials

このクラスについてのpluginlibの宣言。この記述はグローバルスコープで、このパッケージの名前空間の外で重要です。

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(rviz_plugin_tutorials::ImuDisplay,rviz::Display )

imu_visual.h

imu_visual.hの全内容は： src/imu_visual.h

imu_visual.hはこのディスプレイ用のビジュアルクラスについて宣言します。

ImuVisualのそれぞれのインスタンスは、単独のsensor_msgs::Imu messageを可視化したものを表します。今のところ、加速度ベクトルの方向と大きさを矢で表示するだけですが、より多くのメッセージデータを取り込んで拡張するのは容易でしょう。

class ImuVisual
{
public:

コンストラクタ。ビジュアル関連を生成し、シーンに投影しますが、未設定状態です。

ImuVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node );

デストラクタ。ビジュアル関連をシーンから取り除きます。

virtual ~ImuVisual();

メッセージ内のデータを表示するためのビジュアルの設定。

void setMessage( const sensor_msgs::Imu::ConstPtr& msg );

メッセージが参照する座標フレームのポーズをセットする。これらはsetMessage()の内部で実行されます。このsetMessage（）の内部でFrameManagerとエラー処理の呼び出しを必要としますが、これはきれいな方法ではありません。このImuVisualの方法はビジュアライゼーションに対してのみ保証します。

void setFramePosition( const Ogre::Vector3& position );
void setFrameOrientation( const Ogre::Quaternion& orientation );

それぞれのビジュアルの現在のカラーとアルファ値をセットする。これらはユーザ変更可能なパラメータで、Imuメッセージからやってくる値ではない。

  void setColor( float r, float g, float b, float a );

private:

実際の矢の形状をインプリメントするオブジェクト

boost::shared_ptr<rviz::Arrow> acceleration_arrow_;

ポーズをImuメッセージヘッダの座標フレームに合わせたシーンのノード

Ogre::SceneNode* frame_node_;

シーンマネージャ。デストラクタがframe_node_を破棄するよう問い合わせるためだけに、ここで保持される。

  Ogre::SceneManager* scene_manager_;
};

imu_visual.cpp

imu_visual.cppの全内容は：src/imu_visual.cpp

Ogre::SceneNodeはそれぞれのノードとその親ノードとの相対的な変換（位置と方向）で木構造を形成します。Ogreはレンダリングする時に、これらの変換の組み合わせ計算を行います。

ここでは、RVizの固定フレーム相対するImuのヘッダフレームのポーズを保持するノードを生成します。

frame_node_ = parent_node->createChildSceneNode();

ノードのヘッダフレームに相対するノードの位置と方向をセットできるように、フレームノードの中で矢のオブジェクトを生成する。

  acceleration_arrow_.reset(new rviz::Arrow( scene_manager_, frame_node_ ));
}

ImuVisual::~ImuVisual()
{

必要が無くなったフレームノードを破棄する。

  scene_manager_->destroySceneNode( frame_node_ );
}

void ImuVisual::setMessage( const sensor_msgs::Imu::ConstPtr& msg )
{
  const geometry_msgs::Vector3& a = msg->linear_acceleration;

geometry_msgs::Vector3からOgre::Vector3に変換する

Ogre::Vector3 acc( a.x, a.y, a.z );

加速度ベクトルの大きさを取得する。

float length = acc.length();

ベクトルの各次元ごとの長さから矢の厚みを調整し設定する

Ogre::Vector3 scale( length, length, length );
acceleration_arrow_->setScale( scale );

加速度ベクトルの方向に合わせて矢の方向をセットする

  acceleration_arrow_->setDirection( acc );
}

位置と方向はSceneNodeにまで渡されます

void ImuVisual::setFramePosition( const Ogre::Vector3& position )
{
  frame_node_->setPosition( position );
}

void ImuVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
{
  frame_node_->setOrientation( orientation );
}

カラーは矢のオブジェクトにまで渡されます

void ImuVisual::setColor( float r, float g, float b, float a )
{
  acceleration_arrow_->setColor( r, g, b, a );
}

プラグインをビルドする

プラグインをビルドするには、通常の“rosmake”を実行します。

rosmake rviz_plugin_tutorials

プラグインをエクスポートする

プラグインを他のROSパッケージから使えるようにする（今回の場合はRVizから使えるようにする）には、“plugin_description.xml”ファイルが必要です。このファイルは実際はどんな名前でもいいのですが、同じ名前で特定できるようにプラグインのパッケージの“package.xml”ファイルの中で以下のように設定します：

<export>
    <rviz plugin="${prefix}/plugin_description.xml"/>
</export>

plugin_description.xmlの中身は以下のようになります：

<library path="lib/librviz_plugin_tutorials">
  <class name="rviz_plugin_tutorials/Teleop"
         type="rviz_plugin_tutorials::TeleopPanel"
         base_class_type="rviz::Panel">
    <description>
      A panel widget allowing simple diff-drive style robot base control.
    </description>
  </class>
  <class name="rviz_plugin_tutorials/Imu"
         type="rviz_plugin_tutorials::ImuDisplay"
         base_class_type="rviz::Display">
    <description>
      Displays direction and scale of accelerations from sensor_msgs/Imu messages.
    </description>
    <message_type>sensor_msgs/Imu</message_type>
  </class>
  <class name="rviz_plugin_tutorials/PlantFlag"
         type="rviz_plugin_tutorials::PlantFlagTool"
         base_class_type="rviz::Tool">
    <description>
      Tool for planting flags on the ground plane in rviz.
    </description>
  </class>
</library>

最初の行は、コンパイル済みのライブラリがlib/librviz_plugin_tutorialsにあるという意味です。(末端の”.so”は、OSごとにpluginlibによって付加されている）このパスはパッケージのトップディレクトリに関連しています。

<library path="lib/librviz_plugin_tutorials">

次のセクションはTeleopPanelについて記述しているクラスのエントリです。

<class name="rviz_plugin_tutorials/Teleop"
       type="rviz_plugin_tutorials::TeleopPanel"
       base_class_type="rviz::Panel">
  <description>
    A panel widget allowing simple diff-drive style robot base control.
  </description>
</class>

ここには名前、型、基底クラス、そしてクラスの説明を明記します。ネームフィールドは最初の２つの文字列の組み合わせを、ソースファイルのPLUGINLIB_DECLARE_CLASS()マクロに与えたものでなければなりません。“package”名、“/”（スラッシュ）、そしてクラスの“display name”になるでしょう。“display name”はユーザインタフェースでクラスとして使われる名前です。

型のエントリは完全修飾のクラス名で、どんな名前空間もこの中に含まれていなければなりません。

base_class_typeは通常、rviz::Panel、rviz::Display、rviz::Tool、rviz::ViewControllerの一つです。

このサブセクションの説明はクラスの単純なテキスト説明で、クラス選択ダイアログとDisplaysパネルのヘルプエリアに表示されています。このセクションはハイパーリンクを含むHTMLを保持することができますが、マークアップは、XMLマークアップと解釈されることを避けるために使用をさけなければなりません。例えば、リンクタグはこのようになるでしょう：&lt;a href="my-web-page.html"&gt;

Diplayプラグインは複数のmessage_typeタグを持つことができ、その配信をまず選択しているDisplayを追加した時のRVizで使われます。

Pluginを試してみる

RVizのプラグインをコンパイルしエクスポートしたら、いつも通りrvizを走らせるだけです：

rosrun rviz rviz

そして、rvizはpluginlibを使って、rvizにエクスポートされた全てのプラグインを見つけます。

“Displays”パネルの下の方にある“Add”ボタンをクリックすることで（またはControl-Nで）、ImuDisplayを追加します。そして自分のプラグインパッケージ名（ここでは“rviz_plugin_tutorials”）の下に“Imu”が見つかるまで、下にスクロールします。

もし“Imu”がDisplay Typesのリストに無かったら、RVizのコンソールの出力からプラグインのロードに関するエラーメッセージを確認します。よくある問題として：

• plugin_description.xmlファイルが無い

• package.xmlファイルの中にプラグインのエクスポートする記述がない

• plugin_description.xmlからの、ライブラリファイル(librviz_plugin_tutorials.soのような)の参照が正しくない

一度ImuディスプレイをRVizに追加したら、ディスプレイのトピック名をsensor_msgs/Imu messagesのソースにセットするだけです。

もしIMUや他のsensor_msgs/Imu messagesのソースがでなければ、以下のPythonのスクリプトでプラグインをテストすることができます：scripts/send_test_msgs.py

このスクリプトは“/test_imu”トピックを配信するので、このトピックが入ります。

このスクリプトはImuメッセージと動いているTFのフレームの両方(“/map”に関係する“/base_link” )を配信するので、“Fixed Frame”は“/map”にセットしていることを確認してください。

最後に、Imuディスプレイの“History Length”パラメータを10に調整すると、このページの上の方の画像に似た画像が出ているはずです。

Note：もし実際のIMUからのメッセージの可視化にこれを使う際には。多くのロボットと比べて、矢は巨大なものになることでしょう：

(紫色の矢のベース部分に表示されているPR2ロボットに注目してください。） これは、Imuの加速度の単位が m/s² で、重力加速度は9.8 m/s²、そして今ここにいかなるスケーリングまたは重力補償も加速度ベクトルに適用していないということによるものです。

次のステップ

このImuDisplayはまだ強烈に有用なDisplayクラスなっていません。より有用にするための拡張は：

• 加速度ベクトルに重力補償オプションを加える
• Imuメッセージのデータをより多く可視化する

重力補償オプションを加えるには、以下の処置をすることになります：

• オプションがオンかオフかをストアするrviz::BoolPropertyを新たにInuDisplayに追加する

• Imuフレームの方向に相対する重力の方向を算出する（ImuVisual::setFrameOrientation()にセットされるように）、そしてImuVisual::setMessage()で毎回これを加速度ベクトルから引く

入力として完全なImuメッセージをImuVisualが取得するためには、Imuデータのより多くの可視化を追加する修正がImuVisualに必要です。Imuデータの表示は以下のようになるでしょう：

• 方向：Imuの参照フレームでのrviz::Axesオブジェクトで、方向を表示するように曲げる

• 角速度: これが回転の軸と回転のスピードを表示するために回転軸の周りでカーブしている３Dの矢になる？
• 方向の共分散: これが、X、Y、Z軸の各々の終端で方向を示す楕円になる？
• 直線加速度の共分散: たぶんこれが加速度の矢の終端で楕円体になる？

これらを全ての設定は視覚的に取り散らかるので、これらの機能の一部を使用可または使用不可にするためにブール型のオプションを含めるのは意味があるでしょう。