Gazebo から ROS のプラグインを呼ぶ処理の仕組み（1. Gazebo起動時のSystemPluginの読込み）

# はじめに
さて，先日小生が投稿した記事である[Controller と HardwareInterface との間の処理の仕組み（2. RobotHWSimのプラグインについて）](http://qiita.com/MoriKen/items/5cab7436c1b36c25e0ce)に対して頂いたご意見の中に，
「よく読むと`Gazebo`からプラグインを呼んでいることが分かる」
という趣旨のコメントがありました．

これを見た瞬間，
「むむ，たしかに，`Gazebo`くんから`GazeboRosControlPlugin`ってどうやって呼んでいるのか全く気にしなかったわけではないのだが，こっそり棚上げにしていたのがバレた．かたじけない．」
という思いがこみ上げ，結局調べてみたので，忘れる前にまとめることにしました．

ただ，実際調べてみると，`GazeboRosApiPlugin`や`GazeboRosApiPlugin`なるプラグインも読み込まれているようで，この際だから両方流れだけでも追っておこうと考え，まとめることにしました．

# 目次
1. [Gazebo起動時のSystemPluginの読込み](http://qiita.com/MoriKen/items/2ed9c887dd078e69cc23)　←　イマココ
 1. `ROS`からの`gazebo`の起動
 2. `gazebo::addPlugin()`を追う
 3. `this->PreLoad()`を追う
1. [ロボットモデル登録時のModelPluginの読込み](http://qiita.com/MoriKen/items/86289224696756388df4)
 1. `gazebo_ros_pkgs`の`spawn_model`を追う
 2. `spawn_urdf_model`のサービスサーバ
 3. `gazebo::physics::World`を追う
 4. `"entity_info"`について

# クラス構成
今回扱う`ROS`のプラグイン周りのクラス図です．
![gazebo_plugin.png](https://qiita-image-store.s3.amazonaws.com/0/110138/3b2d42c8-df18-0211-8709-bd3749500357.png)

図の上側が`gazebo`のプロジェクトに属し，下側が`gazebo_ros_pkgs`のプロジェクトに属します．
同図の左側にある`SystemPlugin`は外部(今回なら`ROS`)と`gazebo`との連携に最低限必要な処理が実装されるべきプラグインで，`gazebo`の初期化時に読み込まれます．右側にある`ModelPlugin`はモデルに依存するもので`gazebo`にモデルを登録するときに読み込まれるものです．この右側については，まさに[Controller と HardwareInterface との間の処理の仕組み（2. RobotHWSimのプラグインについて）](http://qiita.com/MoriKen/items/5cab7436c1b36c25e0ce)で扱ったもので，ロボットモデルを管理しているプラグインです．

例えば，`SystemPlugin`は最初に読み込まれるのですが，`ModelPlugin`を読み込むためには`SystemPlugin`が必要である，と言った位置づけなので，その理由が分かります．

`gazebo`は，`ROS`とは独立して稼働するシミュレータとなっているので，`ROS`の仕組みとつなげるために，このようなプラグインのフレームワークが開放されています．

# `ROS`からの`gazebo`の起動
まずは`SystemPlugin`から見てみます．`ModelPlugin`である`GazeboRosControlPlugin`が気になったことがきっかけなのに`SystemPlugin`から見ていくことを邪道と言われるかもしれませんが，時系列もいいではないですか．`SystemPlugin`が準備できていないと`ModelPlugin`も読めないわけですし．

というわけで（？），`gazebo`の起動から見ていくと面白いので，頭からトレースしていきます．

例えば，`roslaucn`で`gazebo`の`empty_world`を起動するときには，下記のファイルが呼び出されます．
[ros-simulation/gazebo_ros_pkgs/gazebo_ros/launch/empty_world.launch](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/indigo-devel/gazebo_ros/launch/empty_world.launch)

```xml:empty_world.launch
<launch>
  <!-- 省略 -->
  <arg unless="$(arg debug)" name="script_type" value="gzserver"/>
  <!-- 省略 -->

  <!-- start gazebo server-->
  <node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="false" output="screen"
	args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)" />
	
  <!-- 省略 -->
</launch>
```

この`launch`ファイルをデフォルト設定で起動すれば，`gzserver`というモジュールが呼ばれるようです．`gzserver`は`gazebo`による物理シミュレーションを司るエンジン部分です．GUIのモジュールは上記とは切り離されていて，`gzclient`から呼び出されるようです．[本家サイト](http://gazebosim.org/tutorials?tut=quick_start#Clientandserverseparation)にこの辺りの説明があります．

さぁ，この人が起動された後，どうプラグインを読みに行くのでしょうか．見てみます．
[ros-simulation/gazebo_ros_pkgs/gazebo_ros/scripts/gzserver
](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/indigo-devel/gazebo_ros/scripts/gzserver)

```bash:gzserver
#!/bin/sh
final="$@"

EXT=so
if [ $(uname) = "Darwin" ]; then
    EXT=dylib
fi

# add ros path plugin if it does not already exist in the passed in arguments
if [ `expr "$final" : ".*libgazebo_ros_paths_plugin\.$EXT.*"` -eq 0 ]
then
    final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`"
fi

# add ros api plugin if it does not already exist in the passed in arguments
if [ `expr "$final" : ".*libgazebo_ros_api_plugin\.$EXT.*"` -eq 0 ]
then
    final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`"
fi

setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/
. $setup_path/setup.sh && gzserver $final
```

`catkin_find`で，引数で指定したリソースが存在するディレクトリを取得し，`gzserver`の引数としています．これで，`gazebo`が`libgazebo_ros_paths_plugin.so`と`libgazebo_ros_api_plugin.so`を読み込んでくれます．

`gzserver`実体がどこにあるのか確認すると，もう`gazebo`のソースコードに移ります．`gazebo`のコードの最新版は[bitbacket](https://bitbucket.org/osrf/gazebo/overview)で管理されているようです．
[gazebo/gazebo/CMakeLists.txt](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/CMakeLists.txt?at=default&fileviewer=file-view-default)

```cmake:CMakeLists.txt
# 省略
gz_add_executable(gzserver server_main.cc)
# 省略
```

[gazebo/gazebo/server_main.cc](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/server_main.cc?at=default&fileviewer=file-view-default) を見てみます．

```cpp:server_main.cc
// 省略
int main(int argc, char **argv)
{
  gazebo::Server *server = NULL;

  try
  {
    // Initialize the informational logger. This will log warnings, and
    // errors.
    gzLogInit("server-", "gzserver.log");

    // Initialize the data logger. This will log state information.
    gazebo::util::LogRecord::Instance()->Init("gzserver");

    server = new gazebo::Server();
    if (!server->ParseArgs(argc, argv))
      return -1;

    server->Run();
    server->Fini();

    delete server;
  }
  catch(gazebo::common::Exception &_e)
  {
    _e.Print();

    server->Fini();
    delete server;
    return -1;
  }

  return 0;
}
```

`ParseArgs()`が匂います．受け取った引数には，プラグインのパスが格納されているので，怪しさ満点です．`Sever`の実体を見てみます．

[gazebo/gazebo/Server.cc](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/Server.cc?at=default&fileviewer=file-view-default)

```cpp:Server.cc
// 省略
bool Server::ParseArgs(int _argc, char **_argv)
{
  // 省略
  try
  {
    po::store(po::command_line_parser(_argc, _argv).options(desc).positional(
          positionalDesc).allow_unregistered().run(), this->dataPtr->vm);

    po::notify(this->dataPtr->vm);
  }
  // 省略
  /// Load all the plugins specified on the command line
  if (this->dataPtr->vm.count("server-plugin"))
  {
    std::vector<std::string> pp =
      this->dataPtr->vm["server-plugin"].as<std::vector<std::string> >();

    for (std::vector<std::string>::iterator iter = pp.begin();
         iter != pp.end(); ++iter)
    {
      gazebo::addPlugin(*iter);
    }
  }
  // 省略
  if (!this->PreLoad())
  {
    gzerr << "Unable to load gazebo\n";
    return false;
  }
  // 省略
}
// 省略
```

ざっくりとした理解ですが，下記のような流れです．

- `gazebo::addPlugin()`
 - 引数で与えられたプラグインを片っ端から登録していく．
- `this->PreLoad()`
 - 登録されたプラグインを読み込む．

これでほぼ分かったようなものですが，両者をもう少しだけ追っていくことにします．

# `gazebo::addPlugin()`を追う
[gazebo/gazebo/gazebo.cc](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/gazebo.cc?at=default&fileviewer=file-view-default)

```cpp:gazebo.cc
//省略
void gazebo::addPlugin(const std::string &_filename)
{
  gazebo_shared::addPlugin(_filename, g_plugins);
}
//省略
```

プラグインが実際に登録されるのは，更に内部の`gazebo_shared`のメソッドが働いているようです．プラグインは`gazebo`単体のクラスとは切り離して管理をしようとしている設計思想が垣間見れます．
[gazebo/gazebo/gazebo_shared.cc](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/gazebo_shared.cc?at=default&fileviewer=file-view-default)

```cpp:gazebo_shared.cc
// 省略
void gazebo_shared::addPlugin(const std::string &_filename,
    std::vector<gazebo::SystemPluginPtr> &_plugins)
{
  if (_filename.empty())
    return;

  gazebo::SystemPluginPtr plugin =
    gazebo::SystemPlugin::Create(_filename, _filename);

  if (plugin)
  {
    if (plugin->GetType() != gazebo::SYSTEM_PLUGIN)
    {
      gzerr << "System is attempting to load "
        << "a plugin, but detected an incorrect plugin type. "
        << "Plugin filename[" << _filename << "].\n";
      return;
    }
    _plugins.push_back(plugin);
  }
}
// 省略
```

`g_plugins`というグローバル配列にプラグインを登録していることが確認できました．

# `this->PreLoad()`を追う

再び[gazebo/gazebo/Server.cc](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/Server.cc?at=default&fileviewer=file-view-default)を追ってみます．

```cpp:Server.cc
// 省略
bool Server::PreLoad()
{
  // setup gazebo
  return gazebo::setupServer(this->dataPtr->systemPluginsArgc,
                             this->dataPtr->systemPluginsArgv);
}
// 省略
```

お次は，`gazebo::serupServer()`です．
[gazebo/gazebo/gazebo.cc](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/gazebo.cc?at=default&fileviewer=file-view-default)

```cpp:gazebo.cc
// 省略
bool gazebo::setupServer(int _argc, char **_argv)
{
// 省略
  if (!gazebo_shared::setup("server-", _argc, _argv, g_plugins))
  {
    gzerr << "Unable to setup Gazebo\n";
    return false;
  }
// 省略
}
// 省略
```

`gazebo_shared::setup()`が臭いです．行きましょう．
[gazebo/gazebo/gazebo_shared.cc](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/gazebo_shared.cc?at=default&fileviewer=file-view-default)

```cpp:gazebo_shared.cc
// 省略
bool gazebo_shared::setup(const std::string &_prefix, int _argc, char **_argv,
    std::vector<gazebo::SystemPluginPtr> &_plugins)
{
// 省略
  // Load all the system plugins
  for (std::vector<gazebo::SystemPluginPtr>::iterator iter =
       _plugins.begin(); iter != _plugins.end(); ++iter)
  {
    (*iter)->Load(_argc, _argv);
  }

// 省略
}
```

プラグインが格納されている`g_plugins`のイタレータで`Load()`メソッドが呼ばれていることが分かります．`gazebo::SystemPluginPtr`型のイタレータのようです．この型について一応確認しておきます．
[gazebo/gazebo/common/CommonTypes.hh](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/common/CommonTypes.hh?at=default&fileviewer=file-view-default)

```cpp:CommonTypes.hh
// 省略
namespace gazebo
{
// 省略
  /// \def SystemPluginPtr
  /// \brief boost::shared_ptr to SystemPlugin
  typedef boost::shared_ptr<SystemPlugin> SystemPluginPtr;
// 省略
}
```

名前の通り，`SystemPlugin`型へのポインタです．この人の在り処を見てみます．
[gazebo/gazebo/common/Plugin.hh](https://bitbucket.org/osrf/gazebo/src/6d74980f0de7948c3af15b72fca84d0f8d7f60aa/gazebo/common/Plugin.hh?at=default&fileviewer=file-view-default)

```cpp:Plugin.hh
// 省略
  class SystemPlugin : public PluginT<SystemPlugin>
  {
    /// \brief Constructor
    public: SystemPlugin()
             {this->type = SYSTEM_PLUGIN;}

    /// \brief Destructor
    public: virtual ~SystemPlugin() {}

    /// \brief Load function
    ///
    /// Called before Gazebo is loaded. Must not block.
    /// \param _argc Number of command line arguments.
    /// \param _argv Array of command line arguments.
    public: virtual void Load(int _argc = 0, char **_argv = NULL) = 0;

    /// \brief Initialize the plugin
    ///
    /// Called after Gazebo has been loaded. Must not block.
    public: virtual void Init() {}

    /// \brief Override this method for custom plugin reset behavior.
    public: virtual void Reset() {}
  };
// 省略
```

やはり，`Load()`だけは純粋仮想メソッドとなっており，実装が強制されているようです．

念のため，`libgazebo_ros_paths_plugin.so`と`libgazebo_ros_api_plugin.so`のプラグインの型を調べておきます．
[gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_paths_plugin.cpp](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/jade-devel/gazebo_ros/src/gazebo_ros_paths_plugin.cpp)

```cpp:gazebo_ros_paths_plugin.cpp
namespace gazebo
{
// 省略
class GazeboRosPathsPlugin : public SystemPlugin
{
// 省略
 void Load(int argc, char** argv)
  {
  }
// 省略
}
// Register this plugin with the simulator
GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosPathsPlugin)
}
```
やはり，`SystemPlugin`を継承しています．こちらは，`Load()`には処理の実体がなく，コンストラクタで初期化するような実装となっていました．

[gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_api_plugin.h](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/jade-devel/gazebo_ros/src/gazebo_ros_api_plugin.h)

```cpp:gazebo_ros_api_plugin.h
/// \brief A plugin loaded within the gzserver on startup.
class GazeboRosApiPlugin : public SystemPlugin
{
// 省略
  void Load(int argc, char** argv);
// 省略
}
```
こちらも`SystemPlugin`を継承しています．このクラスでは，オーバライドされた`Load()`で処理が記述されていました．

これで，いずれのプラグインもちゃんと読み込まれていることが確認できました．

# おわりに
今回は，`ROS`に関わる`gazebo`の`SystemPlugin`の読込みに関してトレースしました．
次は，`ModelPlugin`の読込みをトレースします．