シミュレータ上のラズパイマウスを動かす方法 Part6

ラズパイマウスを動かすまでの流れ

1. 距離センサの値の読み取り方

2. モータの動かし方

3. キーボードを用いたラズパイマウスの動かし方

4. コントローラを用いたラズパイマウスの動かし方

5. 距離センサの値を利用したモータの動かし方

6. 測域センサ(URG)を用いたSLAMの行い方 ←今ここ

Part6では測域センサ(URG)を用いてSLAMを行っていきましょう。

URGとは、北陽電気株式会社さんから発売されている測域センサで、前方240°を検出できます。

公式サイト : URG-04LX-UG01

SLAMについて

SLAMとは、Simultaneous Localization and Mappingの略で、自己位置推定と地図生成を同時に行うものです。

SLAMにはLIDER(Light Detection and Ranging)呼ばれるセンサを使用します。 上記のURGもLIDERの一種です。

SLAMを行うときはtfとscanという2つのデータが必要になります。

大まかな流れ

tf

ロボットを動かす上で必要になる、ロボットの位置姿勢や座標を変換したり配信しているトピックです。 ロボットの移動量(odometry, odom)から座標変換したパラメータを使用します。 tfトピックで配信しています。

scan

URGから取得できるセンサ値を含まれているトピックです。scanトピックで配信しています。

SLAM

自己位置推定と地図生成を同時に行っています。 地図生成に関してはtfトピックとscanトピックから、地図を生成するためのmapデータを生成しています。

map_server

SLAMによって出来たmapデータから地図画像(.pgm)と地図データ(.yaml)を生成しています。 他に地図画像(.pgm)と地図データ(.yaml)より地図情報をmapトピックで配信することが出来ます。

前準備

まずURGを動かすためのROSパッケージをインストールします。 すでにurg_nodeをインストールしている場合は行わなくて大丈夫です。

sudo apt install ros-kinetic-urg-node

次に、SLAMのROSパッケージをインストールします。

sudo apt install ros-kinetic-slam-gmapping

最後にmap_serverをインストールしましょう。

sudo apt install ros-kinetic-map-server

以上で準備完了です。

トピックの確認

URGを使用するには先程インストールしたurg_nodeが必要になりますが、Gazeboを起動すると同時に起動します。

URG付きラズパイマウスは以下のコマンドで起動します。

roslaunch raspimouse_gazebo raspimouse_with_willowgarage.launch

tfの相対座標変換の見てみましょう。

roslaunch raspimouse_ros_2 raspimouse.launch
rosrun rqt_tf_tree rqt_tf_tree

odomから座標変換されてbase_linkになり、さらに座標変換して枝分かれしてることがわかります。 これらの情報をtfトピックは配信しています。

次にURGから配信されるセンサ値を確認してみましょう。

センサ値は/scanトピックで配信されます。

rostopic list

と打ち、/scanトピックがあることを確認し、

rostopic echo /scan

と打つと、数字がたくさん表示されると思います。

上に遡って見てみると以下のようになっています。

画像の ranges: [inf,inf, ... ]の部分がセンサ値になります。カンマで区切られている値1つ1つがセンサ値で、URGが検出できる前方240°分あります。

センサ値が大きいほどURGと壁や物との距離が遠く、値が小さいほど距離が近いです。また遠すぎするとinf(infinity)が出ます。

センサ値の可視化

センサ値の可視化にはRvizというGUIツールを使用します。 Rvizの起動には以下のコマンドを使用します。

rviz rviz

起動したら、Fixed Frameのurg_lrf_linkに変更します。

次に画面左下のAddを押しBy display typeのLaserScanを追加し、Topicを/scanにします。

起動時のシミュレータは以下の図のようになっています。 上のRvizの図と見比べると、赤い点群がシミュレータの壁と対応していることがわかります。

gmappingのパラメータを設定

gmappingはSLAMのアルゴリズムの1つで、正確に地図作成するために様々なパラメータを設定する必要があります。パラメータはlaunchファイルに記述します。

今回は、raspimouse_sim_gmapping.launchという名前にします。

<launch>
 <include file="$(find raspimouse_game_controller)/launch/run_with_base_nodes.launch" />

<!-- <node pkg="urg_node" name="urg_node" type="urg_node" required="true" > -->
<!--  <param name="frame_id" value="base_link" /> -->
<!-- </node> -->

 <node pkg="rviz" type="rviz" name="rviz" args="-d $(find raspimouse_sim_tutorial_program)/config/gmapping.rviz" /> 

 <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
  <param name="base_frame" value="base_link" />
  <param name="odom_frame" value="odom" />

  <param name="maxUrange" value="4.0" />
  <param name="maxRange" value="4.0" />

  <param name="xmin" value="-15" />
  <param name="ymin" value="-15" />
  <param name="xmax" value="15" />
  <param name="ymax" value="15" />

  <param name="srr" value="0.1" />
  <param name="srt" value="0.1" />
  <param name="str" value="0.1" />
  <param name="stt" value="0.1" />

  <param name="particles" value="30" />
 </node>
</launch>

コード解説

 <include file="$(find raspimouse_game_controller)/launch/run_with_base_nodes.launch" />

チュートリアルのコントローラを用いたラズパイマウスの動かし方で使用した、ロボットをコントローラで動かすためのlaunchファイルを起動しています。

<!-- <node pkg="urg_node" name="urg_node" type="urg_node" required="true" > -->
<!--  <param name="frame_id" value="base_link" /> -->
<!-- </node> -->

urg_node を起動するための記述ですが、 urg_nodeはGazeboの起動と同時に立ち上がるため、コメントアウトしています。

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find raspimouse_sim_tutorial_program)/config/gmapping.rviz" />

Rvizを起動します。 今回はgmapping用のRvizの設定(gmapping.rviz)を作っておいたので、こちらを使用します。

 <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
  <param name="base_frame" value="base_link" />
  <param name="odom_frame" value="odom" />

  <param name="maxUrange" value="4.0" />

  <param name="xmin" value="-15" />
  <param name="ymin" value="-15" />
  <param name="xmax" value="15" />
  <param name="ymax" value="15" />

  <param name="srr" value="0.1" />
  <param name="srt" value="0.1" />
  <param name="str" value="0.1" />
  <param name="stt" value="0.1" />

  <param name="particles" value="30" />
 </node>

ここでgmappingのパラメータの設定をしています。

base_frameはロボットのメインフレームでbase_link、 odom_frameはオドメトリのフレームでodomを設定しています。

maxUrangeでURGの最大測定範囲を設定しています。

xminとymin、xmax、ymaxで地図の大きさを設定しています。

以下の4つはそれぞれオドメトリの誤差の設定をしています。この値が大きすぎても小さすぎても正確な地図は出来ません。今回はすべて0.1にしました。

srr：移動したときの移動の誤差

srt：移動したときの回転の誤差

str：回転したときの移動の誤差

stt：回転したときの回転の誤差

particlesはパーティクルの数を設定しています。この値が大きいほど正確な地図ができやすいですが、計算量が増えてしまいます。今回はデフォルトの30にしました。

SLAMを行う

まず以下の2つを起動させます。

roslaunch raspimouse_gazebo raspimouse_with_willowgarage.launch

roslaunch raspimouse_sim_tutorial_book_sample_program raspimouse_sim_gmapping.launch

起動したら、コントローラを使用してラズパイマウスを動かして地図を作っていきます。 初期位置にいる部屋を壁に沿って一周してみましょう。

一周させたら地図を保存しましょう。下のコマンドではroomという名前で保存しています。

roscd raspimouse_sim_tutorial_program/
rosrun map_server map_saver -f map/room

実行結果

行っている様子

YouTubeで公開しています。

[Raspberry Pi Mouse Simulator] チュートリアルPart6 測域センサ(URG)を用いたSLAMの行い方