Controller と HardwareInterface との間の処理の仕組み（2. RobotHWSimのプラグインについて）


# 目次
1. [ロボットモデルの定義と登録](http://qiita.com/MoriKen/items/613635b90f3a98042dc5)
 1. `URDF`でロボットモデルを定義する
 1. `HardwareInterface`の存在意義
 1. `URDF`で`Gazebo`の設定をする
 1. ロボットモデルをパラメータサーバに登録する
1. [`RobotHWSim`のプラグインについて](http://qiita.com/MoriKen/items/5cab7436c1b36c25e0ce)　←　イマココ
 1. プラグインの定義
 1. プラグインで定義された処理が実行される場所
 1. `RobotHWSin`と`RobotHW`との対比
 1. プラグインとして使えるようにする 
 1. `DefaultRobotHWSim`について
1. [`Controller`について](http://qiita.com/MoriKen/items/c29f653d03baffe5f0e2)
 1. `Gazebo`にロボットモデルをSpawnする
 1. `Controller`を起動する
 1. `ControllerManager`と`HardwareInterface`とのマッピング

# お題
- 書籍
 - [Learning ROS for Robotics Programming - Second Edition](http://www.amazon.co.jp/Learning-ROS-Robotics-Programming-Edition-ebook/dp/B00YSIL6VM) `Chapter 10: Manipulation with MoveIt!`<br/>
<img src="https://qiita-image-store.s3.amazonaws.com/0/110138/92e03e5e-dce8-008b-19a2-00ecafd6fbec.jpeg" data-canonical-src="http://www.amazon.co.jp/Learning-ROS-Robotics-Programming-Edition-ebook/dp/B00YSIL6VM" width="20%" />
<br /><br />
- リポジトリ
 - [github: AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition](https://github.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition)

# はじめに
「[1. ロボットモデルの定義と登録](http://qiita.com/MoriKen/items/613635b90f3a98042dc5)」の説明において，`gazebo.urdf.xacro`で`rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo`というカスタムプラグインを指定した箇所がありました．この正体を棚上げにしていたので，ここで理解を深めることにします．

本家サイトの図において，下記ピンクで囲った箇所です．
![gazebo_plugin.png](https://qiita-image-store.s3.amazonaws.com/0/110138/59cb1a64-54cd-410a-7e20-0cfe5ed62f63.png)

本記事用のまとめの図においては，下記の緑で囲った箇所です．
![2.summary.png](https://qiita-image-store.s3.amazonaws.com/0/110138/b5066445-fe50-eede-9977-ee2ebde25198.png)


# プラグインの定義
このカスタムプラグインの実体は，`rosbook_arm_hardware_gazebo`パッケージの中にあり，C++で実装されています．大事なところだけ覗いてみます．

```cpp:rosbook_arm_hardware_gazebo/include/rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo.h
  // 省略
#include <hardware_interface/robot_hw.h>
  // 省略
#include <gazebo_ros_control/robot_hw_sim.h>
  // 省略
namespace rosbook_arm_hardware_gazebo
{

class ROSBookArmHardwareGazebo : public gazebo_ros_control::RobotHWSim
{
public:

  ROSBookArmHardwareGazebo();

  bool initSim(const std::string& robot_namespace,
      ros::NodeHandle model_nh,
      gazebo::physics::ModelPtr parent_model,
      const urdf::Model* const urdf_model,
      std::vector<transmission_interface::TransmissionInfo> transmissions);

  void readSim(ros::Time time, ros::Duration period);

  void writeSim(ros::Time time, ros::Duration period);
  // 省略
private:
  // 省略
  std::vector<double> jnt_pos_;
  std::vector<double> jnt_vel_;
  std::vector<double> jnt_eff_;

  std::vector<double> jnt_pos_cmd_;

  std::vector<gazebo::physics::JointPtr> sim_joints_;

  // Hardware interface: joints
  hardware_interface::JointStateInterface    jnt_state_interface_;
  hardware_interface::PositionJointInterface jnt_pos_cmd_interface_;
  // 省略
};

} // namespace rosbook_arm_hardware_gazebo

#endif // ROSBOOK_ARM_HARDWARE_GAZEBO_H
```
![ROSBookArmHardwareGazebo2.png](https://qiita-image-store.s3.amazonaws.com/0/110138/b2f83ab7-caad-8d8c-e5ec-ec6f6d4fee5c.png)


initSim(), readSim(), writeSim() メソッドが重要です．`HardwareInterface`と`Controller`とのやりとりや，`HardwareInterface`と`Gazebo`とのやりとりを実現する部分です．これらのメソッドは，`gazebo_ros_control::RobotHWSim`クラスで純粋仮想メソッドとして宣言されているので，カスタマイズしたメソッドでオーバーライドしてあげます．定義については，下記のコードで見てみましょう．

```cpp:rosbook_arm_hardware_gazebo/src/rosbook_arm_hardware_gazebo.cpp
  // 省略
#include <rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo.h>

namespace rosbook_arm_hardware_gazebo
{
  using namespace hardware_interface;
  // 省略
  bool ROSBookArmHardwareGazebo::initSim(const std::string& robot_namespace,
      ros::NodeHandle nh,
      gazebo::physics::ModelPtr model,
      const urdf::Model* const urdf_model,
      std::vector<transmission_interface::TransmissionInfo> transmissions)
  {
    using gazebo::physics::JointPtr;

    // Cleanup
    sim_joints_.clear();
    jnt_pos_.clear();
    jnt_vel_.clear();
    jnt_eff_.clear();
    jnt_pos_cmd_.clear();

    // Simulation joints
    sim_joints_ = model->GetJoints();
    n_dof_ = sim_joints_.size();

    std::vector<std::string> jnt_names;
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_names.push_back(sim_joints_[i]->GetName());
    }

    // Raw data
    jnt_pos_.resize(n_dof_);
    jnt_vel_.resize(n_dof_);
    jnt_eff_.resize(n_dof_);
    jnt_pos_cmd_.resize(n_dof_);

    // Hardware interfaces
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_state_interface_.registerHandle(
          JointStateHandle(jnt_names[i], &jnt_pos_[i], &jnt_vel_[i], &jnt_eff_[i]));

      jnt_pos_cmd_interface_.registerHandle(
          JointHandle(jnt_state_interface_.getHandle(jnt_names[i]), &jnt_pos_cmd_[i]));

      ROS_DEBUG_STREAM("Registered joint '" << jnt_names[i] << "' in the PositionJointInterface.");
    }

    registerInterface(&jnt_state_interface_);
    registerInterface(&jnt_pos_cmd_interface_);

  // 省略
    }
  // 省略
    return true;
  }

  void ROSBookArmHardwareGazebo::readSim(ros::Time time, ros::Duration period)
  {
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_pos_[i] += angles::shortest_angular_distance
          (jnt_pos_[i], sim_joints_[i]->GetAngle(0u).Radian());
      jnt_vel_[i] = sim_joints_[i]->GetVelocity(0u);
      jnt_eff_[i] = sim_joints_[i]->GetForce(0u);
    }
  }

  void ROSBookArmHardwareGazebo::writeSim(ros::Time time, ros::Duration period)
  {
    // Enforce joint limits
    jnt_limits_interface_.enforceLimits(period);

    // Compute and send commands
    for (size_t i = 0; i < n_dof_; ++i)
    {
      const double error = jnt_pos_cmd_[i] - jnt_pos_[i];
      const double effort = pids_[i].computeCommand(error, period);

      sim_joints_[i]->SetForce(0u, effort);
    }
  }

} // namespace rosbook_hardware_gazebo

PLUGINLIB_EXPORT_CLASS(rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo, gazebo_ros_control::RobotHWSim)
```

重要な三つのメソッドがオーバーライドされていることが確認できます．概要は下記のとおりです．

- initSim()
 - パラメータサーバに登録されたロボットモデルから，joint 名を取得します．
 - 取得したjoint 名で`JointState`のハンドルを作成し，`Hardware Resource Interface`をインターフェースとして登録します．状態の読み出しに使うのが`hardware_interface::JointStateInterface jnt_state_interface_`で，指令に書き込みに使うのが`hardware_interface::PositionJointInterface jnt_pos_cmd_interface_`です．（これらはいずれも`hardware_interface::HardwareResourceManager`クラスのサブクラスのインスタンスです．）
 - 後々`Controller`が`HardwareInterface`を介してやりとりできるように，`JointStateInterface`や`PositionJointInterface`のハンドル作成時に，joint にまつわるメンバ変数（`jnt_pos_`, `jnt_vel_`, `jnt_eff_`, `jnt_pos_cmd_`）のポインタを登録しています．
- readSim()
 - 現在の`Gazebo`におけるロボットモデルのjoint の状態（位置，速度，トルク）を取得して，先ほどinitSim()で渡したメンバ変数を介して`HardwareInterface`に取得した情報を教えてあげます．
- writeSim()
 - `Gazebo`におけるロボットモデルのjoint に指令値を送ります．今回のサンプルの場合，`hardware_interface::PositionJointInterface jnt_pos_cmd_interface_`で取得した位置指令から力指令を計算し，`Gazebo`に送ります．

上述のイメージを図で表すと，下図のようになります．<br/>
![RobotHWSim_HardwareInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/398a087e-7e08-9c28-6add-d0461b2ae953.png)

ここまで見れば，この3つのメソッドが，プラグインの動作を決定づける重要項目であることが理解できます．

# プラグインで定義された処理が実行される場所
では，これらのメソッドはいつ呼ばれるのでしょうか？

`ROS`と`Gazebo`を連携させると，`GazeboRosControlPlugin`という，`Gazebo`のプラグインと`ROS`間のインターフェースを制御するためのクラスが働いてくれます．ロボットモデルをSpawnする際に，`robot_description`の情報が渡されます．使用するプラグインの情報がその中に組み込まれているので，`GazeboRosControlPlugin`はどのプラグインを使えば良いのかを知ることができます．

では，上記プラグインのメソッドが実際に呼ばれる場所がどこになるのかが気になります．調べると，下記のリポジトリにありました．
[github: ros-controls/gazebo_ros_control](https://github.com/ros-controls/gazebo_ros_control)

```cpp:gazebo_ros_control/src/gazebo_ros_control_plugin.cpp
  // 省略
// Called by the world update start event
void GazeboRosControlPlugin::Update()
{
  // Get the simulation time and period
  gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
  ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
  ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;

  robot_hw_sim_->eStopActive(e_stop_active_);

  // Check if we should update the controllers
  if(sim_period >= control_period_) {
    // Store this simulation time
    last_update_sim_time_ros_ = sim_time_ros;

    // Update the robot simulation with the state of the gazebo model
    robot_hw_sim_->readSim(sim_time_ros, sim_period);

    // Compute the controller commands
  // 省略
    controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);
  }

  // Update the gazebo model with the result of the controller
  // computation
  robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
  last_write_sim_time_ros_ = sim_time_ros;
}
  // 省略
```

本`Update()`メソッドはシミュレーション周期毎にコールバックされます．本コールバック関数の登録は同クラスのメソッドである`Load()`で行われています．

```cpp:gazebo_ros_control/src/gazebo_ros_control_plugin.cpp
// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
  // 省略
    // Listen to the update event. This event is broadcast every simulation iteration.
    update_connection_ =
      gazebo::event::Events::ConnectWorldUpdateBegin
      (boost::bind(&GazeboRosControlPlugin::Update, this));
  // 省略
}
```
`Load()`は，プラグインのエントリポイントであり，本メソッド内で`Gazebo`と`ROS`を連携するための様々な初期化が行われています．

- 追記
 - `Gazebo`からどのようにこの`Load()`を呼んでいるかについては，[Gazebo から ROS のプラグインを呼ぶ処理の仕組み（2. ロボットモデル登録時のModelPluginの読込み）](http://qiita.com/MoriKen/items/86289224696756388df4)にて扱いました．

さて，話を戻して，肝心の`Update()`メソッドの大きな流れは次の通りです．

1. シミュレーション周期と制御周期を比較します
 - 制御周期分時間が進んでいたら，`Controller`の状態を更新するため，以降の処理に移ります．
1. `Gazebo`モデルの状態を取得し，`Controller`で使用するロボットモデルの状態を更新します．
 - ここで，`RobotHWSim::readSim()`の登場です．プラグインのコードでオーバライドしたメソッドがここで走ります．
 - 具体的には，前述した通り，`jnt_pos_`, `jnt_vel_`, `jnt_eff_`を更新します．
1. 更新したロボットモデルの情報を元に，`Controller`による制御則の計算を実行します．
 - `controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);` の部分です．`controller_manager_`は，`controller_manager::ControllerManager`クラスのインスタンスです．
 - 本インスタンスは自分が管理すべき`Controller`を全て知っているので，`ControllerManager::update()`メソッド一つで全コントローラを更新できます．
 - 例えば，`Controller`が`JointTrajectoryController`であれば，前述した`jnt_pos_cmd_`が更新されるのがここです．
1. `Controller`で算出した結果を，`Gazebo`モデル側に反映させます．
 - ここで，`RobotHWSim::writeSim()`が登場します．
 - 今回の例では，位置指令である`jnt_pos_cmd_`を元に力指令を計算して，`Gazebo`側に書き込んでいます．

# `RobotHWSin`と`RobotHW`との対比
参考として，`Gazebo`利用時に使われる`RobotHWSim`を，実機適用時に使われる`RobotHW`と対比させます．[`RobotHW`のWiki](
https://github.com/ros-controls/ros_control/wiki/hardware_interface)に記載された`main()`の記述を見れば，シミュレータ側との違いを比較でき，理解が深まると思います．

```cpp:declaration_of_MyRobot
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW
{
 // 省略
};
```
`hardware_interface::RobotHW`を継承して，`MyRobot`というオリジナルロボット用のサブクラスを作成しています．
これをどう使うのか．`main()`を見てみます．

```cpp:main_routine
main()
{
  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  while (true)
  {
     robot.read();
     cm.update(robot.get_time(), robot.get_period());
     robot.write();
     sleep();
  }
}
```
`read()`と`write()`は，前述した`readSim()`と`writeSim()`と対応します．ただし，実際のロボットのハードウェアの仕様（`read()`ならエンコーダ等，`write()`ならアクチュエータ等）に合わせて，自分でドライバ部分を実装する必要があります．
`cm.update()`は，まさに前述の`controller_manager_->update()`と同じ役割です．`cm`の宣言の部分を見ると，`controller_manager::ControllerManager`のコンストラクタの引数に`MyRobot robot`のポインタを渡しています．`cm`は，`robot`の情報を全部知っていることになります．

実は，ここが重要です．詳細な挙動は後述しますが，`ControllerManager`側は，`RobotHW`または`RobotHWSim`の情報を知らないと，にっちもさっちもいかないことになるのです．実際，先ほどの`gazebo_ros_control_plugin.cpp`の中でも，`ControllerManager`の初期化は`Load()`メソッド内で下記のように行われています．

```cpp:gazebo_ros_control/src/gazebo_ros_control_plugin.cpp
// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
 // 省略
    // Create the controller manager
    ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
    controller_manager_.reset
      (new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));
 // 省略
}
```
`RobotHW`と`RobotHWSim`では，下記のような違いが有ります．

- `RobotHW`を使った先ほどのサンプルでは，`ControllerManager`の初期化時にコンストラクタで`RobotHW`のインスタンスのポインタ渡していました．
- `GazeboRosControlPlugin`では，`reset()`というメソッドで`RobotHWSim`の情報を`ControllerManager`に渡しています．
 - コンストラクタの段階で，どんなロボットがやってくるのか，`GazeboRosControlPlugin`では知ったこっちゃありません．
 - 入れ物だけ作って，後でロボットの情報を上書きしてあげるイメージです．

![robothw_robothwsim_controllermanger_init.png](https://qiita-image-store.s3.amazonaws.com/0/110138/01a6ceeb-1e11-4298-2b84-c93686353e03.png)

いずれにしろ，`ControllerManager`にロボットモデルの情報を渡していることは，共通しているようです．

以上が，プラグインの動作の理解を深めるために解析した内容です．

ただ，上述した説明では`RobotHW`の具体的な実装は省略されています．そこで，本節の最後に`RobotHW`の良いサンプルについてご紹介します．

- `minimum_ros_control`([Github: yoneken/minimum_ros_control](https://github.com/yoneken/minimum_ros_control))
 - `RobotHW`のサンプルとしては，[yoneken](http://qiita.com/yoneken)さんによる`minimum_ros_control`というリポジトリが有用ですので，ここでご紹介させていただきます．
 - このサンプルでは，`RobotHW`を使う上で必要となる最低限の要素で構成された，非常にシンプルなロボットが扱われている，極めて見通しの良い素晴らしいサンプルです．`RobotHW`の理解を深める上で，ぜひ一読することをオススメします．
 - 実機を組むときにはこの`minimum_ros_control`をベースにカスタマイズしていくと，効率的に開発ができると思います．

ここで，`gazebo_ros_pkg`で`Gazebo`内のロボットを制御する場合と，`minimum_ros_control`で実機を制御する場合のソフトウェア構成図を比較してみます．

---
![gazebo_summary.png](https://qiita-image-store.s3.amazonaws.com/0/110138/6801a022-6ff3-4361-6e4f-978be26d62dd.png)
---
![real_robot_sammary.png](https://qiita-image-store.s3.amazonaws.com/0/110138/3d5230c9-6225-ab84-d6cd-232f6e0e125b.png)
---
モジュールとして異なる部分は，上記図中において緑の矢印で示した部分ですが，本質的には全く同一の構成を取っていることが分かります．
使用するコントローラが，`JointTrajectoryController`と`JointPositionController`で異なっていますが，これは設定というかロボットの制御の仕方の違いというレベルで，`Gazebo`だろうが`Real Robot`だろうが共通で入れ替えできる部分です．ただ，周期やゲインの設定がある場合には，シミュレーションか実機かで設定値を切り替えるのが実用的でしょう．この辺りの話は，
[次のエントリ](http://qiita.com/MoriKen/items/c29f653d03baffe5f0e2#gazebo%E3%81%AB%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%E3%83%A2%E3%83%87%E3%83%AB%E3%82%92spawn-%E3%81%99%E3%82%8B)でも触れます．

# プラグインとして使えるようにする
ここまで，プラグインの定義方法とその仕組みについて述べました．しかし，実はこれまでの作業だけではまだ使える状態にはありません．`ROS`システム側に対して，プラグインを有効にするための情報を教えてあげる必要があります．
そこで，[pluginlib](http://wiki.ros.org/pluginlib)の仕組みを使って，上記コードを正式にプラグイン化します．

- プラグインとして出力するための設定
 - プラグイン用のファイルとして出力されるように，コード側に追記が必要です．
 - コード内のどこでもよいので，`PLUGINLIB_EXPORT_CLASS(登録対象のクラス名, 登録対象のクラスの親クラス名)`のマクロを書きます．
 - 先ほどの例では，最終行に書いてありました．これが一般的なやり方とのことです．

```cpp:rosbook_arm_hardware_gazebo/src/rosbook_arm_hardware_gazebo.cpp
// ぜーんぶ省略して，最終行のみ
PLUGINLIB_EXPORT_CLASS(rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo, gazebo_ros_control::RobotHWSim)
```

- プラグイン説明ファイルの登録
 - 上記マクロでは書ききれない情報を登録します．プラグインの仕様とかはこちらです．
 - ROSシステムがこのプラグインを自動で検出・読込できるようにするために必要とのことです．
 - `rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo`の例では下記のとおりです．

```xml:rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo_plugins.xml
<library path="lib/librosbook_arm_hardware_gazebo">
  <class
    name="rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo"
    type="rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo"
    base_class_type="gazebo_ros_control::RobotHWSim">
    <description>
      ROS Book arm simulation interface.
    </description>
  </class>
</library>
```

- `ROS`パッケージシステムへのプラグインの登録
 - `ROS`システム全体でこのプラグインを使えるようにするための設定をします．
 - `package.xml`に`<export>`タグを追加します．
 - `rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo`の例では下記のとおりです．

```xml:rosbook_arm_hardware_gazebo/package.xml
  <export>
    <gazebo_ros_control plugin="${prefix}/rosbook_arm_hardware_gazebo_plugins.xml"/>
  </export>
```

ファイルとしての設定は以上です．以降の処理は[pluginlib](http://wiki.ros.org/pluginlib)を参考に進めればOKです．

# `DefaultRobotHWSim`について
作成した`ROSBookArmHardwareGazebo`では，`JointStateInterface`や`PositionJointInterface`を自分で宣言しましたが，`DefaultRobotHWSim`ではどうしているのでしょうか？デフォルトで用意されたプラグインを使う場合，私達が編集をする余地はありません．
ちょっとコードを覗いてみます．

```cpp:gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h
// 省略
namespace gazebo_ros_control
{
class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
// 省略
{
// 省略
  hardware_interface::JointStateInterface    js_interface_;
  hardware_interface::EffortJointInterface   ej_interface_;
  hardware_interface::PositionJointInterface pj_interface_;
  hardware_interface::VelocityJointInterface vj_interface_;
// 省略
}
// 省略
}
```
`JointStateInterface`，`EffortJointInterface`，`PositionJointInterfaceｍ`，`VelocityJointInterface`のインスタンスの宣言がオンパレードです．
ユーザ定義の設定ファイルからどんなインターフェースが降ってくるかなど，`DefaultRobotHWSim`からしたら分からないことなので，代表的なインターフェースについては予め土台を作っておくようです．これをどう扱うのか，`initSim()`を見てみます．

```cpp:gazebo_ros_control/src/default_robot_hw_sim.cpp
// 省略
namespace gazebo_ros_control
{
// 省略
bool DefaultRobotHWSim::initSim(
  const std::string& robot_namespace,
  ros::NodeHandle model_nh,
  gazebo::physics::ModelPtr parent_model,
  const urdf::Model *const urdf_model,
  std::vector<transmission_interface::TransmissionInfo> transmissions)
{
// 省略
  n_dof_ = transmissions.size(); 
// 省略
  // Initialize values 
  for(unsigned int j=0; j < n_dof_; j++) 
  { 
// 省略
    std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
// 省略
    const std::string& hardware_interface = joint_interfaces.front();
// 省略
    // Add data from transmission
    joint_names_[j] = transmissions[j].joints_[0].name_;
// 省略
    if(hardware_interface == "EffortJointInterface")
    {
      // Create effort joint interface
      joint_control_methods_[j] = EFFORT;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_effort_command_[j]);
      ej_interface_.registerHandle(joint_handle);
    }
    else if(hardware_interface == "PositionJointInterface")
    {
      // Create position joint interface
      joint_control_methods_[j] = POSITION;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_position_command_[j]);
      pj_interface_.registerHandle(joint_handle);
    }
    else if(hardware_interface == "VelocityJointInterface")
    {
      // Create velocity joint interface
      joint_control_methods_[j] = VELOCITY;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_velocity_command_[j]);
      vj_interface_.registerHandle(joint_handle);
    }
// 省略
  }
// 省略
}
// 省略
}
```
`URDF`の`<transmission>`タグ内の`<hardwareInterface>`タグで指定したインターフェース種別を読みだして，その種別に応じて登録対象のインターフェースを切り替えています．だから，`Gazebo`を使うときには，`<transmission>`の設定で`HardwareInterface`の種別を入力する必要があったのです．

# おわりに
今回は，`RobotHWSim`の動作にフォーカスを当てて解析を行いました．プラグインの作成方法と絡めながら，周辺のモジュールとどのように関わっているかも少し理解できるようになりました．

次回は，「[3. `Controller`について](http://qiita.com/MoriKen/items/c29f653d03baffe5f0e2)」説明します．ここまで分かれば，`Gazebo`と`ROS`の連携に関する，一通りの流れがつかめるようになります．