ROS navigationパッケージを使ってみよう

navigationパッケージは、自己位置推定、大域的な経路計画、局所的な動作計画など、地図ベースの自律走行に必要なノードを含むパッケージ群で、世界中のROS開発者によって作成・管理されています。世界中の英知の塊とも言える、navigationパッケージですが、含まれるソフトウェアの内側ではアドホックな構造やパラメータが多数あり、まともに動く(例えばつくばチャレンジを完走する)ようにチューニングするのは、(これら全てを自分で実装できるくらいの)知識と経験が必要です。

本セミナーでは、地図作成と、自律ナビゲーションを、簡易的に調整済みのlaunchファイルを利用して動作させます。

下記コマンドを用いて、地図生成に用いる、slam_gmappingパッケージと、マウスでロボットを操縦するmouse_teleopパッケージをインストールします。navigationパッケージは、サイズが非常に大きいため、Live USBに既にインストールしてあります。

$ sudo apt-get install ros-indigo-slam-gmapping ros-indigo-mouse-teleop

また、セミナー教材用にパラメータを調整してある地図生成や自律ナビゲーションのlaunchファイルが入ったパッケージをダウンロードします。

$ cd ~/catkin_ws/src/

$ git clone https://github.com/at-wat/rsj_seminar_navigation

ダウンロードしたrsj_seminar_navigationパッケージを、catkin_makeでビルドします。

$ cd ~/catkin_ws/

$ catkin_make

まず、地図ベースの自律ナビゲーションを実現するために、slam_gmappingパッケージを用いて地図を作成します。PCにロボットとURGのUSBケーブルを接続し、地図を生成したい場所にロボットを置いて、下記のコマンドを実行します。

$ roslaunch rsj_seminar_navigation mapping.launch robot_param:=/home/ubuntu/params/rsj-seminar20??.param該当するものに置き換えること

rvizが起動し、下記のように複数のURGのスキャンデータをつなげて、大きな占有格子地図を生成し始めます。

「Mouse Teleop」のウインドウ内をマウスでドラッグ操作することで、ロボットの動作を制御できますので、地図を作りたい範囲を移動させてみましょう。「Mouse Teleop」のウインドウが隠れている場合は、左の「？」アイコンをクリックすることで最前面に表示できます。

この地図生成は、URGのスキャンデータを逐次つなげていく仕組みのため、ロボットの位置姿勢を大きく変化させた場合や、センサに見えているものが少ない場合には、地図が破綻する場合があります。その際は、roslaunchを実行した端末で、Ctrl+cを押して終了し、もう一度実行し直します。

ロボットを走行させていくと、図のように、走行させた範囲の地図が表示されます。

必要な範囲の地図ができあがったら、端末をもう一つ開き、下記のコマンドで、保存するディレクトリを作成し、地図データをファイルに出力します。

$ mkdir ~/maps

$ cd ~/maps/

$ rosrun map_server map_saver

画面左の「ホームホーフォルダー」アイコンから、ホームディレクトリ下の、「maps」ディレクトリを見ると、map.yamlと、map.pgmファイルが生成されていることが確認できます。

map.yamlをダブルクリックして開くと、地図の解像度や原点の情報が書かれています。また、map.pgmを開くと、地図データが画像ファイルとして保存されていることがわかります。

地図の画像ファイルが正しく出力できていることを確認したら、roslaunchを実行した端末のウインドウを選択して、Ctrl+cを押して終了します。

先ほど作成した地図を用いて、自律ナビゲーションを試してみましょう。PCにロボットとURGのUSBケーブルを接続し、地図を作成した際の初期位置・姿勢と同じようにロボットを置いて、下記のコマンドを実行します。

$ roslaunch rsj_seminar_navigation navigation.launch robot_param:=/home/ubuntu/params/rsj-seminar20??.param該当するものに置き換えること

rvizが起動し、下記のように先ほど作成した地図が表示されます。この例では、地図と、オレンジ色でプロットされているURGの現在のデータがずれており、自己位置がずれていることがわかります。また、水色のたくさんの矢印は、推定している自己位置の候補を表しています。

rvizのウインドウ上中央にある、「2D Pose Estimate」ボタンを押し、ロボットの本来の位置から、ロボットの向いている方向に向かってドラッグすると、緑色の矢印が現れ、自己位置推定ノードに位置姿勢の修正を指示することができます。

地図と、URGのデータが概ね一致しました。

次に、ウインドウ上中央の、「2D Nav Goal」ボタンで、ナビゲーションのゴールを指定します。同様に、与えたいゴールの位置から、目標姿勢の方向に向かってドラッグすると、緑色の矢印が現れ、ナビゲーションのノードに目標位置姿勢の指示を与えることができます。

すると、緑色の線でグローバルプランナーが生成したパス、赤色の線でローカルプランナーが生成したパスが表示され、ロボットが走行を開始します。

走行していくと、水色の矢印で表された、自己位置の候補の分布が小さくなり、自己位置推定の確度が高くなったことが確認できます。

ゴールに到着し、姿勢を目標の方向に向けると、動作が終了します。

この状態で、どのようなノードが立ち上がっているのか、確認してみましょう。新しい端末を開き、rqt_graphを実行します。

$ rqt_graph

左上の、「Nodes only」の部分で、「Nodes/Topics (active)」を選択します。

丸枠で書かれた名前は、ノードを表しており、下記の仕事をしています。

ypspur_ros

ロボットの制御

urg_node

URGデータの取り込み

map_server

地図ファイルの読み取り

amcl

自己位置推定(モンテカルロローカライゼーション、いわゆる、パーティクルフィルター)

move_base

ナビゲーション(プラグインで、ダイクストラ法のグローバルプランナーと、ダイナミックウインドウアプローチのローカルプランナー、2Dコストマップの処理を実行)

visualizer

データの可視化(rviz)

stp_laser

ロボットの座標原点とURGの座標原点の座標変換の定義

四角枠で書かれた名前は、トピックを表しています。

rsj_seminar_navigationパッケージのlaunchファイルの中身を開き、どのようなノードが実行されているのか、講義の内容と照らし合わせて確認してみましょう。