はじめに

今回は、手順5.についてです。

尚、本記事に出てくるロボット（２種類）のソースコードは、GitHubにて公開しております。

独立二輪型ロボット：http://github.com/CIR-KIT/fourth_robot_pkg

また、本記事で対象にしているROSバージョンはIndigoです。

Gazebo と ROS

これまでに、ROSで自作のロボットモデルを用いるためのノウハウを記してきました。
しかし、Gazeboという、動力学シミュレーションソフトとの連携について触れていませんでしたので、
ざっと説明したいと思います。詳しい説明は、参考サイトをご覧ください。特に、参考1、2はめちゃくちゃわかりやすいです。神記事です。
ただ、ご紹介した記事は非常に細かい＆わかりやすいのですが、ある程度Gazeboを触ったことのある人でないと、
そもそもGazeboが何を目指しているのか？そのためにどんな仕組みを使っているのか?具体的な実装方法はどうするのか？
と言ったことがわかりづらいかもしれないと思ったので、本記事では、その辺りの事を簡単に記し、具体的な実装方法を例を用いてご紹介したいと思います。

ros_control

GazeboとROSの連携を語る上で、欠かせないのがros_controlです。
ここで、GazeboとROSの概念図を紹介します。概念図は公式wikiのものです。

はい。いきなりこれだけを見てもよくわからないと思うので、概要をご説明したいと思います。

Gazeboの目指すところ

Gazeboなどのシミュレーションの最終目標は、現実と遜色ない環境をシミュレートすることです。
とくにGazeboでは、ROSで開発されたプログラムをシミュレータ用に編集することなく使用でき、
Gazebo上でシミュレートすることができるようにする事を目標に製作されています。
つまり、
現実のロボットとGazeboのシミュレーションロボットに完全な互換性を持たせることが目標
ということです。
大まかに言うと、上図のオレンジ色の枠で囲ってある部分がその機能を果たしていることになります。
この部分の機能を担っているのが、シミュレーションの場合はRobotHWSim、実機の場合はRobotHWです。
つまり、制御の入力値を受け取って、実際に動かす部分に当たります。
この、制御の入力値というのは、１つのRobotHWSim・RobotHWに１種類だけ設定することができます。
例えば、速度入力を受け付けるといった具合です。
この、一意に定められた入力値に対して、柔軟にコントローラを設計するため、hardware_interfaceとros_controlがあります。

hardware_interfaceとは

hardware_interfaceとは、実機や、Gazeboシミュレーションが、どんな指令値を受け取り、どんな値を返すのかを指定するときに使います。上の概念図では、斜線で塗りつぶされているブロックの部分です。この、hardware_interfaceについては次回の投稿で詳しく触れたいと思いますが、このhardware_interfaceがあるおかげで、ros_controlの柔軟性が保たれていると言えます。

ros_controlとは

ros_controlは、現実のロボットとシミュレーションのロボットのどちらにも同様のインタフェースで
柔軟にコントローラーを提供することを目標にしています。
どういうことかわかりづらいと思うので、例を示したいと思います。

hardwar_simとros_controlを使うと何が良いのか

今、ロボットについてるあるアクチュエータ（モータ）をコントロール（制御）したいとします。
また、同時にそのアクチュエータをGazeboシミュレータでシミュレーションしたいとします。
このとき、まず何から考えるかというと、
そのアクチュエータ（モータ）は何で制御するのか？
ということです。
つまり、力制御・速度制御・位置制御のうちどれによって制御するのか？ということです。
制御の種類は実機で何制御を使うのかによって決定することになります。
大体は、モータの制御だと、速度制御か位置制御を用います。
（Hydraや、Atlasなどは、力制御。そもそもAtlasは油圧アクチュエータを使用している）
このとき、実機とGazeboがコントローラから受け取る指令値はhardware_interfaceというものを使って指定することができます。
つまり、hardware_simとros_controlを用いることで、制御方法を柔軟に選択できるようになっています。
これにより、たとえば、アクチュエータが速度制御であるとして、位置指令を入力値として使えるようなコントローラを作りたいという要望も満たせるようになっています。

要するに...

以上のことから、Gazebo + ROSで目標にしていることをまとめると、

実機と同じ機能を持ったシミュレーションを作りたい (Gazebo)
実機と同じ機能というのは、制御方法を同じにするということ (RobotHW・RobotHWSim)
ロボットの制御法に対して、柔軟なコントローラを作りたい (hardware_sim・ros_control)

以上です。
特に最後の目標の良さが実感できない方は、実際にロボットを作ってみるとよくわかると思います。

で、結局どうすれば実装できるのか

これまでに、hardware_interfaceとros_controlの仕組みについて軽く触れましたが、結局何をどうすればそれを実現できるのか分からなかったと思います（というか、説明していません）。私が実装していく時はここが一番ネックでした。
なので、簡単な例を用いながら進めていきたいと思います。
尚、今回用いるhardware_interfaceとros_controlは、デフォルトで用意されているものを用います。

例

ということで、今回も独立二輪型ロボットを例に用いたいと思います。
hardware_interfaceやros_controlは、ロボットの可動部分（アクチュエータを取り付ける部分）がある場合に用います。
つまり、独立二輪型ロボットなら、駆動輪を動かす２つのモータ部分ということです。（従輪については、ここでは忘れてください）

今回は、diff_drive_controllerという、独立二輪用ロボットのためのコントローラを使います。
diff_drive_controllerは以下のような仕様になっています。

入力トピック名	タイプ	説明
cmd_vel	geometry_msgs/Twist	並進・回転角速度を指定します
出力トピック名	タイプ	説明
odom	nav_msgs/Odometry	ホイールオドメトリを計算します
tf	tf/tfMessage	ホイールオドメトリから算出したbase_footprint（座標系名）の位置を出力します

ホイールオドメトリって何？という方は、参考11、12をご覧ください。
diff_drive_controllerでは、並進・回転角速度を受け取り、角車輪の回転速度を決定します。
その際に、ホイールベース長や車輪径が必要になるので、それらの情報も同時に設定しています。

URDFに必要な記述

URDF(Xacro)に最低限追加する必要のある要素は以下の通りです。

hardware_interfaceの指定
ロボットの各マニピュレータが力・速度・位置のどれを入力値として受け取り動くのか決定します。
gazebo_ros_controlプラグインの指定
GazeboとROSをつなげるプラグインの指定です。たいていは、１種類しか使わないので、あまり心配しなくてOKです。

以上の二つが最低限追加が必要な要素ですが、Gazebo上でシミュレートするためには、
物体の摩擦係数等を設定する必要があるので、通常は、以下の要素も追加します。

gazebo要素の追加
前述の通り、物体の摩擦係数や、シミュレータ独自のパラメータ、Gazebo上での色等の要素を指定します。

具体例

独立二輪ロボットを例に、各要素が記述されている部分をソースコードと共にご紹介します。
尚、例ではXacroを使用していますが、必要要素の記述はURDFと同様です。
また、ソースコードをGitHubに公開しております。
http://github.com/CIR-KIT/fourth_robot_pkg/fourth_robot_description

hardware_interface

wheel.transmission.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:macro name="wheel_trans_v0" params="prefix">
    <transmission name="${prefix}_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${prefix}_wheel_joint">
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
      <actuator name="${prefix}_wheel_motor">
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>30</mechanicalReduction>
      </actuator>
    </transmission>
  </xacro:macro> 

</robot>

例では、ロボットの各車輪が速度指令を受け取って動く必要があるので、VelocityJointInterfaceを使用する必要があります。
このInterfaceはデフォルトで用意されており、これを使用するために、transmission_interface/SimpleTransmissionを指定しています。
この要素は、アクチュエータが存在する部分に設定が必要です。
今回は、各車輪に設定する必要があったため、左右の情報を引数としたXacroのマクロを作っています。

gazebo_ros_controlプラグイン

fourth_robot_gazebo_element.urdf

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>fourth_robot</robotNamespace>
    </plugin>
  </gazebo>

上記の通り、プラグインにはlibgazebo_ros_controlを使用します。また、上の例ではロボット名の名前空間を指定しています。
名前空間については、指定しなくても大丈夫です。
この設定は、必ず記述する必要があります。

gazebo要素

wheel.gazebo.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:macro name="wheel_gazebo_v0" params="prefix">
    <gazebo reference="${prefix}_wheel_link">
      <selfCollide>true</selfCollide>
      <mu1 value="0.8" />
      <mu2 value="0.8" />
    </gazebo>
  </xacro:macro>
</robot>

上の例では、Gazeboシミュレータで使用するパラメータを設定しています。
GazeboシミュレータはデフォルトでODEを物理エンジンとして使用しています。
上の例に書いてある、mu1とmu2はこの、ODEで使用するパラメータを設定してます。摩擦係数だと思っていただければOKです。
この他にも、Gazebo上での色設定など行う事ができますが、上の例では行っていません。というのも、COLLADAファイルを読み込むため、
特に設定する必要が無いためです。
上の例では、タイヤ部分の設定をXacroのマクロで記述しています。
また、この設定は、すべてのLinkに対して設定できます。

ros_controllerの設定

ROSとGazeboを連携させるためには、hardware_interfaceとros_controlが必要ですが、前項までにURDFに追加しなければならない要素を記しました。更に実際に動かすためには、どんなコントローラを使うのか指定する必要があります。
本記事で例にしているロボットでは、diff_drive_controllerを使用しています。なので、タイヤ径をいくらにするのかといったことを設定する必要があります。ここでは、その具体的方法を記します。

1. パッケージを作る

まずは、<your_robot_name>_controlというパッケージを作成してください。もちろんROSパッケージです。やり方がわからない方は、これまでの記事で触れているのでそちらをご覧ください。パッケージができたら、パッケージの下にlaunchディレクトリと、configディレクトリを作成しておいてください。つまり、以下のようなディレクトリ構成になっているはずです。

<your_robot_name>_control
├── CMakeLists.txt
├── config
│   └── controller.yaml
├── launch
│   └── <your_robot_name>_control.launch
└── package.xml

2. package.xmlを編集

run_dependを追加する必要があります。コントローラによって多少変わりますが、diff_drive_controllerを使用する場合、追加する要素は以下の通りです。

controller_manager
actionlib_msgs
control_msgs
sensor_msgs
robot_state_publisher

です。追加すると、package.xmlは以下のようになっているはずです。

package.xml

<?xml version="1.0"?>
<package>
  <name><your_robot_name>_control</name>
  <version>1.0.0</version>
  <description>The <your_robot_name>_control package</description>

  <maintainer email="your@mail.com">YourName/maintainer>
  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>  
  <run_depend>controller_manager</run_depend>
  <run_depend>actionlib</run_depend>
  <run_depend>actionlib_msgs</run_depend>
  <run_depend>control_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>robot_state_publisher</run_depend>

</package>

3. config.yamlを編集する

configディレクトリの下にcontroller.yamlを作成してください。ただし、ファイル名は自由です。
今回は、diff_drive_controllerを使用しているので、以下のように設定をしてください。

controller.yaml

fourth_robot:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  diff_drive_controller:
    type        : "diff_drive_controller/DiffDriveController"
    left_wheel  : 'left_wheel_joint'
    right_wheel : 'right_wheel_joint'
    publish_rate: 25.0               # default: 50
    pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

    # Wheel separation and diameter. These are both optional.
    # diff_drive_controller will attempt to read either one or both from the
    # URDF if not specified as a parameter
    wheel_separation : 0.43515
    wheel_radius : 0.032

    # Wheel separation and radius multipliers
    wheel_separation_multiplier: 1.0 # default: 1.0

ただし、上の例は独立二輪ロボットの例です。
設定する項目の詳細などは、参考9をご覧ください。

4. launchファイルをつくる

あとは、コントローラを起動するlaunchファイルを作成すれば完了です。独立二輪ロボットの例を示します。

_control.launch

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find fourth_robot_control)/config/controller.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager"
                type="spawner" ns="fourth_robot" output="screen" 
                args="joint_state_controller
                          diff_drive_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher"
                type="robot_state_publisher"
                respawn="false" output="screen" ns="/fourth_robot">
  </node>

</launch>

このlaunchファイルでは、先ほどのYAMLファイルをパラメータとして読み込み、controller_spawnerを使ってコントローラを登録します。
また、robot_state_publisherで、ロボットのjointの状態をPublishするようにしています。

まとめ

ということで、ROSとGazeboを連携するためのしくみと、具体的な例についてご紹介しました。
結構複雑に感じますが、全てはご紹介した理念のためです。一旦理解すればなんと言うこともなくなります。
本記事でも触れましたが、ros_controlは奥が深いので、次回はそのへんを掘り下げたお話を紹介したいと思います。

参考

動作環境


PC	Lenovo ThinkPad X240
Prosessor	Intel Core i7-4600U (2.10GHz, 4MB, 1600MHz)
RAM	PC3-12800 DDR3L (8GB)
OS	Ubuntu 14.04 LTS 64bit
Kernel	3.13.0-44-generic
ROS	Indigo


PC	Desktop
Prosessor	Intel® Core™ i5-4460 CPU @ 3.20GHz × 4
RAM	DDR3 (24GB)
OS	Ubuntu 14.04 LTS 64bit
Kernel	3.13.0-83-generic
ROS	Indigo

Gazebo + ROS で自分だけのロボットをつくる 5. GazeboとROSの連携