目次

Gazebo起動時のSystemPluginの読込み
ROSからのgazeboの起動
gazebo::addPlugin()を追う
this->PreLoad()を追う
ロボットモデル登録時のModelPluginの読込み　←　イマココ
gazebo_ros_pkgsのspawn_modelを追う
spawn_urdf_modelのサービスサーバ
gazebo::physics::Worldを追う
"entity_info"について

はじめに

次は，ModelPluginの読込みです．下記図の右側です．

ROSのlaunchファイルから，gazebo_ros_pkgsのspawn_modelを呼ぶことがトリガーとなっているのは想像が付きます．と言うことで，スタートはここです．

初めに申し上げておくと，今回のトレースは不完全です．gazeboのコードは非常に細かく構造化されていて，私の力量では全てを読解することが出来ませんでしたorz.
まぁ，全てを読解して記述が複雑になったところで誰得か分からなくなるという問題もありますし，そもそもの目的はざっくりとでもgazeboがどうやってModelPluginを読み込んでいるのかが分かることだったはずです．

という言い訳をしておき，トレースのスタートです．

`gazebo_ros_pkgs`の`spawn_model`を追う

起動スクリプト

gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model

spawn_model

# 省略
if __name__ == "__main__":
    if len(sys.argv) < 2:
        print usage()
    else:
        print("spawn_model script started") # make this a print incase roscore has not been started
        sm = SpawnModel()
        sm.parseUserInputs()
        sm.callSpawnService()

引数をパースして，Spawnのサービスを呼んでいそうなのが分かります．callSpawnService()を見てみます．

spawn_model

# 省略
    def callSpawnService(self):
# 省略（パラメータを読みこんだり，初期位置を設定したり）
        # spawn model
        if self.urdf_format:
          success = gazebo_interface.spawn_urdf_model_client(self.model_name, model_xml, self.robot_namespace, 
                                                             initial_pose, self.reference_frame, self.gazebo_namespace)
# 省略
        return
# 省略

実際にサービスコールしているのはgazebo_interface.spawn_urdf_model_client()のようです．

gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py

gazebo_interface.py

# 省略
def spawn_urdf_model_client(model_name, model_xml, robot_namespace, initial_pose, reference_frame, gazebo_namespace):
    rospy.loginfo("Waiting for service %s/spawn_urdf_model"%gazebo_namespace)
    rospy.wait_for_service(gazebo_namespace+'/spawn_urdf_model')
    try:
      spawn_urdf_model = rospy.ServiceProxy(gazebo_namespace+'/spawn_urdf_model', SpawnModel)
      rospy.loginfo("Calling service %s/spawn_urdf_model"%gazebo_namespace)
      resp = spawn_urdf_model(model_name, model_xml, robot_namespace, initial_pose, reference_frame)
      rospy.loginfo("Spawn status: %s"%resp.status_message)
      return resp.success
# 省略

spawn_urdf_modelという名称のサービスをコールしています．この正体が気になるところです．

`spawn_urdf_model`のサービスサーバ

既にROSのサービスサーバを立ち上げてくれている人が誰かを考えると，GazeboRosApiPluginが怪しい予感がしますね．エントリポイントから見てみます．

gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_api_plugin.cpp

gazebo_ros_api_plugin.cpp

// 省略
void GazeboRosApiPlugin::Load(int argc, char** argv)
{
// 省略
  // below needs the world to be created first
  load_gazebo_ros_api_plugin_event_ = gazebo::event::Events::ConnectWorldCreated(boost::bind(&GazeboRosApiPlugin::loadGazeboRosApiPlugin,this,_1));

  plugin_loaded_ = true;
  ROS_INFO("Finished loading Gazebo ROS API Plugin.");
}
// 省略

ワールド生成時にloadGazeboRosApiPlugin()を呼ぶようイベントハンドラを設定しています．

gazebo_ros_api_plugin.cpp

// 省略
void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
{
// 省略
  /// \brief advertise all services
  advertiseServices();
// 省略
}

いかにもサービスサーバを立ち上げていそうなadvertiseServices()というメソッドが出てきました．

gazebo_ros_api_plugin.cpp

void GazeboRosApiPlugin::advertiseServices()
{
// 省略
  // Advertise spawn services on the custom queue
  std::string spawn_urdf_model_service_name("spawn_urdf_model");
  ros::AdvertiseServiceOptions spawn_urdf_model_aso =
    ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
                                                                  spawn_urdf_model_service_name,
                                                                  boost::bind(&GazeboRosApiPlugin::spawnURDFModel,this,_1,_2),
                                                                  ros::VoidPtr(), &gazebo_queue_);
  spawn_urdf_model_service_ = nh_->advertiseService(spawn_urdf_model_aso);
// 省略

// 省略}

spawn_urdf_modelのサービスを起動しています．サービスコールバックの実体であるGazeboRosApiPlugin::spawnURDFModel()を見てみましょう．

gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_api_plugin.cpp

gazebo_ros_api_plugin.cpp

// 省略
bool GazeboRosApiPlugin::spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,
                                        gazebo_msgs::SpawnModel::Response &res)
{
  // get name space for the corresponding model plugins
  robot_namespace_ = req.robot_namespace;

  // incoming robot model string
  std::string model_xml = req.model_xml;

// 省略（変換処理）

  req.model_xml = model_xml;

  // Model is now considered convert to SDF
  return spawnSDFModel(req,res);
}
// 省略

URDFをSDFに変換して，spawnSDFModel()を呼んでいます．

gazebo_ros_api_plugin.cpp

bool GazeboRosApiPlugin::spawnSDFModel(gazebo_msgs::SpawnModel::Request &req,
                                       gazebo_msgs::SpawnModel::Response &res)
{
// 省略
  // incoming robot model string
  std::string model_xml = req.model_xml;
// 省略
  TiXmlDocument gazebo_model_xml;
  gazebo_model_xml.Parse(model_xml.c_str());
// 省略
  // do spawning check if spawn worked, return response
  return spawnAndConform(gazebo_model_xml, model_name, res);
}

spawnAndConform()で，ようやくspawnしてくれそうです．

gazebo_ros_api_plugin.cpp

// 省略
bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::string model_name,
                                         gazebo_msgs::SpawnModel::Response &res)
{
// 省略
  // publish to factory topic
  gazebo::msgs::Factory msg;
  gazebo::msgs::Init(msg, "spawn_model");
  msg.set_sdf( gazebo_model_xml_string );
// 省略
  // looking for Model to see if it exists already
  gazebo::msgs::Request *entity_info_msg = gazebo::msgs::CreateRequest("entity_info", model_name);
  request_pub_->Publish(*entity_info_msg,true);
// 省略
}
// 省略

なにやらentity_infoなるトピックメッセージを作成して，Publishしています．ここで，メッセージの型がgazebo::msgs::Factoryであることを，頭の片隅に入れておきましょう．

このPublisherの型を確認しましょう．

gazebo_ros_api_plugin.h

// 省略
  gazebo::transport::PublisherPtr request_pub_;
// 省略

宛先はgazeboのようです．ようやくROSとgazeboのインターフェースにたどり着きました．
ですが，この先の方が長いです．

`gazebo::transport::Publisher::Publish()`を追う

以降，ひたすらgazeboを追っていくことになります．名前から想像が付きそうですが，gazebo::transport::PublisherPtrの正体を見てみます．

gazebo/gazebo/transport/TransportTypes.hh

TransportTypes.hh

// 省略
namespace gazebo
{
  namespace transport
  {
// 省略
    class Publisher;
// 省略
    typedef boost::shared_ptr<Publisher> PublisherPtr;
// 省略
  }
// 省略
}

gazebo::transport::Publisherへのポインタです．先ほどGazeboRosApiPluginで呼ばれていたPublish()メソッドはここにあるはずです．

gazebo/gazebo/transport/Publisher.hh

Publisher.hh

// 省略
      public: void Publish(const google::protobuf::Message &_message,
                 bool _block = false)
              { this->PublishImpl(_message, _block); }
// 省略

実装はPublishImpl()メソッドでなされているようです．

gazebo/gazebo/transport/Publisher.cpp

Publisher.cpp

void Publisher::PublishImpl(const google::protobuf::Message &_message,
                            bool _block)
{
//省略
  // Save the latest message
  MessagePtr msgPtr(_message.New());
  msgPtr->CopyFrom(_message);

  this->publication->SetPrevMsg(this->id, msgPtr);

//省略
  TopicManager::Instance()->AddNodeToProcess(this->node);

  if (_block)
  {
    this->SendMessage();
  }
  else
  {
    // Tell the connection manager that it needs to update
    ConnectionManager::Instance()->TriggerUpdate();
  }
}

引数で指定された_messageがthis->publicationに登録されています．これに対して何かアクションがあるはずです．
引数_blockはtrueでしたので，SendMessage()を確認します．

Publisher.cpp

void Publisher::SendMessage()
{
// 省略
    // Send all the current messages
    for (std::list<MessagePtr>::iterator iter = localBuffer.begin();
        iter != localBuffer.end(); ++iter, ++pubIter)
    {
      // Send the latest message.
      int result = this->publication->Publish(*iter,
          boost::bind(&Publisher::OnPublishComplete, this, _1), *pubIter);
// 省略
    }
// 省略
  }
}
// 省略

やはり，this->publicationは先ほど登録されたメッセージをPublish()しています．
一応，publicationの型を確認しておきましょう．

gazebo/gazebo/transport/Publisher.hh

Publisher.hh

// 省略
      private: PublicationPtr publication;
// 省略

PublicationPtr型の宣言を確認します．

gazebo/gazebo/transport/TransportTypes.hh

TransportTypes.hh

// 省略
    typedef boost::shared_ptr<Publication> PublicationPtr;
// 省略

その名の通り，Publicationのポインタ型です．

それでは，肝心のPublication::Publish()の実装を見てみます．

gazebo/gazebo/transport/Publication.cc

Publication.cc

// 省略
int Publication::Publish(MessagePtr _msg, boost::function<void(uint32_t)> _cb,
    uint32_t _id)
{
// 省略
  std::list<NodePtr>::iterator iter, endIter;
// 省略
    iter = this->nodes.begin();
    endIter = this->nodes.end();
    while (iter != endIter)
    {
      if ((*iter)->HandleMessage(this->topic, _msg))
        ++iter;
      else
        this->nodes.erase(iter++);
    }
  }
// 省略

(*iter)->HandleMessage(this->topic, _msg)がミソのようです．nodesの詳細は把握していないのですが，メッセージを送信する対象となるオブジェクトの単位を指していると思われます．
それでは，NodePtrの型の確認と，HandleMessage()の解析をします．

gazebo/gazebo/transport/TransportTypes.hh

TransportTypes.hh

// 省略
   typedef boost::shared_ptr<Node> NodePtr;
// 省略

例によって，Nodeのポインタ型であることが確認されました．気になる定義はNode::HandleMessage()に記述されているはずです．

gazebo/gazebo/transport/Node.cc

Node.cc

// 省略
bool Node::HandleMessage(const std::string &_topic, MessagePtr _msg)
{
  boost::recursive_mutex::scoped_lock lock(this->incomingMutex);
  this->incomingMsgsLocal[_topic].push_back(_msg);
  ConnectionManager::Instance()->TriggerUpdate();
  return true;
}
// 省略

メッセージを配列に格納して，TriggerUpdate()でコールバックを呼んでいるようです．
ここからは本来，コールバックを丁寧に追っていってどんな処理がなされているかを確認する必要があるのですが，私の力量ではそこに達することが出来ませんでした.とりあえず，状態を更新するための処理が行われるのだろう，という想定をして先に進みます．

というわけで，これ以降はそれらしい処理が存在する，physics/World側に飛びます．

`gazebo::physics::World`を追う

ざっとですがコードを眺めると，Worldクラスが物理シミュレーションやメッセージの管理をしているようでしたので，ここを深入りしてみます．
また，頭から行ってみます．

gazebo/gazebo/gazebo_main.cc

server_main.cc

// 省略
int main(int argc, char **argv)
{
  gazebo::Server *server = NULL;
// 省略
    server = new gazebo::Server();
    if (!server->ParseArgs(argc, argv))
      return -1;
// 省略
    server->Run();
// 省略
}

server->ParseArgs()は前回見ましたので，ここではserver->Run()を見てみます．

gazebo/gazebo/Server.cc

Server.cc

// 省略
void Server::Run()
{
// 省略
  // Run each world. Each world starts a new thread
  physics::run_worlds(iterations);
// 省略
}
// 省略

サーバから，物理演算を司るWorldを起動しているようです．

gazebo/gazebo/physics/PhysicsIface.cc

PhysicsIface.cc

// 省略
void physics::run_worlds(unsigned int _steps)
{
  for (auto &world : g_worlds)
    world->Run(_steps);
}
// 省略

PhysicsIface.ccはServerとphysicsとのインターフェースの役割を担っています．これで，world->Run()が起動されます．いかにも物理シミュレーションを始めてくれそうな（？）名前です．

gazebo/gazebo/physics/World.cc

World.cc

// 省略
void World::Run(unsigned int _iterations)
{
  this->dataPtr->stop = false;
  this->dataPtr->stopIterations = _iterations;

  this->dataPtr->thread = new boost::thread(boost::bind(&World::RunLoop, this));
}
// 省略

World::RunLoop()を実行するスレッドを立ち上げています．

World.cc

// 省略
void World::RunLoop()
{
// 省略
  if (!util::LogPlay::Instance()->IsOpen())
  {
    for (this->dataPtr->iterations = 0; !this->dataPtr->stop &&
        (!this->dataPtr->stopIterations ||
         (this->dataPtr->iterations < this->dataPtr->stopIterations));)
    {
      this->Step();
    }
  }
// 省略
}
// 省略

ループで実行されているStep()が怪しいです．

World.cc

// 省略
void World::Step()
{
// 省略
  this->ProcessMessages();
// 省略
}
// 省略

this->ProcessMessages()という，これまで受け取ったメッセージを処理してくれそうなメソッドが現れました．

gazebo/gazebo/physics/World.cc

World.cc

// 省略
void World::ProcessMessages()
{
// 省略
  if (common::Time::GetWallTime() - this->dataPtr->prevProcessMsgsTime >
      this->dataPtr->processMsgsPeriod)
  {
    this->ProcessEntityMsgs();
    this->ProcessRequestMsgs();
    this->ProcessFactoryMsgs();
    this->ProcessModelMsgs();
    this->ProcessLightFactoryMsgs();
    this->ProcessLightModifyMsgs();
    this->dataPtr->prevProcessMsgsTime = common::Time::GetWallTime();
  }
// 省略
}

さて，様々な種類のメッセージを処理しているようです．ここでおさらいをします．gazebo_ros_api_plugin.cppで作成したメッセージはどんなものだったでしょう．

gazebo_ros_api_plugin.cpp(再掲)

// 省略
bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::string model_name,
                                         gazebo_msgs::SpawnModel::Response &res)
{
// 省略
  // publish to factory topic
  gazebo::msgs::Factory msg;
  gazebo::msgs::Init(msg, "spawn_model");
  msg.set_sdf( gazebo_model_xml_string );
// 省略
  // looking for Model to see if it exists already
  gazebo::msgs::Request *entity_info_msg = gazebo::msgs::CreateRequest("entity_info", model_name);
  request_pub_->Publish(*entity_info_msg,true);
// 省略
}
// 省略

gazebo::msgs::Factory型のメッセージを発行していたことを，頭の片隅に入れていたと思います．

Worldのインスタンスには，factorySubという名前のサブスクライバがいて，Factory型のメッセージを受け取った時にWorld::OnFactoryMsg()というコールバックメソッドが呼ばれるように初期化がされています．参考として，OnFactoryMsg()の処理を覗いてみます．

gazebo/gazebo/physics/World.cc

World.cc

// 省略
void World::OnFactoryMsg(ConstFactoryPtr &_msg)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  this->dataPtr->factoryMsgs.push_back(*_msg);

// 省略

やはり，factoryMsgsというメッセージキューにメッセージを蓄えているようです．これらのメッセージは，自分が処理されるのを心待ちにしている状態なわけです．

ということまで理解して，ようやくfactoryMsgsを処理してくれそうな名前を持つ，this->ProcessFactoryMsgs()にたどり着きます．

World.cc

// 省略
void World::ProcessFactoryMsgs()
{
  std::list<sdf::ElementPtr> modelsToLoad, lightsToLoad;

  {
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
    for (auto const &factoryMsg : this->dataPtr->factoryMsgs)
    {
// 省略
      if (factoryMsg.has_edit_name())
      {
// 省略
      }
      else
      {
// 省略
        sdf::ElementPtr elem = this->dataPtr->factorySDF->Root()->Clone();
// 省略
        if (elem->HasElement("model"))
        {
          elem = elem->GetElement("model");
          isModel = true;
        }
// 省略
        else if (isModel)
        {
          modelsToLoad.push_back(elem);
        }
// 省略
      }
    }

    this->dataPtr->factoryMsgs.clear();
  }

  // Load models
  for (auto const &elem : modelsToLoad)
  {
    try
    {
      boost::mutex::scoped_lock lock(this->dataPtr->factoryDeleteMutex);

      ModelPtr model = this->LoadModel(elem, this->dataPtr->rootElement);
      model->Init();
      model->LoadPlugins();
    }
// 省略
  }
// 省略
}
// 省略

メッセージを解析して，読み込むモデルを指定しています．そして念願(?)のmodel->LoadPlugins()の登場です．ModelPluginを読み込んでくれそうな予感がします．

gazebo/gazebo/physics/Model.cc

Model.cc

// 省略
void Model::LoadPlugins()
{
// 省略
    if (iterations < 50)
    {
      // Load the plugins
      sdf::ElementPtr pluginElem = this->sdf->GetElement("plugin");
      while (pluginElem)
      {
        this->LoadPlugin(pluginElem);
        pluginElem = pluginElem->GetNextElement("plugin");
      }
    }
// 省略
}
// 省略

LoadPlugins()はモデルに関わる全てのプラグインを読み込むメソッドですが，内部では個別のプラグインを読み込むthis->LoadPlugin()がループ実行されています．

Model.cc

// 省略
void Model::LoadPlugin(sdf::ElementPtr _sdf)
{
// 省略
  gazebo::ModelPluginPtr plugin;

  try
  {
    plugin = gazebo::ModelPlugin::Create(filename, pluginName);
  }
// 省略

  if (plugin)
  {
// 省略
    ModelPtr myself = boost::static_pointer_cast<Model>(shared_from_this());
    try
    {
      plugin->Load(myself, _sdf);
    }
// 省略
    try
    {
      plugin->Init();
    }
// 省略
    this->plugins.push_back(plugin);
  }
}
// 省略

ようやっと，やってきました．喉から手が出るほど欲しかった，plugin->Load()です．pluginメンバの型であるModelPluginPtrは．例によってModelPluginのポインタです．宣言を確認します．

gazebo/gazebo/common/Plugin.hh

Plugin.hh

// 省略
  class ModelPlugin : public PluginT<ModelPlugin>
  {
    /// \brief Constructor
    public: ModelPlugin()
             {this->type = MODEL_PLUGIN;}

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

    /// \brief Load function
    ///
    /// Called when a Plugin is first created, and after the World has been
    /// loaded. This function should not be blocking.
    /// \param[in] _model Pointer to the Model
    /// \param[in] _sdf Pointer to the SDF element of the plugin.
    public: virtual void Load(physics::ModelPtr _model,
                              sdf::ElementPtr _sdf) = 0;

    /// \brief Override this method for custom plugin initialization behavior.
    public: virtual void Init() {}

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

Load()メソッドが純粋仮想メソッドで宣言されています．ModelPluginのサブクラスでは，これのオーバーライドが強制されます．
正に，gazebo_ros_control::GazeboRosControlPluginはModelPluginのサブクラスです．そして，このLoad()がプラグインのエントリポイントであることは，以前に投稿したController と HardwareInterface との間の処理の仕組み（2. RobotHWSimのプラグインについて）に記載された通りです．

これこそが，gazeboからROSのプラグインを呼び出すポイントとなっています．ようやくすっきりしました^^

`"entity_info"`について

ちょっと気になることがあります．さらっと見るだけですが．
gazebo_ros_apiからトピックメッセージを作成するとき，"entity_info"という名称をつけたと思いますが，これはどこかで回収されているのかな，と思い検索したら，World::ProcessRequestMsgs()が引っかかりました．

gazebo/gazebo/physics/World.cc

World.cc

// 省略
void World::ProcessRequestMsgs()
{
// 省略
  for (auto const &requestMsg : this->dataPtr->requestMsgs)
  {
// 省略
    else if (requestMsg.request() == "entity_info")
    {
      BasePtr entity = this->dataPtr->rootElement->GetByName(requestMsg.data());
      if (entity)
      {
        if (entity->HasType(Base::MODEL))
        {
          msgs::Model modelMsg;
          ModelPtr model = boost::dynamic_pointer_cast<Model>(entity);
          model->FillMsg(modelMsg);

          std::string *serializedData = response.mutable_serialized_data();
          modelMsg.SerializeToString(serializedData);
          response.set_type(modelMsg.GetTypeName());
        }
// 省略
      }
// 省略
    }
// 省略
    if (send)
    {
      this->dataPtr->responsePub->Publish(response);
    }
  }
  this->dataPtr->requestMsgs.clear();
}
// 省略

ROS側で定義したモデルの構成情報を，gazebo側のモデルに反映させるための処理をしているように見えます．この先までトレースをしても詳細過ぎて ~~心が折れそう~~ 本エントリの主題から逸れてしまうので，今回はここまでです．

ちなみに，this->dataPtr->responsePub->Publish(response)で発行されたメッセージは，gui側で購読しているようです．この辺りの思想は，ROSのトピックと同等のようです．

終わりに

gazeboからROSのプラグインを呼ぶフローを，なんとかつかむことができました．想定したよりもハマりましたが^^;
大規模なソフトウェアをオブジェクト指向で設計するこういう作り方になるんだ！と言う発見が色々あって，とても勉強になりました．

これを知ったところで，ROSアプリケーションの開発には全然影響はしないのですが^^;
まぁ，こんな風に無駄にトレースをしている記事は見かけないので，ニッチな世界を開拓しようかなとw
少数でも，誰かの眼に止まって，知見を共有できることをは喜ばしいことですし，私自身の中にあったモヤモヤも晴れたし，良し（？）ということにします．