Navigation Stack を理解する - 3.1 amcl: ROSで遊んでみる

# 目次
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 (地図生成)
 1. [ROSで遊んでみる](http://qiita.com/MoriKen/items/74cbff9198f82f2b7ea8)
 2. [ソフトウェア構成をみる](http://qiita.com/MoriKen/items/65ac41abca9fdf6051be)
 3. [原理をみる(応用編)](http://qiita.com/MoriKen/items/0f2550a2adbdcd3da04e)
 4. 原理をみる(準備編 その1)
 5. 原理をみる(準備編 その2)

# はじめに
本節ではNavigation Stack の子分的存在の一つである，[amcl](http://wiki.ros.org/amcl) について記述します．子分だからといって，技術的に劣っているとかそういうことではありません．move_baseという頭からみたらサポート役という位置づけであって，こちらも技巧的に組まれています．

やりたいことは，自己位置推定です．スリップ等でずれまくってしまうオドメトリ情報を，地図とセンサ情報を駆使して補正しようという発想です．
![07_現実はずれている.png](https://qiita-image-store.s3.amazonaws.com/0/110138/40a3eada-b7ed-a304-3f74-3e7cdeaea151.png)

さて，ソフトウェア構成内の位置づけとしては，下記の図に示すとおりです．

- ROS Wiki の図中
![01_位置づけ1.png](https://qiita-image-store.s3.amazonaws.com/0/110138/1f09797a-2a60-9fa6-4b8d-568dac4422f7.png)

- 導入部で示したパッケージの図中
![02_位置づけ2.png](https://qiita-image-store.s3.amazonaws.com/0/110138/9dfa4487-ab6f-52a2-e62c-9a438dc7e0bf.png)

# 環境
- OS: Ubuntu 14.04
- ROS: Indigo
- モデル：[Husky](http://wiki.ros.org/Robots/Husky)
- パッケージ：[husky_navigation](http://wiki.ros.org/husky_navigation)
- チュートリアル：[Husky AMCL Demo](http://wiki.ros.org/husky_navigation/Tutorials/Husky%20AMCL%20Demo)
![husky-grass](http://wiki.ros.org/Robots/Husky?action=AttachFile&do=get&target=husky-grass.png)

# Husky AMCL Demo について
[amcl](http://wiki.ros.org/amcl) を使った自己位置推定を体感します．ただし，地図は既に作成されたものを使用しています．

![03_チュートリアル位置づけ.png](https://qiita-image-store.s3.amazonaws.com/0/110138/39d4ef61-6311-0221-1226-f93511776366.png)

# ROSで遊んでみる
例によって，遊んでみましょう．

## インストール
[Navigation Stack を理解する - 2.1 move_base: ROSで遊んでみる](http://qiita.com/MoriKen/items/8387b279e968368783f1) でインストールしたパッケージで十分です．

## 各種launch ファイルの起動
i. Husky シミュレータの起動（gazebo）

```bash
$ roslaunch husky_gazebo husky_playpen.launch
```
![01.png](https://qiita-image-store.s3.amazonaws.com/0/110138/9507065d-fb12-948f-94e6-099662e1fdff.png)

ii. ビジュアライザの起動（rviz）

```bash
$ roslaunch husky_viz view_robot.launch
```
今回は地図を使いますので，ワールド座標系の原点は /map フレームとしておきます．初期状態では，/map フレームと /odom フレームと /base_link フレームは一致しています．

![05_rviz.png](https://qiita-image-store.s3.amazonaws.com/0/110138/9e95420c-cfcc-4524-657c-38bb70277092.png)

iii. move_base の起動

```bash
$ roslaunch husky_navigation amcl_demo.launch
```
これで，自己位置推定付きでナビゲーションが可能な状態となります．

## 自己位置推定を体感する
[Navigation Stack を理解する - 2.1 move_base: ROSで遊んでみる](http://qiita.com/MoriKen/items/8387b279e968368783f1) と同じ要領で目標を設定すれば，動きます．しばらく遊んでいると，いくつか違いに気がつきます．

### 大域的コストマップが地図全体に割り当てられる

[2.1 move_base: ソフトウェア構成をみる](http://qiita.com/MoriKen/items/d5cd6208143d6c40caff)では部分的にしか割あたらなかった大域的コストマップが，地図全体に行き渡っています．いい眺め（？）です．

![12.png](https://qiita-image-store.s3.amazonaws.com/0/110138/6cbc3d50-7faf-b06c-1c00-2064091fe9ad.png)

### /odom フレームと/map の補正が確認できる

![06_rviz_odomとmapのずれ.png](https://qiita-image-store.s3.amazonaws.com/0/110138/2597ab0e-926f-72e1-c8ec-f737b1b52d7f.png)

オドメトリの原点である/odom フレームが，ワールド座標系の原点である/map フレームからずれた位置に表示されています．これは，move_base のチュートリアルではなかった挙動です．amcl がオドメトリのずれを補正してくれていることが理解できます．

## おわりに
遊べるのはこんなところです．rvizで遊ぶ分にはあまり違いはなさそうですが，amcl が裏でかなりいい仕事をしているのです．次項以降でその辺りを紐解いていきます．

Next: [3.2 amcl: ソフトウェア構成をみる](http://qiita.com/MoriKen/items/26483e86fb556f0dc5ce)
Prev: [2.2 move_base: ソフトウェア構成をみる](http://qiita.com/MoriKen/items/d5cd6208143d6c40caff)

## 参考文献
1. [S Thrun, et al., "Probabilistic Robotics", the MIT Press, 2005.](http://www.probabilistic-robotics.org/)
2. [ROS Wiki.](http://wiki.ros.org)
3. ["ros-planning/navigation", github.](https://github.com/ros-planning/navigation)
3. ["husky_navigation", github.](http://wiki.ros.org/husky_navigation/)