GazeboでBlock Laserを使用する

# はじめに

今回は、Gazeboで[Block Laser](http://gazebosim.org/tutorials?tut=ros_gzplugins#BlockLaser)を使用する方法を調査しました。ドキュメントには、Velodyneのようなグリッドスタイルの測域センサ（3D LiDAR）とありますが、設定方法の説明はありませんでしたので、ソースコードを確認していきます。

# 前提条件

| 環境  | バージョン |
|:--:|:--:|
| 開発機  | Ubuntu 16.04.6 LTS |
| ROS  | Kinetic Kame |
| Gazebo  | version 9.11.0 |
| プラグイン  | gazebo9-ros-pkgs |

# プラグインパラメータの確認

プラグインに設定するパラメータを調べるのは、それほど難しくありません。下記のパラメータ表に対応するソースコードを示します。

| 名称  | ソースコード |
|:--:|:--:|
| robotNamespace | [gazebo_ros_block_laser.cpp#L101](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp#L101) |
| frameName | [gazebo_ros_block_laser.cpp#L104](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp#L104) |
| topicName | [gazebo_ros_block_laser.cpp#L112](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp#L112) |
| gaussianNoise | [gazebo_ros_block_laser.cpp#L120](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp#L120) |
| hokuyoMinIntensity | [gazebo_ros_block_laser.cpp#L128](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp#L128) |
| updateRate | [gazebo_ros_block_laser.cpp#L138](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp#L138) |

これでプラグインに設定するパラメータがわかりました。

# sensorに設定するパラメータ

ここでいうsensorは、Gazeboのクラスになります。この先は、Gazeboのソースコードを調べていきます。

## typeを調べる

まず設定すべきtypeを調べます。この仕組みは少々複雑ですが、[gazebo_ros_block_laser.cpp#L95](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp#L95)でRaySensorにダイナミックキャストをしているため、_parentは、RaySensorと想定されます。

[RaySensor.cc#L46](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/RaySensor.cc#L46)では、`ray`という名称でGazeboに登録していることがわかります。このマクロは、[SensorFactory.hh#L88](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/SensorFactory.hh#L88)に実装されており、`SensorFactory::RegisterSensor`が定義されることがわかります。これは、[SensorFactory.cc#L67](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/SensorFactory.cc#L67)で`sensorMap`に登録されており、[SensorFactory.cc#L45](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/SensorFactory.cc#L45)で定義されている[SensorFactoryFn](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/SensorFactory.hh#L41)は、`Sensor*`であることがわかります。

では、`type=ray`が設定されるときにどのように呼ばれるか確かめると、[SensorManager.cc#L227](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/SensorManager.cc#L227)の`_elem->Get<std::string>("type")`でtypeを取得し、[sensors::SensorFactory::NewSensor(type)](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/SensorManager.cc#L233)で作成されています。これは、[SensorFactory.cc#L74](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/SensorFactory.cc#L74)で`sensorMap`に登録されているclassnameの場合、コンストラクタが呼ばれていることがわかります。つまり`type=ray`を設定すれば、`RaySensor`になることが確認できました。

## laserShapeを調べる

次に[laserShape](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/RaySensor.cc#L94)について調べます。この`laserShape`が何をやるものかというと、[RaySensor.cc#L155](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/RaySensor.cc#L155)であるような`GetMaxAngle`が設定されているものになります。つまりセンサの性能値を読み取るクラスであることがわかります。[RaySensor.hh#L169](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/RaySensor.hh#L169)で`laserShape`は、`physics::MultiRayShapePtr`であり、これは[PhysicsTypes.hh#L122](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/PhysicsTypes.hh#L122)で`boost::shared_ptr<MultiRayShape>`のtypedefです。[MultiRayShape.cc#L25](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L25)より、laserShapeで必要なパラメータがわかります。

## sensorパラメータ一覧

それでは全ての設定するパラメータがわかったので、下記に示します。`noise`については、これまでと同様の手順のため今回はソースコードだけ示し詳細は省略します。

| 名称  | 親 | ソースコード |
|:--:|:--:|:--:|
| ray | - | [MultiRayShape.cc#L56](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L56) |
| scan | ray | [MultiRayShape.cc#L57](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L57) |
| horizontal | scan | [MultiRayShape.cc#L58](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L58) |
| samples | horizontal | [MultiRayShape.cc#L221](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L221) |
| resolution | horizontal | [MultiRayShape.cc#L227](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L227) |
| min_angle | horizontal | [MultiRayShape.cc#L233](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L233) |
| max_angle | horizontal | [MultiRayShape.cc#L239](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L239) |
| vertical | scan | [MultiRayShape.cc#L61](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L61) |
| samples | vertical | [MultiRayShape.cc#L246](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L246) |
| resolution | vertical | [MultiRayShape.cc#L255](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L255) |
| min_angle | vertical | [MultiRayShape.cc#L264](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L264) |
| max_angle | vertical | [MultiRayShape.cc#L273](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L273) |
| range | ray | [MultiRayShape.cc#L59](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L59) |
| min | range | [MultiRayShape.cc#L203](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L203) |
| max | range | [MultiRayShape.cc#L209](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L209) |
| resolution | range | [MultiRayShape.cc#L215](https://github.com/arpg/Gazebo/blob/master/gazebo/physics/MultiRayShape.cc#L215) |
| noise | ray | [RaySensor.cc#L105](https://github.com/arpg/Gazebo/blob/master/gazebo/sensors/RaySensor.cc#L105) |
| noise詳細 | noise | [Noise.cc#L28](https://github.com/arpg/Gazebo/blob/619218c0bb3dc8878b6c4dc2fddf3f7ec1d85497/gazebo/sensors/Noise.cc#L28) |

設定すべきパラメータがわかったのでURDFの設定例を下記に示します。

```
<gazebo reference="${front_rear}_lidar_link">
    <sensor type="ray" name="${front_rear}_lidar_sensor">
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <update_rate>10</update_rate>
        <ray>
            <scan>
                <horizontal>
                    <samples>400</samples>
                    <resolution>1</resolution>
                    <min_angle>-${PI/2}</min_angle>
                    <max_angle>${PI/2}</max_angle>
                </horizontal>
                <vertical>
                    <samples>16</samples>
                    <resolution>1</resolution>
                    <min_angle>-${15.0*PI/180.0}</min_angle>
                    <max_angle> ${15.0*PI/180.0}</max_angle>
                </vertical>
            </scan>
            <range>
                <min>0.3</min>
                <max>131.0</max>
                <resolution>0.001</resolution>
            </range>
            <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.0</stddev>
            </noise>
        </ray>
        <plugin name="${front_rear}_lidar_link_block_laser_controller" filename="libgazebo_ros_block_laser.so">
            <topicName>${front_rear}_lidar_link/scan</topicName>
            <frameName>${front_rear}_lidar_link</frameName>
            <min_range>0.9</min_range>
            <max_range>130.0</max_range>
            <gaussianNoise>0.008</gaussianNoise>
        </plugin>
    </sensor>
</gazebo>
```

上記の例は、車体の前後に3D Lidarを水平180度垂直30度で設定しています。下記の画像がGazeboで取得したデータをrvizで可視化したPointCloudです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/559128/a3db0962-f641-1536-c087-261b68b3ac26.png)

# Gazeboに設定するパラメータについて

今回は、ソースコードから調査しましたが、Gazeboの仕組みさえ理解していれば[SDFormat](http://sdformat.org/spec?ver=1.6&elem=sensor#sensor_ray)から設定パラメータ一覧を確認することができます。

# Velodyneについて

Velodyneを使用している場合、専用の[プラグイン](https://bitbucket.org/DataspeedInc/velodyne_simulator/src/master/)が存在します。なぜGazeboに共通プラグインが存在するのにvelodyneが独自のプラグインが存在するかですが、`RaySensor`の処理方法が異なります。下記にlaserの例を示します。

| プラグイン | 概要 |
|:-:|:-:|
| [gazebo_ros_laser](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_laser.cpp) | [sensors::RaySensor](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_laser.cpp#L77)を用いて[sensor_msgs::LaserScan](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_laser.cpp#L149)をpublishする |
| [gazebo_ros_gpu_laser](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp) | [sensors::GpuRaySensor](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp#L80)を用いて[sensor_msgs::LaserScan](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp#L152)をpublishする |

これまでみてきたようにBlock Laserは、`RaySensor`を用いて演算をおこなっていましたが、Velodyneのプラグインについては、[GAZEBO_GPU_RAY](https://bitbucket.org/DataspeedInc/velodyne_simulator/src/506664dd478984aa6645d8210802a4a7ddc40629/velodyne_gazebo_plugins/src/GazeboRosVelodyneLaser.cpp#lines-57)で選択ができるようになっています。ただし[Known Issues](https://bitbucket.org/DataspeedInc/velodyne_simulator/src/master/)にあるとおりシミュレーションをするには、いくつか問題があるようです。わたしの環境（ROS Kinetic Kame & Gazebo9）では、こちらのプラグインがインストールできず実行はできませんでしたので詳細な確認はしていません。

# まとめ

* Gazeboのソースコードからパラメータを設定する仕組みを確認できる
* 仕組みを理解すればSDFormatからパラメータ名称を確認することができる
* RaySensorにはGPU対応があるが、Block Laserは非対応のため、必要な場合は自分で実装する必要がある