目的
自律移動ロボットが要求される技術の一つに指定した位置へ移動するナビゲーション機能あります．ナビゲーションを実現するためには，ロボット自身が地図上のどこに存在するかを推定し，障害物を回避しながらロボットにとって最適な経路を計画，そして経路通りにロボットを走行させる必要があります．ROSではナビゲーション関連のパッケージが用意されているため，簡単に試すことができます．今回はロボットシミュレータstageを用いてナビゲーションを体験します．
準備
今回はstageシミュレータを使用します．2次元のスキャンデータが取得可能であれば，stage以外でも利用可能です．
$ rosrun stage_ros stageros $(rospack find stage_ros)/world/willow-erratic.world
地図の作成
まずは地図を作成しましょう．ROSでは，地図を作成するパッケージが多数公開されていますが，今回はgmappingパッケージを使用します．2次元のスキャンデータを引数として指定する必要があるため，トピックbase_scanを指定しましょう．
$ rosrun gmapping slam_gmapping scan:=base_scan
上記のコマンドで地図を作成することができます．作成した地図を確認するために，可視化ツールであるrvizを起動しましょう．ロボットシミュレータstageの設定に合わせたファイルが用意されていますので，引数として指定します．
$ rosrun rviz rviz -d `rospack find stage_ros`/rviz/stage.rviz
rvizが問題なく起動したら地図情報を可視化しましょう．可視化するためには，rvizの左下のDisplays=>addボタンをクリック，Mapの項目を追加してTopicに/mapを設定しましょう．
2次元のレーザースキャンが取得できる範囲の地図が作成できています．地図の見方については，下記の画像を参考にしてください．
最後に下記によってロボットを移動させ，広範囲の地図を作成してください．
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
下記の画像は，ロボットシミュレータstageと可視化ツールrvizで作成した地図を並べて表示したものです．今回はシミュレータを利用しているため，センサから取得できるデータにノイズが含まれていません．また，車輪のスリップ等も発生しません．そのため，綺麗な地図が作成できていることが確認できます．
地図を作成できたら保存しましょう．下記のコマンドにより地図を作成できます．
$ mkdir ~/map $ cd map $ rosrun map_server map_saver
上記のコマンドを実行すると，map.pgmとmap.yamlの2つが保存されます．この2つはナビゲーションを行うために必要なファイルです．
これで地図の作成は完了です．ここまでの作業で複数のノードを起動しましたので，rqt_graphによってノードとトピックの関係を確認してみましょう．
/teleop_twist_keyboardからキーに応じた/cmd_velが配信されます．そしてそのトピックをロボットシミュレータ/stagerosが購読してロボットが動きます．ロボットからは，ロボット座標系/tfと2次元のスキャンデータ/base_scanが配信され，/slam_gmappingが購読します．そして，これら2つのトピックから地図を作成し，作成した地図は/mapとして配信されていることがわかります．
ナビゲーション
地図を作成できたらナビゲーションを試しましょう．ナビゲーションに必要なノードを起動します．ノード起動時の引数が多いため，下記のようなlaunchファイルを作成しましょう．今回はnavigation.launchという名前でlaunchファイルを作成します．
<launch>
<param name="/use_sim_time" value="true"/>
<include file="$(find navigation_stage)/move_base_config/move_base.xml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(env PWD)/map.yaml" respawn="false" />
<node pkg="stage_ros" type="stageros" name="stageros" args="$(find stage_ros)/world/willow-erratic.world" respawn="false" >
<param name="base_watchdog_timeout" value="0.2"/>
</node>
<include file="$(find navigation_stage)/move_base_config/amcl_node.xml"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation_stage)/single_robot.rviz" />
</launch>
作成したlaunchファイルを起動します．
$ roslaunch ./navigation.launch
問題なくノードが起動するとrvizが起動します．起動時には，地図上のロボットの位置と実際の位置にズレが生じますのでロボットの位置のズレを解消する必要があります．解消するためには，rvizの上部にある「2D Pose Estimate」をクリックし，ロボットの位置・姿勢を修正しましょう．下記の動画のように，地図上の障害物と2次元のレーザーの点群が一致するように修正すると良いです．
位置・姿勢の微調整は難しいですが，多少のズレであればROS側で修正するため今回は大まかに点群が一致すれば問題ありません．位置・姿勢の修正が完了したらナビゲーションを試しましょう．rvizの上部にある「2D Nav Goal」で目標位置・姿勢を設定してナビゲーションを試してください．可視化すると良い情報が多数ありますので，下記の動画を参考にしましょう．
最後に，少し複雑ですがナビゲーション時のノードとトピックの関係をrqt_graphによって確認しましょう．
先ほどと同様に，ロボットシミュレータ/stagerosからロボット座標系/tfと2次元のスキャンデータ/base_scan，そしてロボットの移動量を表すオドメトリ情報/odomが配信されます．そのうちのロボットの座標系とセンサ情報が/amclに購読されます．この/amclノードは，センサ情報や地図を用いてロボットの自己位置を推定するノードです．また，rvizから初期位置を設定した際には，/initialposeというトピックを購読して，指定した位置・姿勢となるよう修正します．
次に，/odomと修正されたロボットの座標系/tf，2次元のスキャンデータ/base_scan，地図情報/mapを/mobe_base_nodeが購読していることがわかります．この/move_base_nodeは，現在位置から目標位置までの経路を計画して，移動するためにロボットに対して並進・角速度(cmd_vel)を配信しています．また，/move_base_nodeという名前空間がつけられたトピックが数多く存在します．これらは経路や経路計画に必要なコストマップ等を表しています．これらの情報は，/move_base_node内で使用するため配信不要ですが，rvizで情報を可視化することができるため，使用時だけグラフに表れます．ちなみに，/move_base_simple/goalというトピックが存在しますが，こちらはrvizから目標位置を設定した際に/move_base_nodeが購読して，経路計画を行います．
今回は，ロボットの初期位置と目標位置をrvizで設定しました．これらを設定すると，それぞれ/initialposeと/move_base_simple/goalというトピックで配信されていることがわかります．ナビゲーション機能を備えたソフトウェアを開発したい場合には，これらのトピックをプログラムから配信すれば良いことがわかります．