ROS講座59 ROSにおけるpluginの書き方

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
c言語ではプラグインという形でプログラムを分割することができます。処理の一部をプラグインとするとその部分だけを実行時の処理で交換することができます。ROSではpluginlibという仕組みでこれを行います。以下のような作業を行います。

* インターフェイスクラスの定義(ros_lecture_base)
    * プラグイン用の基本クラスを定義します。 
* プラグインを呼ぶROSノードの実装(adv_lecture)
    * プラグインを読んでインターフェイスクラスを実体化 
    * プラグインを呼ぶプログラムの作成
        * (option)与えられた引数やrosparamによって処理を変える
    * cmakeの設定
* プラグインの実装(plugin_lecture)
    * インターフェイスクラスを継承したクラスの作成
    * cmakeの設定
    * plugin_descriptionの記述
    * package.xmlでのプラグインのexportの記述

今回はクラスを単位としたプラグインで書きます。

```plantuml
ros_lecture_base<|--plugin_lecture
ros_lecture_base<|--adv_lecure
plugin_lecture <..adv_lecure
```

# インターフェイスクラスの定義(ros_lecture_base)

## インターフェイスクラス

プラグインのひな型を作ります。

```c++:ros_lecture_base/include/adv_lecture/adv_calc_base.h
#pragma once

namespace ros_lecture_base{
class calc_base{
public:
  virtual int op(int a, int b)=0;
};
}
```

* メソッドを仮想関数として定義します。後々このメソッドがプラグインとして実装されます。
* このヘッダファイルは他のパッケージでビルド時に参照されます。呼び出す側でインターフェイスクラスの名前がぶつかる可能性があるので名前空間を切ります。ROSではROSパッケージ名の名前空間をつける慣習があります。このファイルでの`namespace ros_lecture_base{}`がそれにあたります。

## CMakeList
```cmake:ros_lecture_base/CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(ros_lecture_base)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  pluginlib
)

catkin_package(
 INCLUDE_DIRS include
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)
```

* pluginlibに依存しているのでfind_packageに入れます。
* またこのROSパッケージのincludeの内容を他のROSパッケージが参照するのでcatkin_packageにincludeを追加します。

# プラグインを呼ぶROSノードの実装(adv_lecture)

## プラグインを呼ぶプログラム

プラグインを呼んでそれを実行するプログラムです。

```c++:adv_lecture/src/adv_call_plugin.cpp
#include <pluginlib/class_loader.h>
#include <ros_lecture_base/adv_calc_base.h>
#include <string>

int main(int argc, char** argv){
  pluginlib::ClassLoader<ros_lecture_base::calc_base> calc_loader("adv_lecture", "calc_base::calc_base");

  std::string plugin_name = "plugin_lecture/add";
  try{
    boost::shared_ptr<ros_lecture_base::calc_base> add = calc_loader.createInstance(plugin_name);
    ROS_INFO("%s(2,3)=%i", plugin_name.c_str(), add->op(2,3));
  }
  catch(pluginlib::PluginlibException& ex)
  {
    ROS_ERROR("failed to load add plugin. Error: %s", ex.what());
  }
  return 0;
}
```

* インクルードではプラグインをロードするクラスである`#include <pluginlib/class_loader.h>`と上記で定義したインターフェスクラスの`#include <ros_lecture_base/adv_calc_base.h>`を読み込みます。
* 次に`ClassLoader`を生成します。`pluginlib::ClassLoader<ros_lecture_base::calc_base> calc_loader("adv_lecture", "calc_base::calc_base");`引数などは以下のようにします。
    * テンプレートの部分はインターフェイスクラスの型
    * 1つ目の引数はこのROSノードの所属するパッケージ名
    * 2つ目の引数はインターフェイスクラスの型。
* `boost::shared_ptr<ros_lecture_base::calc_base> add = calc_loader.createInstance(plugin_name);`でプラグインをロードします。最初のテンプレートの部分はインターフェイスクラスの型、引数はプラグインの名前です。「プラグインの名前」については後述します。

## ビルドの設定

includeしている`pluginlib`を依存に追加します。

```cmake:adv_lecture/CMakeLists.txtに追加
find_package(catkin REQUIRED COMPONENTS
  # すでにいろいろある
  ros_lecture_base
  pluginlib #この行を追加
)
```

またadv_call_plugin.cppをビルドする設定をします。

```cmake:adv_lecture/CMakeLists.txtに追加
add_executable(adv_call_plugin src/adv_call_plugin.cpp)

target_link_libraries(adv_call_plugin
  ${catkin_LIBRARIES}
)
```

## とりあえず実行すると
まだプラグインを作っていないが`catkin_make`するとビルドは通ります。プラグインのロードは実行時に行われるのでビルド時には問題になりません。ここで`rosrun adv_lecture adv_call_plugin`を実行すると以下のようにプラグインがロードできないというエラーが出ます。

```text:プラグインのロードエラー
[ERROR] [1536998994.267477707]: failed to load add plugin. Error: According to the loaded plugin descriptions the class add with base class type calc_base does not exist. Declared types are 
```

# プラグインの実装(plugin_lecture)

## プラグイン（ライブラリ）の実装

```c++:plugin_lecture/src/plugin_calc_add.cpp
#include <pluginlib/class_list_macros.h>
#include <ros_lecture_base/adv_calc_base.h>

namespace plugin_lecture
{
class add : public ros_lecture_base::calc_base
{
public:
  int op(int a, int b)
  {
    return a + b;
  }
};
}  // namespace plugin_lecture
PLUGINLIB_EXPORT_CLASS(plugin_lecture::add, ros_lecture_base::calc_base)
```

* 先ほどのadv_lectureのインターフェイスクラスと同様で、このヘッダファイルはほかのパッケージから読まれるので、ROSパッケージ名の名前空間で切ります。
* `PLUGINLIB_EXPORT_CLASS(plugin_lecture::add, ros_lecture_base::calc_base)`はプラグインを登録するpluginlibのマクロです。1つ目の引数はプラグインとして登録するクラス、2つ目はそのベースクラスです。

## CMakeList.txt

依存にros_lecture_baseを足します。インターフェイスクラスのヘッダファイルのインクルードに必要です。

```cmake:plugin_lecture/CMakeLists.txtに追加
find_package(catkin REQUIRED COMPONENTS
  #色々
  ros_lecture_base #この行を追加
)
```

今回作ったのは実行ファイルでなくプラグイン（=ライブラリ）なのでadd_executableではなく以下を書きます。

```cmake:plugin_lecture/CMakeLists.txtに追加
add_library(${PROJECT_NAME}
  src/plugin_calc_add.cpp
)
```

## plugin_description
`plugin_lecture`パッケージの中にプラグインがあることを他のパッケージに知らせるためのファイルです。
library要素のpathはshared objectファイルのパスを書きます。`lib/libplugin_lecture`のように`lib/lib{ターゲット名}`のように生成されます。
class要素のnameはプラグインを呼ぶときに使われるプラグインの名前、typeはプラグインのクラスの名前、base_class_typeはベースクラスの名前です。nameは好きにつけてかまいませんが`package_name/class_name`とするのが普通です。

```xml:plugin_lecture/plugin_description.xml
<library path="lib/libplugin_lecture">
  <class name="plugin_lecture/add" type="plugin_lecture::add" base_class_type="ros_lecture_base::calc_base">
    <description>This is a add plugin.</description>
  </class>
</library>
```

## package.xml

以下の2行を書き加えます。このROSパッケージの中にプラグインがあることを示す上記のplugin_descriptionをadv_lectureに通知します。

```xml:plugin_lecture/package.xmlに追加
<exec_depend>adv_lecture</exec_depend>
<export>
  <adv_lecture plugin="${prefix}/plugin_description.xml" />
</export>
```

exec_dependタグは依存の記述で必須です。

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# プラグインの確認
以下のコマンドで`adv_lecture`に関連付けられているプラグインの一覧が見えます。

```shell
rospack plugins --attrib=plugin adv_lecture
```

# 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

`rosrun adv_lecture adv_call_plugin`でプラグインを呼ぶプログラムを実行します。

```text:実行結果
[ INFO] [1556514977.067981081]: plugin_lecture/add(2,3)=5
```

# プラグイン関連のエラー

そもそもplugin_descriptionが正しく読み込めていないとき（プラグインを作る側のpackage.xmlを正しく治す必要がある）。

```text
failed to load sub plugin. Error: According to the loaded plugin descriptions the class my_namespace::sub with base class type plugin_namespace::calc does not exist. Declared types are  my_namespace::add
```

plugin_description.xmlには書かれているけどsoファイルが見つからないとき（plugin_description.xmlの記述を直す必要がある）

```text
failed to load add plugin. Error: Could not find library corresponding to plugin my_namespace::add. Make sure the plugin description XML file has the correct name of the library and that the library actually exists.
```

plugin_description.xmlの記述が間違えている時には以下のようなエラーも出ます。

```text
terminate called after throwing an instance of 'pluginlib::CreateClassException'
```

またプラグインを作る側でベースクラスの仮想メソッドをすべてオーバーライドしないと以下のようなエラーが出ます。

```
[ERROR] [1566195408.533019595]: failed to load add plugin. Error: Failed to load library /home/..../***.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = /home/..../***.so: undefined symbol: _ZTVN11s4_hardware10TestPluginE)
```

# プラグインの参照を解決するまでの仕組み
普通のコードでは使用する側がライブラリに依存を書いて参照します。しかし今回のコードでは使用する側(adv_lecture)はライブライ(plugin_lecture)に依存を書いていません（adv_lectureの中のコードにはplugin_lectureという文字列はないです）。逆にplugin_lectureの中でadv_lectureへの記述があります。ここがpluginlibの面白いところで依存を逆にexportしています。この仕組みは以下のようになっています。

* 使用する側(adv_lectureのadv_call_plugin)の中のClassLoaderの中でrospackコマンドが実行され、「名前」が第1引数の"adv_lecture"、「属性」"plugin"のexportを検索します。
    * これによっては以下にあるROSパッケージのpackage.xmlの"export"タグの中が次のようになっているものを検索します。`<adv_lecture plugin="*******" />`
    * 上記のタグで書かれているパスのファイル(plugin定義ファイル)をすべて読み込みます。
    * plugin定義ファイルの中のリストからbase_class_typeがClassLoaderの第2引数("ros_lecture_base::calc_base")と一致する物のリストを内部で生成します。
* `ClassLoader::createInstance(plugin_name)`でプラグインを実体化します。
    * 上記のリストからplugin定義ファイルのtype=plugin_nameと一致する物を抜き出します。
    * plugin定義ファイルの上記の行の親要素のlibraryタグのpath属性にあるライブラリのオブジェクトを動的ロードします。

# 参考
[ROSwiki: プラグインの書き方](http://wiki.ros.org/pluginlib/Tutorials/Writing%20and%20Using%20a%20Simple%20Plugin)
[ROSanswer: 他のパッケージのファイルをinclude](https://answers.ros.org/question/201977/include-header-file-from-another-package-indigo/)
[ROSwiki: CMakeListのcatkin_package()](http://wiki.ros.org/catkin/CMakeLists.txt#catkin_package.28.29)
[rospack API](http://docs.ros.org/en/noetic/api/roslib/html/c++/namespaceros_1_1package.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)