navigationスタックで学ぶpluginlibの使い方 - うごくものづくりのために

navigationスタックはpluginlibによる実装がなされていて、拡張しようと思ったらpluginlibの知識が必要ということなので、勉強しています。

今回は、navigationスタックのソースを追っかけていきながら、pluginlibの基本的な使い方について解説します。自分も勉強中の身なので、間違っているところがあれば指摘をおねがいします。

pluginlibとは

pluginlib - ROS Wiki

pluginlibとは、roscppで書いたコードに、プラグインを実装するための機能です。プラグインが何なのか、自分もまだはっきりとはわかっていませんが、基本的な概念としては、

インタフェースのみを定義したクラスを用意し、そのクラスの中身の実装を外部のライブラリによって行うことによって、ユーザによってクラスの実装を入れ替えることができる仕組み

のようです。

プラグイン - Wikipedia

イメージとしては、こんな感じでしょうか。

f:id:tilt_silvie:20150928231636p:plain

で、pluginlibは、roscppで書いたコードのクラスにこのプラグインのしくみを持たせるための機能です。

navigationスタックは、BaseGlobalPlanner、BaseLocalPlannerおよびRecoveryBehaviorがプラグインとして実装されています。

今回は、pluginlibのwikiにかかれている内容を、BaseLocalPlannerのプラグインの一つであるdwa_local_plannerのソースをおっかけながら確認していきます。

解説

プラグイン構成

wikiとソースコードをおっかける前に、プラグインの構成を整理しておきます。

move_baseとBaseLocalPlanner、その実装であるプラグインは下図のような関係になっています。

f:id:tilt_silvie:20150928233033p:plain

move_baseに、nav_core::BaseLocalPlannerというインターフェースが用意されており、その実装として dwa_local_planner::DWAPlannerROSと、base_local_planner::TrajectoryPlannerROSが用意されています。

インタフェースを定義しているパッケージは、nav_coreというパッケージで、move_baseパッケージとは別物になっています。通常はインタフェースを定義するパッケージとそれを使うパッケージは同一だと思います。たぶん。

プラグインの登録

pluginlibのwiki 3.1 Registering/Exporting a Plugin の内容です。

プログラムをプラグインとして登録するには、登録したいプログラムのソースコード中（どこでもいいらしい）に、

PLUGINLIB_EXPORT_CLASS(プラグインとして登録するクラス名, インタフェースクラス名)

というマクロを記述する必要があります。

dwa_local_plannerではどうなっているかというと…

navigation/dwa_planner_ros.cpp at jade-devel · ros-planning/navigation · GitHub

dwa_planner_ros.cppの50行目に、確かにありますね。

PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, nav_core::BaseLocalPlanner)

プラグイン説明ファイル（？）

pluginlibのwiki 3.2 The Plugin Description File の内容です。

プラグインの説明ファイルをxmlで作る必要があります。

内容は、だいたいこんな感じ。

<library path="ライブラリのパス">
  <class type="プラグインとして登録するクラス名" base_class_type="インタフェースクラス名">
  <description>
  説明文
  </description>
  </class>
</library>

dwa_local_plannerではどうなっているかというと…

navigation/blp_plugin.xml at jade-devel · ros-planning/navigation · GitHub

blp_plugin.xmlがそのようです。

<library path="lib/libdwa_local_planner">
  <class name="dwa_local_planner/DWAPlannerROS" type="dwa_local_planner::DWAPlannerROS" base_class_type="nav_core::BaseLocalPlanner">
    <description>
      A implementation of a local planner using either a DWA approach based on configuration parameters.
    </description>
  </class>
</library>

<class name=〜〜〜ってやつが追加されてますが、これは必要なのかな…？　よくわかりません。

ROSのパッケージシステムにプラグインを登録する

pluginlibのwiki 3.3 The Plugin Description File の内容です。

ROSのシステムにプラグインの認識をしてもらうために、プラグインパッケージのpackage.xmlファイルに以下の追記が必要です。

<export>
  <インタフェースが定義されているパッケージ名 plugin="${prefix}/プラグイン説明ファイルの名前" />
</export>

それと、これ。

  <build_depend>インタフェースが定義されているパッケージ名</build_depend>
  <run_depend>インタフェースが定義されているパッケージ名</run_depend>

dwa_local_plannerではどうなっているかというと…

navigation/package.xml at jade-devel · ros-planning/navigation · GitHub

47行目から49行目にかけてありますね。

    <export>
        <nav_core plugin="${prefix}/blp_plugin.xml" />
    </export>

build_dependとrun_dependは、それぞれ30行目、41行目に、nav_coreパッケージが記述されています。

定義されているプラグインを検索する

pluginlibのwiki 3.4 Querying ROS Package System For Available Plugins の内容です。

端末上で rospack plugins --attrib=plugin インタフェースが定義されたパッケージを実行することで、定義されているプラグイン一覧を表示できます。

以下の例では、nav_coreパッケージに定義されているプラグイン一覧を表示します。

rospack plugins --attrib=plugin nav_core

プラグインを使う.

pluginlibのwiki 4. Using a Plugin の内容です。

いままでは、プラグイン側の話でしたが、今回はプラグインを使う側の話です。

定義されたプラグインを使うには、以下のようにします。

#include <pluginlib/class_loader.h>
#include <インタフェースクラスが定義されたヘッダ>

//... some code ...

pluginlib::ClassLoader<インタフェースクラス名> poly_loader("インタフェースが定義されたパッケージ名", "インタフェースクラス名");

try
{
  boost::shared_ptr<インタフェースクラス名> poly = poly_loader.createInstance("プラグインクラス名");

  //何か処理
}
catch(pluginlib::PluginlibException& ex)
{
  //handle the class failing to load
  ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
}

流れとしては、pluginlib::ClassLoaderでインタフェースクラスをロードして、それにcreateInstance()メソッドで実体を定義してやる、という感じでしょうか。

さて、navigationのソースコードですが、プラグインを使っているのはmove_baseパッケージです。

navigation/move_base.h at jade-devel · ros-planning/navigation · GitHub

navigation/move_base.cpp at jade-devel · ros-planning/navigation · GitHub

move_base.hの205行目に、pluginlib::ClassLoaderの定義がありますね。

      pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;

で、move_base.cppの53行目でこれを初期化しています。この書き方を知らない人は、"c++ メンバイニシャライザ"でググってください。

    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),

129行目では、createInstance()していますね。

      tc_ = blp_loader_.createInstance(local_planner);

おわりに

長い記事になってしまってすいません。

pluginlibに関する最低限のことはこれで理解出来たと思います。

既存のBaseLocalPlannerに満足が行かないので、これからがんばってあたらしいBaseLocalPlannerのプラグインをつくろうと思います。