ROSの勉強　第20弾：環境の地図作成

# #プログラミング ROS< 環境の地図作成 >

## はじめに
1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第20弾として，「環境の地図作成」を扱う．

## 環境
##### 仮想環境
|||
|:-:|:--|
|ソフト|VMware Workstation 15|
|実装RAM|2 GB|
|OS|Ubuntu 64 ビット|
|isoファイル|ubuntu-mate-20.04.1-desktop-amd64.iso|

##### コンピュータ
|||
|:-:|:--|
|デバイス|MSI|
|プロセッサ|Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz|
|実装RAM|8.00 GB (7.89 GB 使用可能)|
|OS|Windows (Windows 10 Home, バージョン：1909)|

##### ROS
|||
|:-:|:--|
|Distribution|noetic|
|プログラミング言語|Python 3.8.5|
|シミュレーション|gazebo|

## 環境の地図作成
まず，地図を作成する対象環境を以下に示す．
![testmap.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/a3e9d9b6-4b77-ff69-4cae-ba2004264621.png)

ここでは，turtlebotを動かし，センサデータから地図作成を行う．前回で学んだrosbagが大いに活躍する．

https://qiita.com/Yuya-Shimizu/items/f69b18e8dd2cee392e79

## 手順
以下では，動画像を使って手順を示す．
##### 手順1
**対象環境にセンサを搭載したロボットを配置**（gazeboの起動）
![process1.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/cc385b72-2596-b448-8295-78a6a17f81e6.gif)

##### 手順2
**ロボットを動かす準備と，データを記録する準備**（teleopとrosbag）

![process2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/f4206fd3-1f4a-b969-0926-e1a5e3e0085a.gif)
実行したコマンドは以下のとおりである．

```bash:rosbag
rosbag record -O data.bag /scan /tf
```

```bash:turtlebot_teleop
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
```
これで，data.bagに/scanと/tfのデータを記録する準備ができた．

##### 手順3
**データ収集**（ロボットを動かす）
![process3.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/a27269e3-76f9-42a6-612d-c1a6f53e5d6d.gif)
しばらく適当にデータを収集し続ける．

データ収集後，``rosbag record~``を実行しているターミナル(端末)上でCtrl-cを押して，データの記録終了．そのとき，data.bagというファイルができている．このファイルの情報を詳しく見ることができる．

![キャプチャ.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/33bfa977-9da5-f3c6-c967-18a20b1a95f5.png)

こんな感じである．

##### 手順4
**データから地図を生成する**（gmappingとrosbag）
![process4.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/f24f70c8-dae0-98e5-0dd9-d44d0799910c.gif)

```bash:ターミナル1
rosparam set use_sim_time true
rosrun gmapping slam_gmapping
```
```bash:ターミナル2
rosbag play --clock data.bag
```
```bash:ターミナル3
rosrun robot_state_publisher robot_state_publisher
```

はじめは，ターミナル1と2だけ実行していたが，これではうまく地図を生成できていなかった．どうやら，rvizのとき同様，robotからmapにデータが**うまく変換がされていないようだ**．そこで，rvizのときの解決策と同じようにターミナル3を実行してから行ってみたところ，うまくいった．**必ず，シミュレータを停止して，ターミナル3を実行してから，ターミナル1，ターミナル2**という順で実行する．
これで，地図の作成ができた．

##### 手順5
**地図データの保存**（map_server）
![process5.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/4ee397a9-ae57-4e9f-822a-6d7b672b2f79.gif)

```
rosrun map_server map_saver
```
このとき，手順4でのターミナル2は起動したままとする．なぜなら，そのデータを今から保存しようとしているため．保存が完了すると，地図の画像データと，メタデータを含むyamlファイルが得られる．以下にそれらを示す．
![map.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/f8e569c1-cfeb-69ff-4742-f795fec1abdb.png)

```yaml:map.yaml
image: map.pgm
resolution: 0.050000
origin: [-100.000000, -100.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
```

## 地図の質を向上させる
先ほど得られた地図の画像については，デフォルトではサイズが4000*4000であったため，非常に小さく表示されてしまっていた．また，地図自体もあまりよくないらしい．そこで，ある設定をこなすだけでよくなる方法をここで示しておく．

```
rosparam set /slam_gmapping/angularUpdate 0.1
rosparam set /slam_gmapping/linearUpdate 0.1
rosparam set /slam_gmapping/lskip 10
rosparam set /slam_gmapping/xmax 10
rosparam set /slam_gmapping/xmin -10
rosparam set /slam_gmapping/ymax 10
rosparam set /slam_gmapping/ymin -10
```
これらを，gmapping実行前に設定する．``rosparam set``により，gmappingのパラメータを変更している．上記のものは，新しいスキャンデータを地図に取り込むまでにロボットが動かないといけない回転量（angularUpdate）と並進移動の量（linearUpdate），LaserScanメッセージを処理するときに飛ばして読む光線の数（lskip），地図の広さ（xmin, xmax, ymin, ymax）を変更している．以下にそのときの結果を示す．
![map2.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/456a25fc-0c67-0228-f94b-537d55ca7270.png)

```yaml:map2.yaml
image: map2.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
```

上の地図を見ても分かるように，地図の広さの指定により程よい大きさの地図画像が生成された．非常に見やすくなった．このあたりの設定は，ここでは示さないが，launchファイルなどにまとめておくと便利かもしれない．

## 感想
rvizのときもそうだったが，今回も気づきにくい（明示的にエラーとは示してくれない）ものが，原因となってうまく地図の作成ができず，1日2日悩んだ．何とか解決できてよかった．何か別の空間や形式に持っていくときには，何かしらの変換が必要で，そのための変換器のようなものが必要になることもあるということを頭に入れて，意識しておいた方がいいと身をもって感じた．なお，上では示さなかったが，リアルタイム（bagファイルに保存しない）で地図を作成することも可能で，その場合，ロボットを動かしているときに，``slam_gmapping``を起動するだけである．しかしながら，**ロボットの計算負荷を減らすために，データをいったん記録することがお勧めらしい**．ただし，rosbagで保存するかどうかにかかわらず，結果的には同じような地図ができるらしい．

今回扱ったgmappingについての詳細は，論文が2つある．ここにそのURLを示しておく．
**論文1: Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters**
(https://www.researchgate.net/publication/3450390_Improved_Techniques_for_Grid_Mapping_With_Rao-Blackwellized_Particle_Filters )
**論文2: Improving Grid-based SLAM with Rao-Blackwellized Particle Filters By Adaptive Proposals and Selective Resampling**
(https://www.researchgate.net/publication/221067958_Improving_Grid-based_SLAM_with_Rao-Blackwellized_Particle_Filters_By_Adaptive_Proposals_and_Selective_Resampling )

また，余談だがQiitaの記事を作成する際に，どうやら10MBを超える画像は貼り付けられないらしい．そういう公式の発表を見たわけではないが，10MBを超えたgifデータだけ貼り付けられず，圧縮して軽量化することで，貼り付けられた．

## 参考文献
プログラミングROS Pythonによるロボットアプリケーション開発
　　　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart 著
　　　　　　　　　　　　　　　　　　　　　　　河田　卓志　監訳
　　　　　　　　　　  　松田　晃一，福地　正樹，由谷　哲夫　訳
　　　　　　　　　　　　　　　　　　オイラリー・ジャパン　発行