GazeboでBlock Laserを使用する

はじめに

今回は、GazeboでBlock Laserを使用する方法を調査しました。ドキュメントには、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
frameName	gazebo_ros_block_laser.cpp#L104
topicName	gazebo_ros_block_laser.cpp#L112
gaussianNoise	gazebo_ros_block_laser.cpp#L120
hokuyoMinIntensity	gazebo_ros_block_laser.cpp#L128
updateRate	gazebo_ros_block_laser.cpp#L138

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

sensorに設定するパラメータ

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

typeを調べる

まず設定すべきtypeを調べます。この仕組みは少々複雑ですが、gazebo_ros_block_laser.cpp#L95でRaySensorにダイナミックキャストをしているため、_parentは、RaySensorと想定されます。

RaySensor.cc#L46では、rayという名称でGazeboに登録していることがわかります。このマクロは、SensorFactory.hh#L88に実装されており、SensorFactory::RegisterSensorが定義されることがわかります。これは、SensorFactory.cc#L67でsensorMapに登録されており、SensorFactory.cc#L45で定義されているSensorFactoryFnは、Sensor*であることがわかります。

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

laserShapeを調べる

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

sensorパラメータ一覧

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

名称	親	ソースコード
ray	-	MultiRayShape.cc#L56
scan	ray	MultiRayShape.cc#L57
horizontal	scan	MultiRayShape.cc#L58
samples	horizontal	MultiRayShape.cc#L221
resolution	horizontal	MultiRayShape.cc#L227
min_angle	horizontal	MultiRayShape.cc#L233
max_angle	horizontal	MultiRayShape.cc#L239
vertical	scan	MultiRayShape.cc#L61
samples	vertical	MultiRayShape.cc#L246
resolution	vertical	MultiRayShape.cc#L255
min_angle	vertical	MultiRayShape.cc#L264
max_angle	vertical	MultiRayShape.cc#L273
range	ray	MultiRayShape.cc#L59
min	range	MultiRayShape.cc#L203
max	range	MultiRayShape.cc#L209
resolution	range	MultiRayShape.cc#L215
noise	ray	RaySensor.cc#L105
noise詳細	noise	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です。

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

今回は、ソースコードから調査しましたが、Gazeboの仕組みさえ理解していればSDFormatから設定パラメータ一覧を確認することができます。

Velodyneについて

Velodyneを使用している場合、専用のプラグインが存在します。なぜGazeboに共通プラグインが存在するのにvelodyneが独自のプラグインが存在するかですが、RaySensorの処理方法が異なります。下記にlaserの例を示します。

プラグイン	概要
gazebo_ros_laser	sensors::RaySensorを用いてsensor_msgs::LaserScanをpublishする
gazebo_ros_gpu_laser	sensors::GpuRaySensorを用いてsensor_msgs::LaserScanをpublishする

これまでみてきたようにBlock Laserは、RaySensorを用いて演算をおこなっていましたが、Velodyneのプラグインについては、GAZEBO_GPU_RAYで選択ができるようになっています。ただしKnown Issuesにあるとおりシミュレーションをするには、いくつか問題があるようです。わたしの環境（ROS Kinetic Kame & Gazebo9）では、こちらのプラグインがインストールできず実行はできませんでしたので詳細な確認はしていません。

まとめ

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