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

はじめに

ROSをいじり始めて半年ぐらいのド素人が，Learning ROS for Robotics Programming - Second Editionという本で，Chapter 10: Manipulation with MoveIt!の章を勉強をしているときに，このメモを残そうと思い立ちました．

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

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

• リポジトリ
  • github: AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition

そのロボットが動いている様子を下記の動画で確認できます．

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

目次

下記の図がインターフェース構成の概要です．この図の構成に対応させながら，理解を深めてくことが目標です．

1. ロボットモデルの定義と登録　←　イマココ
   1. URDFでロボットモデルを定義する
   2. HardwareInterfaceの存在意義
   3. URDFでGazeboの設定をする
   4. ロボットモデルをパラメータサーバに登録する
2. RobotHWSimのプラグインについて
   1. プラグインの定義
   2. プラグインで定義された処理が実行される場所
   3. RobotHWSinとRobotHWとの対比
   4. プラグインとして使えるようにする
   5. DefaultRobotHWSimについて
3. Controllerについて
   1. GazeboにロボットモデルをSpawnする
   2. Controllerを起動する
   3. ControllerManagerとHardwareInterfaceとのマッピング

これらの記事の内容を整理したものが，下記の図になります．

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

URDFでロボットモデルを定義する

ここで対象とするブロックは，下記の図においてピンクの矢印で記した部分です．

今回は，ロボットの構成を定義する要素のうちアーム部のファイルにのみ着目します．そのファイルの構成は，下記のとおりです．

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

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

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にいるはずですので，中身を見てみます．

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に定義があるはずですので，これを確認します．

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の提唱する分散開発の設計思想が体現されている構成です．

HardwareInterfaceとControllerについてもう少しだけ触れると，これはcontroller_interface/Controllerクラスを継承したコントローラ(例えば，position_controllers/JointTrajectoryController)から，位置指令を受け取ることができるインターフェースです．
Gazebo Tutorialの図を引用して，この例と対応付けると，下記のような関係となります．

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

URDFでGazeboの設定をする

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

ここで対象とするブロックはこちらです．本節では使用するプラグインを指定する部分だけを扱います．

着目するファイルはこんな位置づけです．

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

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 も見てみます．

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!で遊べるようにするために必要な設定とノードの起動をしてくれるのですが，ここではロボットモデルの登録部のみに着目します．

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の中で，設定ファイルを読み込んでいます．

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のプラグインについて」です．