GazeboのTurtlebotにHokuyoセンサーを導入する方法

#始めに
株式会社テクノロードさんにインターンシップで行かせて頂き、ROSを勉強しました。
今回はその中からTurtlebotのセンサーをカメラからレーザーレンジであるHokuyoに変更する方法をまとめました。
株式会社テクノロードさんは、ロボット関係等の試作・受託開発を行っている会社です。
https://techno-road.com/

##環境
今回は以下の環境で行いました。
・ubuntu16.04LTS
・kinetic
・SSDによるデュアルブート
・GTX760M

kineticのインストールは以下をご覧下さい。
http://wiki.ros.org/ja/kinetic/Installation/Ubuntu

##前提

・ROSのkineticが入っている。
・GazeboのシュミレータでTurtlebotが起動出来る。
  
具体的には以下のコマンドが出来れば大丈夫です。

```sh
$roslaunch turtlebot_gazebo turtlebot_world.launch
$roslaunch turtlebot_gazebo gmapping_demo.launch
```
稀にシュミレーターが上手く起動しない場合がありますが、
そのような時はUbuntuのシステム設定→ソフトウェアとアップデート
→追加のドライバーから、パソコンのGPUドライバーを入れてみると起動出来る場合があります。
また、レーザーレンジを導入後gazebo上でセンサーが貫通している場合も、ドライバーを確認してみて下さい。

##1.hokuyoのurdfファイルを作る
まず、hokuyoセンサーの情報を記述したurdfファイルを作ります。
有り難い事に、見た目を決めるdaeファイルは予め用意されていますのでそちらを使います。

まずはセンサーフォルダに移動して、ファイルを作成します。

```sh
cd $ROS_ROOT/../turtlebot_description/urdf/sensors
sudo gedit hokuyo.urdf.xacro
```
そして、以下をコピペして保存します。

```hokuyo.urdf.xacro
<?xml version="1.0"?>
<robot name="sensor_hokuyo" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_gazebo.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_properties.urdf.xacro"/>

  <xacro:macro name="sensor_hokuyo" params="parent">
    <link name="hokuyo_link">
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.1 0.1 0.1"/>
        </geometry>
      </collision>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://gazebo_plugins/test/multi_robot_scenario/meshes/laser/hokuyo.dae"/>
        </geometry>
      </visual>
      <inertial>
        <mass value="1e-5" />
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
      </inertial>
    </link>
    <joint name="hokuyo_joint" type="fixed">
      <!--<axis xyz="0 0 1" />-->
      <origin xyz="0.08 0 0.430" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="hokuyo_link"/>
     </joint>
     <!-- Hokuyo sensor for simulation -->
  <gazebo reference="hokuyo_link">
    <sensor type="ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>0.25</resolution>
            <min_angle>0</min_angle>
            <max_angle>6.28319</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
</xacro:macro>
</robot>
```

このファイルの簡単な説明については、後程紹介します。

##2.関連付けをする
次に、今作ったファイルが呼び出される様に関連付けを行います。

センサーファイルを作った場所から以下のコマンドで別のディレクトリに移動して、新しくHokuyo用のファイルを作ります。

```sh
$ cd ../../robots/
$ sudo cp kobuki_hexagons_asus_xtion_pro.urdf.xacro kobuki_hexagons_hokuyo.urdf.xacro
```
次に作ったファイルを開き、先程のhokuyo.urdf.xacroと関連付けを行います。

```sh
$ sudo gedit kobuki_hexagons_hokuyo.urdf.xacro 
```
変更する点は、以下の一行を消し

```kobuki_hexagons_hokuyo.urdf.xacro 
<xacro:include filename="$(find turtlebot_description)/urdf/sensors/asus_xtion_pro.urdf.xacro"/>
```
その場所に以下を書きます。

```kobuki_hexagons_hokuyo.urdf.xacro 
<xacro:include filename="$(find turtlebot_description)/urdf/sensors/hokuyo.urdf.xacro"/>
```

また、以下の一行を消し

```kobuki_hexagons_hokuyo.urdf.xacro 
<sensor_asus_xtion_pro parent="base_link"/>
```
その場所に以下を書きます。

```kobuki_hexagons_hokuyo.urdf.xacro 
<sensor_hokuyo parent="base_link"/>
```

最終的には、以下の様な内容になります。

```kobuki_hexagons_hokuyo.urdf.xacro 
<?xml version="1.0"?>
<!--
    - Base      : kobuki
    - Stacks    : hexagons
    - 3d Sensor : hokuyo
-->    
<robot name="turtlebot" xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_common_library.urdf.xacro" />
  <xacro:include filename="$(find kobuki_description)/urdf/kobuki.urdf.xacro" />
  <xacro:include filename="$(find turtlebot_description)/urdf/stacks/hexagons.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/sensors/hokuyo.urdf.xacro"/>
  
  <kobuki/>
  <stack_hexagons                 parent="base_link"/>
  <sensor_hokuyo parent="base_link"/>
</robot>
```

##3.環境変数の変更
最後に、環境変数をhokuyoに変更すればgazebo上で、変わっている事が確認出来ます。
この処理は新しい端末を開くたびに行う必要があり、hokuyoセンサーしか使わない場合は、
bashrcに書いておくとコマンドを省略出来ます。

```sh
$ export TURTLEBOT_3D_SENSOR="hokuyo"
```
今のセンサーの変数を確認するには、以下のコマンドで確認する事が出来ます。

```sh
$ echo $TURTLEBOT_3D_SENSOR
```
これでgazeboを起動すると、hokuyoセンサーに変わっている事が確認出来ます。


###4.マッピングやナビゲージョンへの適用方法
マッピングやナビゲージョンをする時に、hokuyo用にファイルを追加する必要があります。
以下の方法で追加をする事が出来ます。

```sh
$ cd $ROS_ROOT/../turtlebot_navigation/launch/includes/gmapping
$ sudo cp asus_xtion_pro_gmapping.launch.xml hokuyo_gmapping.launch.xml
$ cd ../amcl/
$ sudo cp asus_xtion_pro_amcl.launch.xml hokuyo_amcl.launch.xml
```



##hokuyo.urdf.xacroの簡単な説明
細かい詳細は以下のサイトと照らし合わせて見ると、理解しやすいと思います。
http://sdformat.org/spec

テキストの左側に書かれている番号は行番号になります。

```xml
16<geometry>
17  <mesh filename="package://gazebo_plugins/test/multi_robot_scenario/meshes/laser/hokuyo.dae"/>
18</geometry>
```
ここで、見た目を設定しています。
もし見た目が白いブロックになっている場合は、フォルダにファイルが無い場合があるので確認をして、もし無ければネットから持ってくる必要があります。


```xml
28 <origin xyz="0.08 0 0.430" rpy="0 0 0"/>
```
ここで、センサー位置を設定しています。
今回の位置は、Turtlebotの台の上に合わせています。

```xml
36<visualize>true</visualize>
```
ここで、gazebo上でのセンサーの可視化の有無を変更出来ます。
今は360度にしていますので、見難いと感じたらfalseにして見ると良いと思います。

```xml
41<samples>360</samples>
```
ここで、取得するサンプル数を設定します。

```xml
43<min_angle>0</min_angle>
44<max_angle>6.28319</max_angle>
```
ここで、センサーの角度範囲をラジアンで設定出来ます。

```xml
48<min>0.10</min>
49<max>30.0</max>
```
ここで、センサーの奥行き範囲を設定出来ます。

```xml
63<topicName>scan</topicName>
```
ここで、実際に値を入れるトピックの名前を変更出来ます。
ここを変更した場合、他のファイルも変更しなければいけなくなるので、
launchファイルを追える方のみ変更してみて下さい。

##最後に
今回は、以下のページも参考にしました。
https://bharat-robotics.github.io/blog/adding-hokuyo-laser-to-turtlebot-in-gazebo-for-simulation/