PlantFlagTool

概要

このチュートリアルはRViz用の新しいツールの書き方について解説します。

RVizの中で、このツールは１つのクラスで、どのようにマウスイベントで視覚的なインタラクションを実現するかを決定します。　例の中で、PlantFlagToolの“フラッグ(flag)”マーカーを3Dシーンの中で配置する機能について説明します。

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

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

これは新しい PlantFlagToolを実行したサンプルです：

プラグインコード

PlantFlagToolのコードは以下のファイルで構成されます: src/plant_flag_tool.h,src/plant_flag_tool.cpp

plant_flag_tool.h

plant_flag_tool.hの全内容はここにあります: src/plant_flag_tool.h

ここは、rviz::Toolの新しいサブクラスを宣言します。ツールバーに追加できる全てのツールはrviz::Toolのサブクラスです。

class PlantFlagTool: public rviz::Tool
{
Q_OBJECT
public:
  PlantFlagTool();
  ~PlantFlagTool();

  virtual void onInitialize();

  virtual void activate();
  virtual void deactivate();

  virtual int processMouseEvent( rviz::ViewportMouseEvent& event );

  virtual void load( const rviz::Config& config );
  virtual void save( rviz::Config config ) const;

private:
  void makeFlag( const Ogre::Vector3& position );

  std::vector<Ogre::SceneNode*> flag_nodes_;
  Ogre::SceneNode* moving_flag_node_;
  std::string flag_resource_;
  rviz::VectorProperty* current_flag_property_;
};

plant_flag_tool.cpp

plant_flag_tool.cppの全内容はここにあります: src/plant_flag_tool.cpp

コンストラクタ・デストラクタ

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

ここではスーパークラスの中で定義された“shortcut_key_”メンバ変数に値をセットし、どのキーがツールをアクティベートするかを明示します。

PlantFlagTool::PlantFlagTool()
  : moving_flag_node_( NULL )
  , current_flag_property_( NULL )
{
  shortcut_key_ = 'l';
}

フラッグのOgreシーンノードを破棄するとフラッグが3Dシーンから消えます。ツールがマイナスボタンでツールバーから外された時だけ、ツールサブクラスのためのデストラクタは単に呼ばれます。

PlantFlagTool::~PlantFlagTool()
{
  for( unsigned i = 0; i < flag_nodes_.size(); i++ )
  {
    scene_manager_->destroySceneNode( flag_nodes_[ i ]);
  }
}

scene_manager_ and context_がセットされた後に、onInitialize()はスーパークラスから呼ばれます。インスタンス生成ごとに１度だけ呼ばれなければなりません。ここは大部分のインスタンス生成が１度実行されなければならないところです。ツールオブジェクトの最初のインスタンス生成の間にonInitialize（）は呼ばれます。この時点ではツールはまだアクティベートされていないので、生成ずみのいかなるシーンオブジェクトも、シーンには非表示または非接続でなければなりません。

ここ場合では、フラッグの形と外観のメッシュオブジェクトをロードし、可動フラッグのためのOgre::SceneNodeを生成し、これを非表示にしています。

void PlantFlagTool::onInitialize()
{
  flag_resource_ = "package://rviz_plugin_tutorials/media/flag.dae";

  if( rviz::loadMeshFromResource( flag_resource_ ).isNull() )
  {
    ROS_ERROR( "PlantFlagTool: failed to load model resource '%s'.", flag_resource_.c_str() );
    return;
  }

  moving_flag_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  Ogre::Entity* entity = scene_manager_->createEntity( flag_resource_ );
  moving_flag_node_->attachObject( entity );
  moving_flag_node_->setVisible( false );
}

アクティベーション・ディアクティベーション

ツールバー上のツールボタンのクリックによって、またはホットキーの押下によってツールがユーザによって始められる時に、activate()は呼ばれます。

まず可動フラッグを表示可能にセットし、フラッグの位置をユーザに表示するためにrviz::VectorPropertyを生成します。rviz::Displayと違い、rviz::Toolはrviz::Propertyのサブクラスではないため、ツールプロパティを追加したい時にはgetPropertyContainer()で親コンテナを取得し、ここにツールプロパティを追加する必要があります。

current_flag_property_を読み取り専用にセットする必要はないのですが、もし書き込み可能だと、ユーザがプロパティを変更した時にフラッグの位置を実際に変更しなければなりません。これはいいアイデアですし実現可能ですが、読者の練習問題として残しておきましょう。

void PlantFlagTool::activate()
{
  if( moving_flag_node_ )
  {
    moving_flag_node_->setVisible( true );

    current_flag_property_ = new rviz::VectorProperty( "Flag " + QString::number( flag_nodes_.size() ));
    current_flag_property_->setReadOnly( true );
    getPropertyContainer()->addChild( current_flag_property_ );
  }
}

deactivate()は他のツールがすでに選ばれていてそのツールが無効になる時に呼ばれます。

ここでは可動フラッグを非表示にし、カレントのフラッグプロパティを削除します。プロパティの削除はその親プロパティからも消去するので、別々の手順でプロパティの消去を実行する必要はありません。もしここでプロパティを削除しないなら、他のツールにスイッチしても、フラッグのリストの中にプロパティが残っているでしょう。

void PlantFlagTool::deactivate()
{
  if( moving_flag_node_ )
  {
    moving_flag_node_->setVisible( false );
    delete current_flag_property_;
    current_flag_property_ = NULL;
  }
}

マウスイベントをハンドリングする

processMouseEvent()はツールのメイン関数といえるもので、マウスインタラクションはこのツールのポイントです。

ユーティリティ関数rviz::getPointOnPlaneFromWindowXY()を使ってユーザーのマウスが接地平面の上のどこを指しているかを見て、可動フラッグをその場所に移動させ、VectorPropertyを更新します。

もしマウスイベントが左ボタンの押下なら、カレントのフラッグ位置を保存します。したがって新しいフラッグを同じ場所に作り、VectorProperty（変数名はcurrent_flag_property_）からポインタを外します。ここでやりたいことは、ポインタを外すということで、ツールがディアクティベートされた時にVectorPropertyは削除しません。

int PlantFlagTool::processMouseEvent( rviz::ViewportMouseEvent& event )
{
  if( !moving_flag_node_ )
  {
    return Render;
  }
  Ogre::Vector3 intersection;
  Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
  if( rviz::getPointOnPlaneFromWindowXY( event.viewport,
                                         ground_plane,
                                         event.x, event.y, intersection ))
  {
    moving_flag_node_->setVisible( true );
    moving_flag_node_->setPosition( intersection );
    current_flag_property_->setVector( intersection );

    if( event.leftDown() )
    {
      makeFlag( intersection );
      current_flag_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
      return Render | Finished;
    }
  }
  else
  {
    moving_flag_node_->setVisible( false ); // If the mouse is not pointing at the ground plane, don't show the flag.
  }
  return Render;
}

これはヘルパー関数で、Ogreシーンの中に新しいフラグを生成し、リストの中にこのシーンノードを保存します。

void PlantFlagTool::makeFlag( const Ogre::Vector3& position )
{
  Ogre::SceneNode* node = scene_manager_->getRootSceneNode()->createChildSceneNode();
  Ogre::Entity* entity = scene_manager_->createEntity( flag_resource_ );
  node->attachObject( entity );
  node->setVisible( true );
  node->setPosition( position );
  flag_nodes_.push_back( node );
}

フラグのロードとセーブ

調整可能なパラメータであるプロパティオブジェクトの固定セットのツールは、一般的にツールのコンストラクタの中で作られ、プロパティコンテナ(getPropertyContainer())に加えられます。この場合、ツールのサブクラスはload() と save() のオーバライドが必要無いのは、Configオブジェクトから全てのプロパティを読むのがデフォルトの振る舞いだからです。

しかしながらここでは、長さが不明なフラッグの位置リストがあるので、load() と save() そのものを実装する必要があります。

まずクラスIDをconfigオブジェクトに保存することで、裏でconfigファイルが読まれて、rviz::ToolManagerが何のインスタンス生成するのか知ることができます。

void PlantFlagTool::save( rviz::Config config ) const
{
  config.mapSetValue( "Class", getClassId() );

このツールconfigのトップレベルはマップですが、フラッグがユニークキーを持とうが持つまいが、フラッグはリストの中に収めなければなりません。したがってリストを格納するために、マップの子(flags_config) を作ります。

rviz::Config flags_config = config.mapMakeChild( "Flags" );

フラッグの位置と名前を読むために、プロパティコンテナの子数でループします：

rviz::Property* container = getPropertyContainer();
int num_children = container->numChildren();
for( int i = 0; i < num_children; i++ )
{
  rviz::Property* position_prop = container->childAt( i );

それぞれのプロパティで、単独フラッグを意味する新しいConfigオブジェクトを生成し、Configリストにこれを追加します。

rviz::Config flag_config = flags_config.listAppendNew();

フラッグのconfigの中に、その名前を保存します。

flag_config.mapSetValue( "Name", position_prop->getName() );

そしてその位置。

    position_prop->save( flag_config );
  }
}

Toolのload()関数の中で、そのクラスを読む必要はありません。なぜなら、それが呼びだされる前にすでに読みだされ、オブジェクトの生成はされていているからです。

void PlantFlagTool::load( const rviz::Config& config )
{

これは“Flags”のsub-configをTool configから取得して、そのエントリ数でループしています。

rviz::Config flags_config = config.mapGetChild( "Flags" );
int num_flags = flags_config.listLength();
for( int i = 0; i < num_flags; i++ )
{
  rviz::Config flag_config = flags_config.listChildAt( i );

このポイントで、それぞれのflag_configは単独フラグを意味しています。

ここでは、名前が何かの理由で構成ファイルの中にない場合に備えて、デフォルトの名前を提供します：

QString name = "Flag " + QString::number( i + 1 );

そして、もしそこに名前があるなら、この名前をflag_configから読むために、便利な関数mapGetString()を使います。（もし“Name”入力が無かったらfalseを返してきますが、これは気にする必要はありません。すでにデフォルトを設定してあるからです。）

flag_config.mapGetString( "Name", &name );

名前を引数として与え、位置を表示するためにrviz::VectorPropertyを生成します。

rviz::VectorProperty* prop = new rviz::VectorProperty( name );

そしてプロパティにconfigからその内容を読み出すよう通知し、全てのデータを読み出すます。

prop->load( flag_config );

それぞれのフラグを読み出し専用に設定して（すでに前の項目で議論した点です）終了し、これをプロパティコンテナに追加し、最後に3Dシーンの中の正しい位置で実際に可視化されたフラッグオブジェクトを作ります。

    prop->setReadOnly( true );
    getPropertyContainer()->addChild( prop );
    makeFlag( prop->getVector() );
  }
}

.cpp ファイルの末尾に

全てのプラグインクラス実装の終端は、ネームスペースを記述して終え、このクラスについてpluginlibに通知します。この記述はグローバルスコープで、このパッケージの名前空間の外で重要です。

} // end namespace rviz_plugin_tutorials

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

プラグインをビルドする

プラグインをビルドするには、通常の“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マークアップと解釈されることを避けるために使用をさけなければなりません。例えば、リンクタグはこのようになるでしょう：<a href="my-web-page.html">

Pluginを試してみる

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

rosrun rviz rviz

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

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

”PlantFlag”がツールバーにある時に、これがクリックされるまたはショートカットキーの“l” が押されるとフラッグの置き始めます。すでに設置したフラッグの位置を見るために、Tool Properties”パネルを開きます。

ツールバーの中の“-”（マイナス）ボタンを押し、PlantFlag”を選んでPlantFlagツールを削除するのが、現状では、旗を外す唯一の方法です。

次のステップ

これまで扱ってきたPlantFlagはまだ特に有用とはいえません。より役に立つようにするいくつかの拡張は：

• 削除、再配置、すでに置いてあるフラッグの名前変更ができるようにする
• ROSメッセージをフラッグの配信する
• フラッグの名前と位置付きでROSメッセージを配信する

すでに置いてあるフラッグに変更を加えるには：

• マウスが既存のフラッグの近くを指していることに気づくようprocessMouseEvent()を変える

• 上記の時に:
  • フラッグをハイライトさせる
  • 右ボタンが押されたら、削除と名前変更の項目が入ったコンテキストメニューを表示する
  • 左ボタンが押されたら、既存のフラッグを引き回し始める

おわりに

RVizのインタラクションの新しい形式の可能性はたくさんあります。あなたが作ったものを見られるのを楽しみにしています。