はじめに
さて，先日小生が投稿した記事であるController と HardwareInterface との間の処理の仕組み（2. RobotHWSimのプラグインについて）に対して頂いたご意見の中に，
「よく読むとGazeboからプラグインを呼んでいることが分かる」
という趣旨のコメントがありました．
これを見た瞬間，
「むむ，たしかに，GazeboくんからGazeboRosControlPluginってどうやって呼んでいるのか全く気にしなかったわけではないのだが，こっそり棚上げにしていたのがバレた．かたじけない．」
という思いがこみ上げ，結局調べてみたので，忘れる前にまとめることにしました．
ただ，実際調べてみると，GazeboRosApiPluginやGazeboRosApiPluginなるプラグインも読み込まれているようで，この際だから両方流れだけでも追っておこうと考え，まとめることにしました．
目次
- Gazebo起動時のSystemPluginの読込み ← イマココ
-
ROSからのgazeboの起動 -
gazebo::addPlugin()を追う -
this->PreLoad()を追う - ロボットモデル登録時のModelPluginの読込み
-
gazebo_ros_pkgsのspawn_modelを追う -
spawn_urdf_modelのサービスサーバ -
gazebo::physics::Worldを追う -
"entity_info"について
クラス構成
今回扱うROSのプラグイン周りのクラス図です．
図の上側がgazeboのプロジェクトに属し，下側がgazebo_ros_pkgsのプロジェクトに属します．
同図の左側にあるSystemPluginは外部(今回ならROS)とgazeboとの連携に最低限必要な処理が実装されるべきプラグインで，gazeboの初期化時に読み込まれます．右側にあるModelPluginはモデルに依存するものでgazeboにモデルを登録するときに読み込まれるものです．この右側については，まさにController と HardwareInterface との間の処理の仕組み（2. RobotHWSimのプラグインについて）で扱ったもので，ロボットモデルを管理しているプラグインです．
例えば，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
<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から呼び出されるようです．本家サイトにこの辺りの説明があります．
さぁ，この人が起動された後，どうプラグインを読みに行くのでしょうか．見てみます．
ros-simulation/gazebo_ros_pkgs/gazebo_ros/scripts/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で管理されているようです．
gazebo/gazebo/CMakeLists.txt
# 省略
gz_add_executable(gzserver server_main.cc)
# 省略
gazebo/gazebo/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の実体を見てみます．
// 省略
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()を追う
//省略
void gazebo::addPlugin(const std::string &_filename)
{
gazebo_shared::addPlugin(_filename, g_plugins);
}
//省略
プラグインが実際に登録されるのは，更に内部のgazebo_sharedのメソッドが働いているようです．プラグインはgazebo単体のクラスとは切り離して管理をしようとしている設計思想が垣間見れます．
gazebo/gazebo/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を追ってみます．
// 省略
bool Server::PreLoad()
{
// setup gazebo
return gazebo::setupServer(this->dataPtr->systemPluginsArgc,
this->dataPtr->systemPluginsArgv);
}
// 省略
お次は，gazebo::serupServer()です．
gazebo/gazebo/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
// 省略
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
// 省略
namespace gazebo
{
// 省略
/// \def SystemPluginPtr
/// \brief boost::shared_ptr to SystemPlugin
typedef boost::shared_ptr<SystemPlugin> SystemPluginPtr;
// 省略
}
名前の通り，SystemPlugin型へのポインタです．この人の在り処を見てみます．
gazebo/gazebo/common/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
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
/// \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の読込みをトレースします．