Controller と HardwareInterface との間の処理の仕組み（1. ロボットモデルの定義と登録）

# はじめに
ROSをいじり始めて半年ぐらいのド素人が，[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!`の章を勉強をしているときに，このメモを残そうと思い立ちました．
<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%" />

理由は，[`ros_control`](http://wiki.ros.org/ros_control)の`Controller`と`HardwareInterface`が絡むあたりで激しく混乱したからです笑．
取り敢えずチュートリアルに沿って作業をすれば動かせるけど，どういうフローでメッセージのやりとりがされていのか，パッとコードを見てもイメージできなかったのです．
そこで，ちょうどこの書籍で`ROS`を勉強していたので，そこで扱われているロボットモデルを例題に，`HardwareInterface`周りの処理の流れを追ってみることにしました．

さて，`Chapter 10: Manipulation with MoveIt!`という章では，勉強用の6軸ロボットモデルを使って，`Gazebo`上に表示されたロボットを`Moveit!`で制御することが題材となっています．下記のGithubリポジトリでコードがオープンになっています．

- リポジトリ
 - [github: AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition](https://github.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition)

そのロボットが動いている様子を下記の動画で確認できます．
[![IMAGE ALT TEXT HERE](https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/hydro-devel/chapter10_tutorials/images/rosbook_arm_moveit.png)](https://www.youtube.com/watch?v=gZJDvElwqg0&feature=youtu.be)

あまりかっこ良くないかもしれませんが（笑），超シンプルな多関節ロボットなので，勉強には持って来いです．

# 目次
下記の図がインターフェース構成の概要です．この図の構成に対応させながら，理解を深めてくことが目標です．
![00_first.png](https://bitbucket.org/osrf/gazebo_tutorials/raw/default/ros_control/Gazebo_ros_transmission.png)

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`とのマッピング

これらの記事の内容を整理したものが，下記の図になります．
![summary.png](https://qiita-image-store.s3.amazonaws.com/0/110138/ff5180a8-fb22-48ea-dc87-a0993b276221.png)

何か色々やってるんだな，ということで，先に進みましょう．この図は最後にもう一度見返します．

# URDFでロボットモデルを定義する
ここで対象とするブロックは，下記の図においてピンクの矢印で記した部分です．
<img src="https://qiita-image-store.s3.amazonaws.com/0/110138/300ecc2b-5a70-52e0-da55-66e568a2c4fb.png" width="50%" />

今回は，ロボットの構成を定義する要素のうちアーム部のファイルにのみ着目します．そのファイルの構成は，下記のとおりです．
![urdf_file_struct_1.png](https://qiita-image-store.s3.amazonaws.com/0/110138/bde52959-4cf9-19fd-22dc-ccd8074b76b1.png)

ここでは，アーム部の純粋なロボットモデルの定義をどう呼び出しているかを知りたいので，上記ファイルのうちピンクで囲ったものを対象に解析をします．

まずは，ロボットモデル全体の定義を司る`rosbook_arm_base.urdf.xacro`から見てみます．

```xml:rosbook_arm_description/robots/rosbook_arm_base.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
       name="rosbook_arm">
  <!--省略-->
  <xacro:include filename="$(find rosbook_arm_description)/urdf/arm/arm.urdf.xacro"/>
  <!--省略-->
  <xacro:arm parent="base"/>
  <!--省略-->
</robot>
```

`arm.urdf.xacro`を`include`して，`arm`という名前のマクロを呼んでいます．`arm`の正体が`arm.urdf.xacro`にいるはずですので，中身を見てみます．

```xml:rosbook_arm_description/urdf/arm/arm.urdf.xacro
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!--省略-->
  <xacro:include filename="$(find rosbook_arm_description)/urdf/arm/arm.transmission.xacro"/>
  <!--省略-->
  
  <xacro:macro name="arm" params="parent">
  <!--省略-->
    <joint name="shoulder_joint" type="revolute">
  <!--省略-->
    </joint>
  <!--省略-->
    <!-- Transmissions -->
    <arm_simple_transmission name="shoulder"  reduction="1.0"/>
  <!--省略-->
  </xacro:macro>
</robot>
```
`arm`というマクロの定義が記載されています．このマクロではアームのリンクを含めた全体の構成を定義していますが，ここではjoint のみに注目します．かつ，本エントリでは説明を簡単化するために，以降`shoulder_joint`のみを扱うこととします．
さて，`arm`マクロの`<joint>`タグの中で，`name="shoulder_joint"`と記載されています．このjoint 名がこのあと繰り返し出てくるので，心に留めておきます．

今度は，`arm_simple_transmission`というマクロが呼ばれています．`include`されている`arm.transmission.xacro`に定義があるはずですので，これを確認します．

```xml:rosbook_arm_description/urdf/arm/arm.transmission.xacro
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro name="arm_simple_transmission" params="name reduction">
    <transmission name="${name}_transmission">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${name}_motor">
        <mechanicalReduction>${reduction}</mechanicalReduction>
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </actuator>
      <joint name="${name}_joint">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
    </transmission>
  </xacro:macro>
</robot>
```

`<joint>`タグに注目です．`arm.urdf.xacro`で`arm_simple_transmission`を呼んだ時に，`shoulder`を引数に取っていたので，`shoulder_joint`という名前のjoint を対象に`HardwareInterface`を割り当てていることになります．

ここでは，`hardware_interface/PositionJointInterface`というインターフェースを設定しています．ユーザ側が任意に`HardwareInterface`を設定できるので，`controller_interface/Controller`側に合わせた指令値でjoint を制御できるようになります（e.g., 位置なのか，速度なのか，力なのか）．

また，ここで`HardwareInterface`の種類を設定しておけば，後ほどその設定がパラメータサーバに登録されることになります．こうすることで，`RobotHWSim`側から，ユーザが指定した`HardwareInterface`の情報を読み出せるようになります．この辺りの話は，追々出てきます．

既存の`Controller`を使う場合は，それの仕様に合わせた`HardwareInterface`を設定すれば使えるようになります．逆に，自分で`Controller`を設計したい場合には，既存の`HardwareInterface`の仕様に合わせて実装すればよいです．もちろん，両者のカスタマイズも可能です．

# `HardwareInterface`の存在意義
さて，この`Hardware Resource Interface`レイヤーが存在することで，便利なことが起こります．例えば，やっぱり`Controller`変えたいって思ったとします．もし，`HardwareInterface`が存在しない状態で`Controller`を変更すると，`RobotHWSim`も変更しなければなりません．逆も然りです．
そういう構成に合わせた設定ファイルを読ませることでこの現象は回避できるではないか，という反論があるかもしれませんが，そういう議論の末に最適と判断されたのが，まさに`HardwareInterface`の導入だったのでしょう．このレイヤーは，`RobotHWSim`とも`Controller`とも独立しているレイヤーなので，各々個別に拡張することができ，（相互に対応関係が担保されていれば）個々の要素を入れ替え可能です．まさに，ROSの提唱する分散開発の設計思想が体現されている構成です．

![RobotHWSim_HardwareInterface_block.png](https://qiita-image-store.s3.amazonaws.com/0/110138/f1a83c3b-08bd-f719-3204-b9d53c2b334f.png)

`HardwareInterface`と`Controller`についてもう少しだけ触れると，これは`controller_interface/Controller`クラスを継承したコントローラ(例えば，`position_controllers/JointTrajectoryController`)から，位置指令を受け取ることができるインターフェースです．
[Gazebo Tutorialの図](http://gazebosim.org/tutorials?tut=ros_control)を引用して，この例と対応付けると，下記のような関係となります．
![PositionJointInterface_JointTrajectoryController.png](https://qiita-image-store.s3.amazonaws.com/0/110138/e70cd5f5-f147-308d-e81f-3c97939dce6d.png)

この辺りの話は後述します．ここでは「ロボットモデルを定義するときには，関節にハードウェアインターフェースを指定する必要がある」という理解で先に進むことにします．

# URDFで`Gazebo`の設定をする
これをちゃんと指定しないと，`Controller`がせっかく頑張って指令値を送ってくれたとしても，`Gazebo`がうんともすんとも言ってくれません．

ここで対象とするブロックはこちらです．本節では使用するプラグインを指定する部分だけを扱います．
![gazebo_plugin.png](https://qiita-image-store.s3.amazonaws.com/0/110138/59cb1a64-54cd-410a-7e20-0cfe5ed62f63.png)

着目するファイルはこんな位置づけです．
![urdf_file_struct_2.png](https://qiita-image-store.s3.amazonaws.com/0/110138/f300609b-587f-573d-3d97-ffceed9334bc.png)

それでは，`rosbook_arm_base.urdf.xacro`を見返して，関連のある部分を抽出してみます．

```xml:rosbook_arm_description/robots/rosbook_arm_base.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
       name="rosbook_arm">
  <!--省略-->
  <xacro:include filename="$(find rosbook_arm_description)/gazebo/gazebo.urdf.xacro"/>
  <!--省略-->
</robot>
```

`gazebo.urdf.xacro` も見てみます．

```xml:rosbook_arm_description/gazebo/gazebo.urdf.xacro
<?xml version="1.0"?>
<robot>
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <ns></ns>
      <robotSimType>rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo</robotSimType>
      <controlPeriod>0.001</controlPeriod>
    </plugin>
  </gazebo>
</robot>
```
 `<robotSimType>`タグで使用したいプラグインを指定します．特に拡張性を重視する場面ではなければ`gazebo_ros_control/DefaultRobotHWSim`を使うのが一般的ですが，本テキストではお勉強のために`rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo`というカスタムプラグインを作成し，これを指定しています．このプラグインの正体は後述することとして，ひとまず先に進みます．

# ロボットモデルをパラメータサーバに登録する
ここまでの設定を他のノードが使えるようにするために，パラメータサーバにロボットモデルの情報を登録します．
これを行っているファイルが`rosbook_arm_moveit_config/launch/demo.launch`となっています．対象のテキストの章の主題が`Moveit!`の解説であり，その流れで本launchファイルを使っています．このファイルでは，最低限`Moveit!`で遊べるようにするために必要な設定とノードの起動をしてくれるのですが，ここではロボットモデルの登録部のみに着目します．

```xml:rosbook_arm_moveit_config/launch/demo.launch
<launch>
  <!--省略-->
  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find rosbook_arm_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>
  <!--省略-->
</launch>
```

`planning_context.launch`の中で，設定ファイルを読み込んでいます． 

```xml:rosbook_arm_moveit_config/launch/planning_context.launch
<launch>
  <!--省略-->
  <!-- The name of the parameter under which the URDF is loaded -->
  <arg name="robot_description" default="robot_description"/>

  <!-- Load universal robot description format (URDF) -->
  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro.py '$(find rosbook_arm_description)/robots/rosbook_arm_base.urdf.xacro'"/>
  <!--省略-->
</launch>
```

ここで，先ほどURDF形式で定義したロボットモデル(`rosbook_arm_base.urdf.xacro`)を，`robot_description`という名前でパラメータサーバに登録しています．これで他のノードからロボットモデルを参照できるようになります．

# おわりに
ここまでが，ロボットモデルの登録までのフローです．
次は「[2. `RobotHWSim`のプラグインについて](http://qiita.com/MoriKen/items/5cab7436c1b36c25e0ce)」です．