Controller と HardwareInterface との間の処理の仕組み（3. Controllerについて）


# 目次
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)」と「2. [RobotHWSimのプラグインについて](http://qiita.com/MoriKen/items/5cab7436c1b36c25e0ce)」で，下記図の青色で囲った部分のお話が終わりました．今回は，同図内のピンクで囲った部分のお話に移ります．
![RobotHWSim_HardwareInterface_ControllerManager.png](https://qiita-image-store.s3.amazonaws.com/0/110138/004aa973-33fc-d5f6-d751-e4e91021fdfc.png)

本記事用のまとめの図においては，下記の緑で囲った箇所です．
![3.summary.png](https://qiita-image-store.s3.amazonaws.com/0/110138/0b451b97-b2d0-5ad7-c701-c04f6b1e70a7.png)


ただ，いきなり`Controller`云々の話から始めるスタイルは取りません．`Gazebo`を起動するlaunchファイルを起点に，どんなファイルが何のために読み込まれるのかを紐解きながら，徐々に`Controller`の中身を見ていくスタイルを取ります．

# launchファイルの構造
最も単純な起動ファイルである`rosbook_arm_empty_world.launch`を起点に，下記のようなファイル構成を取っています．<br />
![gazebo_file_structure.png](https://qiita-image-store.s3.amazonaws.com/0/110138/9ad4a403-a175-c564-72ac-01fb078fc3d6.png)


これを順番に見ていきます．

```xml:rosbook_arm_gazebo/launch/rosbook_arm_empty_world.launch
<launch>
  <include file="$(find rosbook_arm_gazebo)/launch/rosbook_arm_gazebo.launch">
    <arg name="world" default="empty"/>
  </include>
</launch>
```

empty_world で`Gazebo`を起動するよう指定しています．次の`rosbook_arm_gazebo.launch`に行きます．

```xml:rosbook_arm_gazebo/launch/rosbook_arm_gazebo.launch
<launch>
  <arg name="world" default="empty"/>
  <arg name="robot" default="base"/>
  <arg name="gzpose" default="-x 0 -y 0 -z 0.0 -R 0.0 -P 0.0 -Y 0.0"/>
  <!--省略-->
  <!-- Start up world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="$(find rosbook_arm_gazebo)/worlds/$(arg world).world"/>
    <arg name="gui" default="$(arg gui)"/>
    <arg name="debug" default="$(arg debug)"/>
  </include>

  <!-- Spawn robot in simulation -->
  <include file="$(find rosbook_arm_gazebo)/launch/rosbook_arm_spawn.launch">
    <arg name="robot"  value="$(arg robot)"/>
    <arg name="gzpose" value="$(arg gzpose)"/>
  </include>

  <!-- Bring up robot -->
  <include file="$(find rosbook_arm_bringup)/launch/rosbook_arm_bringup.launch"/>
</launch>
```
worldの起動、`Gazebo`上へのロボットモデルのSpawn(`rosbook_arm_gazebo/launch/rosbook_arm_spawn.launch`)、`Controller`の起動(`rosbook_arm_bringup/launch/rosbook_arm_bringup.launch`)を行います．worldの起動はここでは重要ではないのでSpawnから行きます．
実はSpawn自体も，ここの主題を考える上ではあまり重要ではありません．しかし，そこに行き着く過程で`Controller`に必要なパラメータを読み込む箇所があります．実機と`Gazebo`を切り替えるシステムを組む時のノウハウにもなるので，ここも追っておくべきと判断しました．

# `Gazebo`にロボットモデルをSpawn する
対象は下記図のピンク部分です．
![gazebo_file_structure_spawn.png](https://qiita-image-store.s3.amazonaws.com/0/110138/ea710f32-3b00-1163-6e97-09a945270572.png)


では，対象範囲の頭である`rosbook_arm_spawn.launch`から見てみます．

```xml:rosbook_arm_gazebo/launch/rosbook_arm_spawn.launch
<launch>
  <arg name="robot" default="base"/>
  <arg name="gzpose" default="-x 0 -y 0 -z 0.0 -R 0.0 -P 0.0 -Y 0.0"/>

  <!-- PID gains -->
  <rosparam file="$(find rosbook_arm_controller_configuration_gazebo)/config/pids.yaml" command="load"/>

  <!-- Joint trajectory controllers -->
  <rosparam file="$(find rosbook_arm_controller_configuration_gazebo)/config/joint_trajectory_controllers.yaml" command="load"/>

  <!-- Robot model -->
  <include file="$(find rosbook_arm_description)/robots/upload_rosbook_arm.launch">
    <arg name="robot" default="$(arg robot)"/>
  </include>

  <!-- Spawn robot in Gazebo -->
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_model"
        args="-urdf -param robot_description $(arg gzpose) -model rosbook_arm"/>
</launch>
```

まず，いくつか`yaml`ファイルを読み込んでいます．`pids.yaml`と，`joint_trajectory_controllers.yaml`です．いかにも`Controller`向けのパラメータが入っていそうな名前です．中を覗いてみます．例によって，`shoulder_joint`だけを対象に追っていきます．

```yaml:rosbook_arm_controller_configuration_gazebo/config/pids.yaml
gains:
  shoulder_joint:  {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0}
  #省略
```
上記ファイルでは，joint の制御ゲインを設定します．

```yaml:rosbook_arm_controller_configuration_gazebo/config/joint_trajectory_controllers.yaml
arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - shoulder_joint
  #省略

gripper_controller:
  #省略
```
上記ファイルでは，使用する`Controller`の種類と，それを適用するjoint の対応付けを行います．
このファイルでは，`arm_controller`という名前空間を指定し，`position_controllers/JointTrajectoryController`という`Controller`を使うことと，それに対して`shoulder_joint`を割り当てることが設定されていることが分かります．本当は，これ以外のjoint も割り当てているし，その他制約に関する設定も記述されておりますが，ここでは割愛します．


- 少し寄り道をします．
 - はじめ，上記`yaml`ファイルをここで読み込むことに違和感がありました．現在対象としている`rosbook_arm_spawn.launch`では，ロボットモデルを`Gazebo`にSpawnすることが主題のはずなのに，ここで突然`Controller`に必要なパラメータが出るのは何故だ？と思ったからです．
 - しかし，次のように考えれば，これに意味はありそうです．例えば，実機と`Gazebo`のモデルが両方揃っており，切り替えて使える状況下では，それぞれの場合でパラメータを変えたいという要求があるはずです．シミュレータと実機の制御ゲインが一致するなんてことは，通常はありえないためです．


では，話を戻します．次に，`upload_rosbook_arm.launch`を見てみます．

```xml:rosbook_arm_description/robots/upload_rosbook_arm.launch
<launch>
  <arg name="robot" default="base"/>

  <!-- Load URDF into ROS param server -->
  <param name="robot_description" command="$(find xacro)/xacro
    '$(find rosbook_arm_description)/robots/rosbook_arm_$(arg robot).urdf.xacro'"/>

  <!-- Joint limits -->
  <rosparam file="$(find rosbook_arm_description)/config/joint_limits.yaml" command="load"/>
</launch>
```

以前のエントリで，`demo.launch`で`robot_description`にモデルを登録する記述がありましたが，あれは`Gazebo`を抜きにした簡易確認用のファイルなので，ここでは不要です．`Gazebo`を使う場合は，`upload_rosbook_arm.launch`を起動すればロボットモデルがパラメータサーバに登録されます．

その後`joint_limits.yaml`を読み込んでいますが，関節速度や加速度のリミットを設定しています．ここは追わなくても主題に影響しないので，割愛します．

そして，いよいよモデルをSpawn します．
`gazebo_ros`パッケージにある`spawn_model`を起動しています．「ロボットを表示させろ」と，`gazebo_ros`にサービスメッセージを送っています．引数はもちろん，`robot_description`です.これで，`Gazebo`にロボットが出現するはずです．
[Gazeboのチュートリアル](http://gazebosim.org/tutorials?tut=ros_roslaunch#UsingroslaunchtoSpawnURDFRobots)を見るとこの辺りが詳しく書いてあります．

# `Controller`を起動する
ようやく準備が終わり，本丸の話に入ります．対象は下記図のピンクの部分です．
![gazebo_file_structure_controller.png](https://qiita-image-store.s3.amazonaws.com/0/110138/e4f08ba2-5c3e-ad74-2f06-8dd2096f933f.png)

ロボットの世界でも`bringup`という表現を使うのかと(子供が育つ，的な場面で使われるような気がする)，どうでもよいことに若干の関心を示しつつ，頭から見てみます．

```xml:rosbook_arm_bringup/launch/rosbook_arm_bringup.launch
<launch>
  <!-- Load default controllers -->
  <include file="$(find rosbook_arm_controller_configuration)/launch/default_controllers.launch"/>

  <!-- Robot state publisher -->
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" value="50.0"/>
    <param name="tf_prefix"         value=""/>
  </node>

  <!-- Motion planning -->
  <include file="$(find rosbook_arm_moveit_config)/launch/move_group.launch"/>

  <!--省略-->
</launch>
```

`default_controllers.launch`で`Controller`を起動します．
その後，`robot_state_publisher`と`move_group`を起動しています．ここは深入りしなくても良いのでざっくりと解釈すると，次のとおりです．

- robot_state_publihser
 - `hardware_interface::JointStateInterface`が`publish`する`/joint_states`トピックを`subscribe`して，`TF`のフレームによろしく変換してくれる人
- move_group.launch
 - 後者はエンドイフェクタの経路計画とナビゲーションを司る`Moveit!`のノードを起動してくれる人

![Controller_ROSMessage_Moveit_RobotStatePublisher.png](https://qiita-image-store.s3.amazonaws.com/0/110138/1bacadc0-cd63-a7e5-624c-42c4264dcb72.png)


ここでミソなのは`default_controllers.launch`なので，ファイルの中身を見てみます．

```xml:rosbook_arm_controller_configuration/launch/default_controllers.launch
<launch>
  <!-- Joint state controller -->
  <rosparam file="$(find joint_state_controller)/joint_state_controller.yaml" command="load"/>

  <!-- Default controllers -->
  <node pkg="controller_manager" type="spawner" name="default_controllers_spawner"
        args="joint_state_controller
              arm_controller
              gripper_controller"/>
</launch>
```

`joint_state_controller.yaml`というファイルを読み込んでいます．これの位置づけは，次のとおりです．

- 先ほど，`joint_trajectory_controllers.yaml`というファイルを読み込んで`position_controllers/JointTrajectoryController`を使うことを指定しました．
- `joint_state_controller.yaml`は，それの`joint_state_controller/JointStateController`版です．

```yaml:ros_controllers/joint_state_controller/joint_state_controller.yaml
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50
```

これが`joint_state_controller.yaml`の全文です．`JointStateController`では全joint を扱うので，特定のjoint を指定する必要はないようです．

![JointController_yaml.png](https://qiita-image-store.s3.amazonaws.com/0/110138/c0abbf27-9688-663e-88ab-1f19914dcb5b.png)


そして，`Controller`をSpawn します．主役は，`controller_manager`パッケージの`spawner`ノードです．引数に，`hoge_hoge_controller.yaml`で設定した名前空間が入っています．この名前空間にひも付けされた`type`等のパラメータを読み取って，`Controller`を立ち上げるように，`ControllerManager`にサービスメッセージを送ります．

このノードの正体は，[`ros_control/controller_manager/scripts/spawner`](https://github.com/ros-controls/ros_control/blob/9d0d8922d448c563b6f6afb2f7578dd2ba73cabb/controller_manager/scripts/spawner)です．一応覗いてみます．

```py:controller_manager/scripts/spawner
# 省略
def main():
    # find yaml files to load
# 省略
    controllers = []
    for name in args.controllers:
        if os.path.exists(name):
            # load yaml file onto the parameter server, using the namespace specified in the yaml file
            rosparam.set_param("",open(name))
            # list the controllers to be loaded
            name_yaml = yaml.load(open(name))
            for controller in name_yaml:
                controllers.append(controller)
        else:
            controllers.append(name)

    # load controllers
    for name in controllers:
        rospy.loginfo("Loading controller: "+name)
        resp = load_controller(name)
        if resp.ok != 0:
            loaded.append(name)
        else:
            time.sleep(1) # give error message a chance to get out
            rospy.logerr("Failed to load %s" % name)

    rospy.loginfo("Controller Spawner: Loaded controllers: %s" % ', '.join(loaded))

    if rospy.is_shutdown():
        return

    # start controllers is requested
    if autostart:
        resp = switch_controller(loaded, [], 2)
        if resp.ok != 0:
            rospy.loginfo("Started controllers: %s" % ', '.join(loaded))
        else:
            rospy.logerr("Failed to start controllers: %s" % ', '.join(loaded))

    rospy.spin()
# 省略
```

ノード起動時に引数として渡された名前空間名(`joint_state_controller`, `arm_controller`等)を元にパラメータを読み込み，`Controller`の種類(`position_controllers/JointTrajectoryController`等)を読み出します．

`load_controller()`では，パラメータサーバから得た情報を基に`Controller`を起動するための準備をするよう，`ControllerManager`にメッセージを送ります．ここでは，`GazeboRosControlPlugin`側でロボットモデルの情報を元に`HardwareInterface`とのマッピングをします．`ControllerManager`はコンストラクタで`RobotHWSim`の情報を知っていることは，「[2. RobotHWSimのプラグインについて](http://qiita.com/MoriKen/items/5cab7436c1b36c25e0ce)」でも触れました．

そして，`switch_controller()`という関数に`Controller`の種類を渡して，`ControllerManager`に`Controller`を起動させるメッセージを送信しています．

# `ControllerManager`と`HardwareInterface`とのマッピング
ここで，`ControllerManager`と`HardwareInterface`とのマッピングをどう行っているのか，ちょっとだけ見てみます．

```cpp:controller_manager/src/controller_manager.cpp
// 省略
namespace controller_manager{
// 省略
bool ControllerManager::loadController(const std::string& name)
{
// 省略
  boost::shared_ptr<controller_interface::ControllerBase> c;
// 省略
  // Initializes the controller
  try{
// 省略
    initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources);
  }
// 省略
}
// 省略
}
```

このメソッドは，コントローラ1つを読み込む処理を行います．
`c`には，`Controller`のインスタンスが入ります．`JointTrajectoryController`や`JointStateController`等，その時指定された`Controller`のサブクラスのインスタンスが入っています．そして，`c->initRequest()`というメソッドを実行しています．中を見てみます．

```cpp:controller_interface/include/controller_interface/controller.h
namespace controller_interface
{
// 省略
  virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
                           ros::NodeHandle&             root_nh,
                           ros::NodeHandle&             controller_nh,
                           ClaimedResources&            claimed_resources)
  {
// 省略
    // get a pointer to the hardware interface
    T* hw = robot_hw->get<T>();
// 省略
    // return which resources are claimed by this controller
    hw->clearClaims();
    if (!init(hw, controller_nh) || !init(hw, root_nh, controller_nh))
    {
      ROS_ERROR("Failed to initialize the controller");
      return false;
    }
// 省略
  }
}
```
`T* hw = robot_hw->get<T>();`で，`HardwareInterface`が登場します．`T`は`HardwareInterface`の型が入ります．`RobotHW`がメンバとして持っている`HardwareInterface`型のインスタンスの中から，`T`の型を持ったものへのポインタを取得しています．確かにこの情報があれば，`RobotHWSim`でいじくり回していた`jnt_pos_`や`jnt_pos_cmd_`等の値を，`ControllerManger`側で扱えそうな予感がします．

この先少し話がややこしいので，青写真的にイメージ図を載せておきます．白抜き矢印が継承，黒抜き矢印がデータの方向を表します．
![HardwareInterface_ControllerManager.png](https://qiita-image-store.s3.amazonaws.com/0/110138/caa0be1e-0cee-8236-4d68-014656d93418.png)


## `JointStateController`の場合
この`T`ついて使用例を挙げます．`JointStateController`の場合は，下記のように宣言されているので，`T`には`JointStateInterface`が当てはまります．

```ccp:joint_state_controller/include/joint_state_controller/joint_state_controller.h
class JointStateController: public controller_interface::Controller<hardware_interface::JointStateInterface>
```

気になるのは`init()`です．ここがミソなのは間違いなさそうです．構造が単純な`JointStateController`を例に，`init()`の例を見てみます．

```cpp:joint_state_controller/src/joint_state_controller.cpp
// 省略
namespace joint_state_controller
{
// 省略
  bool JointStateController::init(hardware_interface::JointStateInterface* hw,
                                  ros::NodeHandle&                         root_nh,
                                  ros::NodeHandle&                         controller_nh)
  {
    // get all joint names from the hardware interface
    const std::vector<std::string>& joint_names = hw->getNames();
    num_hw_joints_ = joint_names.size();
    for (unsigned i=0; i<num_hw_joints_; i++)
      ROS_DEBUG("Got joint %s", joint_names[i].c_str());

    // get publishing period
    if (!controller_nh.getParam("publish_rate", publish_rate_)){
      ROS_ERROR("Parameter 'publish_rate' not set");
      return false;
    }

    // realtime publisher
    realtime_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(root_nh, "joint_states", 4));

    // get joints and allocate message
    for (unsigned i=0; i<num_hw_joints_; i++){
      joint_state_.push_back(hw->getHandle(joint_names[i]));
      realtime_pub_->msg_.name.push_back(joint_names[i]);
      realtime_pub_->msg_.position.push_back(0.0);
      realtime_pub_->msg_.velocity.push_back(0.0);
      realtime_pub_->msg_.effort.push_back(0.0);
    }
    addExtraJoints(controller_nh, realtime_pub_->msg_);

    return true;
  }
// 省略
}
```
`const std::vector<std::string>& joint_names = hw->getNames();`で，`RobotHWSim`が持っている全joint 名を取得します．
そのjoint 名からハンドルを取得し，`addExtraJoints(controller_nh, realtime_pub_->msg_);`で，`Controller`側のメンバである`joint_state_`と，`HardwareInterface`側で既に作成されているjoint のハンドルをマッピングしています．以降，`JointStateController`は自由にjoint の状態(`RobotHWSim`の`jnt_pos_`等の値)を取得できるようになりました．
これで`JointStateController`が，`JointStateInterface`を介して，`RobotHWSim`と繋がったわけです．納得です！

飛ばしましたが，`realtime_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(root_nh, "joint_states", 4));`で`publisher`が初期化されているようです．ついでに，`JointStateInterface`の`update()`メソッドも見てみます．

```cpp:joint_state_controller/src/joint_state_controller.cpp
// 省略
  void JointStateController::update(const ros::Time& time, const ros::Duration& /*period*/)
  {
    // limit rate of publishing
    if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){

      // try to publish
      if (realtime_pub_->trylock()){
        // we're actually publishing, so increment time
        last_publish_time_ = last_publish_time_ + ros::Duration(1.0/publish_rate_);

        // populate joint state message:
        // - fill only joints that are present in the JointStateInterface, i.e. indices [0, num_hw_joints_)
        // - leave unchanged extra joints, which have static values, i.e. indices from num_hw_joints_ onwards
        realtime_pub_->msg_.header.stamp = time;
        for (unsigned i=0; i<num_hw_joints_; i++){
          realtime_pub_->msg_.position[i] = joint_state_[i].getPosition();
          realtime_pub_->msg_.velocity[i] = joint_state_[i].getVelocity();
          realtime_pub_->msg_.effort[i] = joint_state_[i].getEffort();
        }
        realtime_pub_->unlockAndPublish();
      }
    }
  }
// 省略
```

`/joint_states`トピックを`publish`する処理が記述されているのが確認できます．

## `JointTrajectoryController`の場合
`position_controllers/JointTrajectoryController`でも，大きな流れは同じようです．`init()`で`joint`のマッピングを行い，`update()`で指令値を更新しています．

ただ，`JointTrajectoryController`自体は，位置でも速度でも力でも対応しているようなので，どのインターフェースが来ても対応できるように，クラスの宣言時はベースクラスである`HardwareInteface`を`T`としているようです．ただし，`position_controllers/JointTrajectoryController`の場合は，位置指令で軌道コマンドを受け取る仕様となっており，対応するインターフェースは`PositionJointInterface`となるようです．私の力不足により，ここの解析はしきれませんでした．
ひとまず先に進めます．

```cpp:joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
// 省略
bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInterface* hw,
                                                                     ros::NodeHandle&   root_nh,
                                                                     ros::NodeHandle&   controller_nh)
{
// 省略
  // List of controlled joints
  joint_names_ = getStrings(controller_nh_, "joints");
  if (joint_names_.empty()) {return false;}
  const unsigned int n_joints = joint_names_.size();

  // URDF joints
  boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
// 省略
  std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names_);
// 省略
  for (unsigned int i = 0; i < n_joints; ++i)
  {
    // Joint handle
    try {joints_[i] = hw->getHandle(joint_names_[i]);}
// 省略
    // Whether a joint is continuous (ie. has angle wraparound)
    angle_wraparound_[i] = urdf_joints[i]->type == urdf::Joint::CONTINUOUS;
// 省略
  }
// 省略
  // Hardware interface adapter
  hw_iface_adapter_.init(joints_, controller_nh_);
// 省略
}
// 省略
```
すべてのjoint 名を取得するのではなく，`joint_trajectory_controllers.yaml`内で`joints`として登録した`joint`名だけを取得します．`urdf_joints`を取ってきているのは，`URDF`内で定義した`joint`の種類(continuous等)によって挙動を変えるためのようです．
`hw_iface_adapter_.init()`で，`Controller`とjoint のマッピングをしています．

```cpp:joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
// 省略
void JointTrajectoryController<SegmentImpl, HardwareInterface>::
update(const ros::Time& time, const ros::Duration& period)
{
// 省略
  // Update current state and state error
  for (unsigned int i = 0; i < joints_.size(); ++i)
  {
    current_state_.position[i] = joints_[i].getPosition();
    current_state_.velocity[i] = joints_[i].getVelocity();
    // There's no acceleration data available in a joint handle

    state_error_.position[i] = desired_state_.position[i] - current_state_.position[i];
    state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i];
    state_error_.acceleration[i] = 0.0;
  }
// 省略
  // Hardware interface adapter: Generate and send commands
  hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period,
                                  desired_state_, state_error_);

  // Publish state
  publishState(time_data.uptime);
}
// 省略
```
`update()`メソッドは，現在のjoint の状態を更新して目標値との差分を計算した後，`hw_iface_adapter_.updateCommand()`で指令値(`jnt_pos_cmd_`)を更新します．
`JointTrajectoyController`が，例えば`Moveit!`からメッセージを受け取れば，上記フローを経由して`Gazebo`まで指令が送り届けられます．

## イメージ図
![Controller_ROSMessage.png](https://qiita-image-store.s3.amazonaws.com/0/110138/1027a372-f185-4b91-81f6-5371fbc31e1a.png)

以上で大体追えたと思います．

#まとめ
冒頭に記載した，ここまでのまとめの図を再掲します．
![summary.png](https://qiita-image-store.s3.amazonaws.com/0/110138/ff5180a8-fb22-48ea-dc87-a0993b276221.png)

最後にこれを振り返ると，全体が繋がっているんだなってことを確認できます．

#終わりに
初めて`ros_control`の`Controller`と`HardwareInterface`の設定を見た時，その関係が掴めずあっけにとられていましたが，ようやくクリアになりました．詳細には眼をつむりましたが，流れを理解するには十分と判断しました．プラグインを自作するときにも，どこを触ればいいのか大体わかるようになりました．

個々のモジュールが独立していて，個々の要素は個別に交換が可能です．既存のプラグインでは物足りない，ちょっと変えたい，というところだけプラグイン化してしまえば，自分仕様のロボットモデルが出来上がります．今回の取組みで，`ROS`による分散開発の良さについて，理解を深めることができました．`ROS`を学ぶことの良さの一つして，ロボット開発の巨人が培ったノウハウの集大成を惜しみなく見せてもらえることが挙げられるなぁ，と感じた次第です．

この設計思想は，ロボットに限らず一般のシステムでのインターフェースへも適用できるでしょう．今後ソフトウェア開発でなにか困ったことがあったら，`ROS`から学ばせて頂くことはないか，と思いを馳せてみよう思います．