環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Noetic |
| Gazebo | 11.9.0 |
インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。
概要
ロボットで使えるセンサーとしてLidarがあります。Lidarはレーザーを使ったRadarのようなもので周りの物との距離を測ることができます。Lidarがあると周りの障害物を検知することができるので便利です。
また似たようなものとして超音波や赤外線で距離を測るレンジファインダーというセンサーもあります。今回はこれらgazebo上でシミュレーションします。
Lidarについて
レーザーで距離を測るセンサーは大きく分けて1次元、2次元、3次元の物の3種類に分けられます。
- 1次元Lidar
センサーが向いている1点との距離を測るセンサーで、LRF(LaserRangeFinder)とも呼ばれます。レーザーではなく超音波を使ってたものもロボットではよく使われます。 - 2次元lidar
1次元Lidarを回転台に乗せて水平360度の方向が見えるようにしたものです。 - 3次元Lidar
360度回る回転台の上でさらに上下にLRFを揺らしたり(Hokuyoの物が有名)、縦に16や32個のLRFを並べたものを回転させるものがあります(Velodyneの物が有名)。
方式としては別ですが、似たような機能を持つものとしてステレオカメラやDepthセンサーと呼ばれる種類の物（製品としてはKinectやRealSense）があります。
基本的に車輪ロボットは2次元平面しか動かないことや価格や性能の点で、2次元Lidarがロボットにはよく使われます。
Lidarのシミュレーション
SDFファイルでの例
gazebo2_lecture/models/wheel_robot_sensor/model.sdfの一部
<link name="front_laser_link">
<!-- 省略 -->
<sensor type="ray" name="front_laser">
<always_on>true</always_on>
<update_rate>10</update_rate>
<visualize>true</visualize>
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>400</samples>
<resolution>1</resolution>
<min_angle>-3.1415</min_angle>
<max_angle>3.1415</max_angle>
</horizontal>
</scan>
<range>
<min>0.05</min>
<max>20.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_laser.so">
<topicName>/device/front_laser/scan</topicName>
<frameName>front_laser_link</frameName>
</plugin>
</sensor>
</link>
-
<sensor type="ray" name="head_lidar">はrayはLidarを表すクラスです。nameは自由につけてかまいません。 -
<visualize>false</visualize>はtrueにするとgazebo上でlidar光線が表示されます。 -
<horizontal>タグの中身は-
<samples>400</samples>は1週で何個の点をサンプルするかという設定 -
<min_angle>と<max_angle>はスキャンする範囲の設定（例えばmin:-90, max:90だと前180度だけが範囲のLidarになる）
-
-
rangeタグの<min>と<max>はスキャンが有効な範囲です。Lidarなら最大射程は短くても20m程度はあります。minを小さくしすぎると自分のLidar_linkのロボットモデルに当たってしまうので、0.05mぐらいの適切な値を入れましょう。 -
<noise>の中ではセンサーの測定値に加えるノイズ（誤差）を設定します。 -
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">ではnameは好きなものをいれてかまいません。
urdfファイルでの例
基本的に<gazebo>タグの中でSDFと同様の表記をします。
sim2_lecture/xacro/laser_macro.xacro
<robot name="dtw_robot" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="laser_macro" params="parent prefix xyz">
<joint name="${prefix}_joint" type="fixed">
<parent link="${parent}"/>
<child link="${prefix}_link"/>
<origin xyz="${xyz}" rpy="0 0 0"/>
</joint>
<link name="${prefix}_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.02" length="0.06" />
</geometry>
<material name="red" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.02" length="0.06" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}_link">
<material>Gazebo/Red</material>
<sensor type="ray" name="${prefix}">
<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>${radians(-90)}</min_angle>
<max_angle>${radians( 90)}</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>20.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_laser.so">
<topicName>/${prefix}_link/scan</topicName>
<frameName>${prefix}_link</frameName>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
launchファイル
上記のSDFをロードしたworldファイルを使ってGazeeboを起動&Rvizを起動します。
gazebo2_lecture/launch/wheel_robot_sensor.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="model" default="$(find gazebo2_lecture)/urdf/wheel_robot_sensor.urdf"/>
<arg name="rvizconfig" default="$(find gazebo2_lecture)/rviz/wheel_robot_sensor.rviz" />
<param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="wheel_robot_sensor.world"/>
<arg name="verbose" value="true"/>
</include>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
Rvizでの表示
Lidarプラグインを使って表示をすることが出来ます。デフォルトだとマーカーのサイズが小さくて見ずらいので「Size」の項目の調整をお勧めします。
Rangeセンサーのシミュレーション
SDFファイルでの例
gazebo2_lecture/models/wheel_robot_sensor/model.sdfの一部
<link name="left_back_sonar_link">
<!-- 省略 -->
<sensor type="ray" name="sonar1">
<always_on>true</always_on>
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>3</samples>
<resolution>1</resolution>
<min_angle>-0.05</min_angle>
<max_angle>0.05</max_angle>
</horizontal>
<vertical>
<samples>3</samples>
<resolution>1</resolution>
<min_angle>-0.02</min_angle>
<max_angle>0.02</max_angle>
</vertical>
</scan>
<range>
<min>0.2</min>
<max>4</max>
<resolution>0.02</resolution>
</range>
</ray>
<plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
<robotNamespace></robotNamespace>
<frameName>left_back_sonar_link</frameName>
<topicName>left_back_sonar/range</topicName>
<radiation>ultrasound</radiation>
<fov>0.1</fov>
<gaussianNoise>0.005</gaussianNoise>
<updateRate>10</updateRate>
</plugin>
</sensor>
</link>
- lidarと同じように
rayのセンサータイプを使います。 -
libgazebo_ros_range.soのプラグインを使います。
Rvizでの表示
Rangeプラグインを使って表示をすることが出来ます。
実行
以下のコマンドを実行するといつものようにrvizとgazeboが立ち上がります。各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。
roslaunch gazebo2_lecture wheel_robot_sensor.launch
障害物の設置
デフォルトだと何も物体が無いのでlidarが反応しません。障害物としてコーラ缶を置いてみます。
以下のようにGazeboのInsertタブ開いてその中から「Coke Can」を選んでワールドの中をクリックします。リストの中にない場合はhttp://models.gazebosim.org/ の中にあるので、これを開いてとネット上からモデルのデータセットからダウンロードします（数分かかります）。
Rviz上でlidarのデータを表示するには左下の「Add」->出てくるウィンドウで「By topic」を押して「/front_laser_link/scan」を選択します。すると画面上にLidarの結果が点として現れます。
gpu_rayについて
gpuがある環境ではそれを使ってLidarのシミュレーションを行うことで処理を軽くできます。2次元Lidarは処理がそこまでは重くないのですが、3次元Lidarでは処理量が重いので必須です。gpuを使うときはurdfの記述を以下のように2行変更します。
gpuが無くてもgpu_rayは起動するのですが、値が表示されません。
sim_lecture/urdf/wheel_robot5.urdfのray->gpu_rayへの変更
<!--<sensor type="ray" name="head_lidar">-->
<sensor type="gpu_ray" name="head_lidar">
<!--<plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_laser.so">-->
<plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_gpu_laser.so">