Navigation Stack を理解する - 1. 導入

# はじめに
ROSには，ロボットの自律移動を実現する[Navigation Stack](http://wiki.ros.org/navigation) という枠組みが存在します．

![00_overview_tf.png](https://qiita-image-store.s3.amazonaws.com/0/110138/9509eeaa-dac2-da6d-5e64-8a25bd24250c.png)

ROSのド素人である私がこれと出会ったとき，こう思いました．

「わっけわっかんねーw」

不束者の私は，経路探索，自己位置推定，地図生成などの機能に対して，種々のセンサが情報がどのように作用しているのか，中々全体像が把握できないまま何となく使わせてもらっていました．

このままではいかん！というわけで，Navigation Stack のソフトウェアの構成と，各モジュールがやりとりするメッセージの関係性がある程度分かるようになることを目標に，メモを書きます．

特に，自己位置推定と地図生成については，その原理を直感的に理解できるようにイラストを使った説明を試みます．これらの仕組みは[S Thrunら, Probabilistic Robotics](http://www.probabilistic-robotics.org/)を全面的に参考にさせて頂きました．当方の記事に認識の誤り，ご指摘がございましたら，ご指摘を頂けましたら幸いです．

# 目次
1. [導入](http://qiita.com/MoriKen/items/0b75ab291ab0d95c37c2)
2. move_base (ナビゲーション)
 3. [ROSで遊んでみる](http://qiita.com/MoriKen/items/8387b279e968368783f1)
 4. [ソフトウェア構成をみる](http://qiita.com/MoriKen/items/d5cd6208143d6c40caff)
3. amcl (自己位置推定)
 4. [ROSで遊んでみる](http://qiita.com/MoriKen/items/87be494f048ea03fdab8)
 4. [ソフトウェア構成をみる](http://qiita.com/MoriKen/items/26483e86fb556f0dc5ce)
 5. [原理をみる (準備編)](http://qiita.com/MoriKen/items/239f69df2921cc64bd62)
 6. [原理をみる (応用編)](http://qiita.com/MoriKen/items/dfb6eb168649873589f0)
4. gmapping (地図生成)
 4. [ROSで遊んでみる](http://qiita.com/MoriKen/items/74cbff9198f82f2b7ea8)
 4. [ソフトウェア構成をみる](http://qiita.com/MoriKen/items/65ac41abca9fdf6051be)
 6. [原理をみる(応用編)](http://qiita.com/MoriKen/items/0f2550a2adbdcd3da04e)
 5. 原理をみる(準備編 その1)
 5. 原理をみる(準備編 その2)

- Navigation Stack の中心要素である，ナビゲーション，自己位置推定，地図生成の各節について，ROSのパッケージで遊んでみる項，ソフトウェア構成をみる項，その原理をみる項に分けて記述します．

- ナビゲーションについては，[原, "ROSの活用による屋外の歩行者空間に適応した自律移動ロボットの開発", ロボット工学セミナー, 2015.](http://www.slideshare.net/hara-y/ros-slam-navigation-rsj-seminar) という大変分かりやすい資料があり，現状における私の理解ではこの資料以上の情報を提供できないので，当該節について詳細な原理については書きません．

- 他の節についても，もちろん[原先生の資料]((http://www.slideshare.net/hara-y/ros-slam-navigation-rsj-seminar))で素晴らしい解説がされているので，私のようなド素人がかけることは少ないのです．

- そこで，ここでは私のような本当に無知な人間が挙動を直感的に理解するために必要となりそうな，もう少し細かいステップまで分解したイラストをを付与して説明することにフォーカスしました．他の資料と少しオーバラップしてしまう部分はありますが，説明のために必要な量にすることを心がけます．

## 使用するロボットモデル
ここでは，[Husky](http://wiki.ros.org/Robots/Husky) という4輪ロボットのモデルをGazebo上で動かしながら理解を深めようと思います．[husky_navigation](http://wiki.ros.org/husky_navigation) のチュートリアルを参考に進めてゆきます．

![husky-grass](http://wiki.ros.org/Robots/Husky?action=AttachFile&do=get&target=husky-grass.png)

# Navigation Stack でやりたいこと
「ロボットの自動運転を実現したい」．これに尽きます．

実現されている技術で近い例として，カーナビが挙げられます．行きたい場所を入力すれば現在地からのルートを探索し，交差点での進行方向を示してくれます．これは，目標位置，車の現在位置，道路地図といった情報を利用することで実現されています．

ただし，アクセルを踏んでハンドルを回すのは人間です．これをも自動化してくれるのが「Navigation Stack」の仕組みです．ここをロボットにやらせるだけで気をつけなければならないことが色々あるのですが，基本的な機能はこの中で網羅されており，ROSで自動運転をやるならまずはコレ！というレベルの重要な要素となっているようです．

自律運転の実証実験プロジェクトである[つくばチャレンジ](http://www.tsukubachallenge.jp/)でも，ベースとしてNavigation Stack を使っているチームが存在します．

昨今開発が盛んになっている自動運転技術の中核となる技術，何となくでも知っておきたいですね．

# ROSの各パッケージの位置づけ
さて，ROSパッケージの位置づけについて確認します．これも，[原, ROSの活用による屋外の歩行者空間に適応した自律移動ロボットの開発](http://www.slideshare.net/hara-y/ros-slam-navigation-rsj-seminar)で詳細に解説されているのですが，ここではそれを参考にして，ド素人の私自身がとりあえず全体を把握することを目標にした記述をします．

最も本的な要素に分解すると，2つに大別されます．ナビゲーション関連のパッケージ一式を格納した[navigation](http://wiki.ros.org/navigation)メタパッケージと，地図生成を司る[slam_gmapping](http://wiki.ros.org/slam_gmapping)メタパッケージです．

![01_パッケージ.png](https://qiita-image-store.s3.amazonaws.com/0/110138/64ec689e-134e-6169-2957-48c26b3f9a70.png)

手順としては，次のように行うのが一般的です．

1. gmapping で地図を作成してデータとして保存する
1. move_base, amcl, map_server を使って精度の良いナビーションをする

ただし，上図は説明のために簡略化されています．こんなふうに描くと，まるでgmapping がnavigation と完全に独立した存在の様に見えてしまいますが，そうではありません．gmapping で地図を作成するときは自己位置推定もしており，この情報を使ってnavigation することができます．また，作成した地図はmap_server を使って保存します．したがって，地図生成時のソフトウェア構成は，もう少し正確に描くと，下図のようになります．

![01_パッケージ2.png](https://qiita-image-store.s3.amazonaws.com/0/110138/0f3e78f2-436b-8958-db96-5ee35939f075.png)

何となく図を眺めて，ふむそんなものかとなった（？）ところで，各パッケージの概要を以下に示します．

## メタパッケージ：[navigation](http://wiki.ros.org/navigation?distro=jade)
内部には更に複数のROSパッケージが含まれています．コアとなるのが，[move_base](http://wiki.ros.org/move_base?distro=jade)(経路計画とナビゲーション)，[amcl](http://wiki.ros.org/amcl)(自己位置推定)，[map_server](http://wiki.ros.org/map_server?distro=jade)(地図管理)のパッケージです．特に親分的な役割を担うのが`move_base`です．
地図と自己位置推定の部分は実はオプションという位置づけなので好きな手法を当てはめてよいのですが，ここでは最も一般的な`amcl`と`map_server`を挙げておきます．

### パッケージ：[move_base](http://wiki.ros.org/move_base?distro=jade)
目標位置と自己位置を元に，経路計画とナビゲーションを行ってくれます．これは，`amcl`と`map_server`がなくても単独で動かすことはできます．親分ですから．ただし，それは人が目をつぶったままお出かけするような状態に相当し非常に危ないので，普通は地図と自己位置推定という子分を組み込んで，より正確なナビゲーションができるようにします．

### パッケージ：[amcl](http://wiki.ros.org/amcl)
地図，動作モデル，センサ情報を使って自己位置推定を行うパッケージです．[パーティクルフィルタ](https://ja.wikipedia.org/wiki/粒子フィルタ)を活用した自己位置推定法で，環境の変化に強いという特長があります．

### パッケージ：[map_server](http://wiki.ros.org/map_server)
地図を管理するパッケージです．`move_base`や`amcl`といった他のノードから地図を参照できるように地図情報を提供してくれます．地図のデータは画像形式で提供されます．

## メタパッケージ：[slam_gmapping](http://wiki.ros.org/slam_gmapping)
ここまでの説明では，地図が天下りで登場していますが，当然地図だって作る必要があるわけです．そこで，SLAM (Simultaneous Localization And Mapping) という技術で地図を作成するのが，本メタパケージです．

### パッケージ：[gmapping](http://wiki.ros.org/gmapping)
現状，`slam_gmapping`の中には，`gmapping`というパッケージのみ存在します．`gmapping`では，動作モデルとセンサ情報を元に地図を作成します．ここでも中核をなす技術は[パーティクルフィルタ](https://ja.wikipedia.org/wiki/粒子フィルタ)です．パーティクルフィタの特長については，[MyEnigma-Particle Filterを使用した自己位置推定MATLABサンプルプログラム](http://myenigma.hatenablog.com/entry/20140628/1403956852) で簡潔に分かりやすくまとめられています．

SLAMのROSパッケージとして有名なものとして，[hector_slam](http://wiki.ros.org/hector_slam)が挙げられます．この手法の特長は，`gmapping`では必要だった動作モデルとオドメトリの情報は不要となり，センサ情報だけで地図を作成するということです．

この記事では，前者の`gmapping`だけを扱います．`gmapping`の方が`amcl`からの類推が直接的で，更にパーティクルフィルタの理解に役立つからとも言えますし，当方が`hector_slam`を使ったことが無いからというしょぼい理由がメインかもしれないですし．まぁ，いいです笑．

## おわりに
以上が全体像です．ここまで認識した上で，もう一度冒頭の図を見てみると，少し分かった気になれます．

![02_overview_2.png](https://qiita-image-store.s3.amazonaws.com/0/110138/28671c56-7a58-a136-d16a-69fc14911a5a.png)

これから複数回に渡って，少しずつその中身を紐解いていこうと思います．小生のつたない文章にお付き合い頂ければ幸いです．

Next: [2.1 move_base: ROSで遊んでみる](http://qiita.com/MoriKen/items/8387b279e968368783f1)

## 参考文献
1. [S Thrun, et al., "Probabilistic Robotics", the MIT Press, 2005.](http://www.probabilistic-robotics.org/)
1. [原, "ROSの活用による屋外の歩行者空間に適応した自律移動ロボットの開発", ロボット工学セミナー, 2015.](http://www.slideshare.net/hara-y/ros-slam-navigation-rsj-seminar)
1. [Atsushi(ブログプロフィールより), "Particle Filterを使用した自己位置推定MATLABサンプルプログラム", MyEnigma, 2014.](http://myenigma.hatenablog.com/entry/20140628/1403956852)
1. [ROS Wiki.](http://wiki.ros.org/)
1. ["ros-planning/navigation", github.](https://github.com/ros-planning/navigation)
1. ["ros-perception/slam_gmapping", github.](https://github.com/ros-perception/slam_gmapping)
1. ["tu-darmstadt-ros-pkg/hector_slam", github.](https://github.com/tu-darmstadt-ros-pkg/hector_slam)
1. ["粒子フィルタ", Wikipedia.](https://ja.wikipedia.org/wiki/粒子フィルタ)