Gazebo + ROS で自分だけのロボットをつくる 7. 実際にlaunchしてみる

# はじめに
今回は、ラスト、手順7.についてです。


0. [完成までの手順](http://qiita.com/RyodoTanaka/items/c3014fd6d0f06d12814f)
1. [好きなCADソフトを使ってSTLファイルを作る](http://qiita.com/RyodoTanaka/items/cad2cf100dd916d56717)
2. [好きなソフトを使ってCOLLADAファイルを作る](http://qiita.com/RyodoTanaka/items/b1ebd48da81669db7409)
3. [URDFファイルを作る](http://qiita.com/RyodoTanaka/items/174e82f06b10f9885265) 
4. [GazeboとROSの連携](http://qiita.com/RyodoTanaka/items/6fa7e45f98b55376a95b) 
5. [ros_controlについて](http://qiita.com/RyodoTanaka/items/e7feb74d0e29a8598f48)
6. [実際にlaunchしてみる](http://qiita.com/RyodoTanaka/items/af834ed767589a02e27a) **←今ココ**

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

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

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

# はじめに
とうとう最後です！オリジナルロボットをGazebo上に実際にlaunchするための手順などを紹介したいと思います。
本記事では、これまで例にしてきたロボットをlaunchして動かしてみるまでの手順です。

# 必要な要素
これまでの記事を読んでくださった方々ならお分かりと思いますが、Gazebo上でオリジナルロボットをlaunchするには以下の要素が必要です。

- ロボットのSTLもしくは、COLLADAファイル  
   -> [1. 好きなCADソフトを使ってSTLファイルを作る](http://qiita.com/RyodoTanaka/items/cad2cf100dd916d56717)、[2. 好きなソフトを使ってCOLLADAファイルを作る](http://qiita.com/RyodoTanaka/items/b1ebd48da81669db7409)
- ロボットのURDF(Xacro)モデル  
  -> [3. URDFファイルを作る](http://qiita.com/RyodoTanaka/items/174e82f06b10f9885265)
- URDF上にHardwareInterfaceの記述  
  -> [3. URDFファイルを作る](http://qiita.com/RyodoTanaka/items/174e82f06b10f9885265)
- URDF上にGazeboプラグインの記述  
  -> [4. GazeboとROSの連携](http://qiita.com/RyodoTanaka/items/6fa7e45f98b55376a95b) 
- 必要ならURDF上にセンサプラグインの記述
- ros_controllerの指定  
  -> [5. ros_controlについて](http://qiita.com/RyodoTanaka/items/e7feb74d0e29a8598f48)
- ros_controllerの設定ファイルとその読み込み  
  -> [6. 実際にlaunchしてみる]()

以上です。

# launchの手順
手順は至って簡単です。

1. Gazeboの起動
2. ロボットモデルを`robot_description`にloadする
3. `robot_state_publisher`の起動
4. ロボットモデルをGazebo上にスポーンさせる(urdf_spawner)
5. ロボットのコントローラを読み込む

この後にJoyコントローラを起動して、ロボットがSubscribeしているトピックに指令値を流せばGazebo上のロボットが動き回ります。

# 実際のlaunchファイル
実際のlaunchでは、

1. Gazeboの起動
2. ロボットモデルを`robot_description`にloadする
3. `robot_state_publisher`の起動
4. ロボットモデルをGazebo上にスポーンさせる(urdf_spawner)

までを一つのlaunchファイルに記述し、

- ロボットのコントローラを読み込む

については別launchファイルを作成し、それをincludeすることで起動します。
順にlaunchファイルを見てみます。

### 最初のlaunchファイル
```xml:husky_playworld.launch
<launch>

  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="model" default="$(find fourth_robot_description)/robots/fourth_robot.urdf.xacro"/>
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
	<arg name="world_name" value="$(find fourth_robot_gazebo)/worlds/clearpath_playpen.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- lrf merger -->
  <include file="$(find fourth_robot_bringup)/launch/sensors/lrf_merger.launch"/>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
		 command="$(find xacro)/xacro.py '$(arg model)'" />
  
  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
  		args="-urdf -model fourth_robot -param robot_description"/>

  <!-- ros_control motoman launch file -->
  <include file="$(find fourth_robot_control)/launch/fourth_robot_control.launch"/>
</launch>
```
https://github.com/CIR-KIT/fourth_robot_pkg/blob/master/fourth_robot_gazebo/launch/husky_playworld.launch#L1#L30

順に見ていきます。
まず最初にロボットモデルを`model`という引数にロードします。デフォルト値もこの時ロードしています。このあたりの文法については`roslaunch`に則ったものなのでわからない方は[Wiki](http://wiki.ros.org/roslaunch)をご覧ください。
この他にも、後に使うパラメータを引数で取得しています。

```xml
  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="model" default="$(find fourth_robot_description)/robots/fourth_robot.urdf.xacro"/>
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>
```
続いてGazeboを起動します。

```xml
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find fourth_robot_gazebo)/worlds/clearpath_playpen.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>
```
ここでは、`gazebo_ros`パッケージの`empty_world.launch`というlaunchファイルを起動しています。引数として、

- world名：このコードではhuskyのパッケージに保持されているGazeboのworldファイル
- debug:デバッグメッセージを表示するかどうか
- gui:起動時に`gz_client`を起動するか
- paused:起動時に停止するかどうか
- use_sim_time:sim_timeを使うかどうか
- headless:レンダリングを行うかどうか(gui=falseの時のみ有効)

を設定しています。

```xml
<!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
         command="$(find xacro)/xacro.py '$(arg model)'" />

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -model fourth_robot -param robot_description"/>
```
ここでは、ロボットモデルをXacroファイルから`robot_description`に読み込み、`urdf_spawner`を使ってGazebo上にモデルをスポーンさせます。ここまでで、動かないロボットモデルがGazebo上に登場します。

### ros_controlのlaunchファイル
```xml
  <!-- ros_control motoman launch file -->
  <include file="$(find fourth_robot_control)/launch/fourth_robot_control.launch"/>
```
ここで読み込まれている、コントローラのlaunchファイルを見てみます。

```xml:fourth_robot_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" output="screen" 
		args="joint_state_controller
			  diff_drive_controller">
  </node>

  <!-- 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">
  </node>

</launch>
```
https://github.com/CIR-KIT/fourth_robot_pkg/blob/master/fourth_robot_control/launch/fourth_robot_control.launch

順に見ていきます。

```xml
<!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find fourth_robot_control)/config/controller.yaml" command="load"/>
```
まず、コントローラの設定ファイルをパラメータサーバーに読み込みます。読み込んでいるファイルは[5. ros_controlについて](http://qiita.com/RyodoTanaka/items/e7feb74d0e29a8598f48)で紹介した**diff_drive_controller**と**joint_state_controller**の設定です。

```yaml: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
```

続いて、

```xml
  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager"
        type="spawner" output="screen" 
        args="joint_state_controller
              diff_drive_controller">
  </node>
```
この部分で`ros_controller`を読み込みます。具体的には、`controller_manager`パッケージを起動してコントローラを読み込みます。この時の引数は、先ほどYAMLファイルに指定したコントローラの名前です。

```xml
<!-- 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">
  </node>
```
そして最後に`robot_state_publisher`を起動します。このノードは`robot_description`に記述されているロボットモデルを元にをGazeboから送られてくるJointの情報によりロボットの状態をPublishしてくれるノードです。詳しくは[Wiki](http://wiki.ros.org/robot_state_publisher)をご覧ください。

以上でGazebo上でロボットを起動する要素と手順が揃いました。いよいよlaunchしてみましょう!!

# launchする
これまでに作成してきたlaunchファイルを起動します。
```bash
$ cd <catkin_ws>
$ source <catkin_ws>/devel/setup.bash
$ roslaunch fourth_robot_gazebo husky_playworld.launch
```
ここまでで、以下のようにGazebo上にロボットが登場したと思います。
![Screenshot from 2016-12-20 00:45:33.png](https://qiita-image-store.s3.amazonaws.com/0/110699/279289ab-ec68-6207-f24a-d005e5ac95cc.png)
うまく行ったら、後は`/diff_drive_controller/cmd_vel`にTwist型のメッセージを送れば動きます。というわけで、rqtを使ってロボットを動かしてみます。以下の動画みたいに動いたはずです。
[![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/hGU6g49ULnE/0.jpg)](http://www.youtube.com/watch?v=hGU6g49ULnE)

# まとめ
というわけで、自分のオリジナルロボットをGazebo上で動かすまでのひと通りの手順を記しました。
説明不足の点や、そもそもロボットモデルをアーム型にしたほうが良かったのでは。。。など反省点はいろいろありますが、また各々記事を書きたいと思います。
いずれにせよ、ROS + Gazeboでなにかやりたい！という方のお役に立てれば嬉しいです。


# 参考
3. http://daikimaekawa.github.io/ros/2014/12/19/ros_control/
4. http://cir-kit.github.io/blog/categories/gazebo/
5. http://gazebosim.org/tutorials/?tut=ros_control
6. http://gazebosim.org/tutorials
7. https://github.com/ros-controls/ros_control/wiki
8. http://wiki.ros.org//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 |