Autoware/ROSでマップを作成する

Velodyne VLP-16を使って、部屋の中で自動運転用のマップを作成してみる。

参考：https://qiita.com/yukkysaito/items/21a70ec6b07f1ca559cb#3d%E3%83%9E%E3%83%83%E3%83%97%E4%BD%9C%E6%88%90

# 台車
計測用に台車を作ってみた。
（※最終的にはモーターを付けて走らせる予定）
<img width="400" alt="IMG_5047.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/80263/4658629a-6157-d683-0488-dba67fa63bac.jpeg">

# Velodyneのnodeを起動
## 方法1. AutowareのUIで
ARMのSensingタブでVLP-16をクリックする。
points_rawトピックでデータが流れてくる。

## 方法2. ROSコマンドで
```
$ roscore&
$ roslaunch velodyne_pointcloud VLP16_points.launch &
$ rosnode list
$ rostopic echo /velodyne_points //データが来ているのが分かる
```
velodyne_pointsトピックでデータが流れてくる。

# ROSBAGで記録
## 方法1. AutowareのUIで
ROSBAGボタンをクリック。

## 方法2. ROSコマンドで
```
$ rosbag record -a //すべてのノードを記録
$ rosbag record -O vlp-16.bag /velodyne_points //ファイル名を指定してVelodyneノードのみ記録
```
vlp-16.bagファイルが生成される。

注：記録中はRVizで見ながらゆっくり移動させる。

# ポイントクラウドへ
1. 記録したbagファイルを再生し、一時停止。
2. Computiongタブのndt_mappingのチェックボックスにチェックを入れる。
3. .bagファイルの一時停止を解除。しばらくして再度一時停止する。
4. Computiongタブのndt_mappingの、appボタンからポップアップを開き、保存先を指定して「PCD OUTPUT」ボタンを押す。
5. .pcdファイルが生成される。

※参考：https://project.nikkeibp.co.jp/mirai/jusk1906/day3.pdf

topic名が「points_raw」でないと、pcdファイルに記録されないようなので、
「velodyne_points」で記録した場合は、topic名を下記で転送する。
https://qiita.com/misoragod/items/6d9ce990eb19c105dd96#3d-map%E4%BD%9C%E6%88%90

```
$ rosrun topic_tools relay /velodyne_points /points_raw
```

ちなみに、参考PDFにあるndt_mapping.rvizは見つけることができなかった。

```
$ find ./ -name ndt_mapping.rviz
// ヒットしない。
```

# 作成したマップの確認
RVizで確認する。ARMのMapタブで、TFを押した上で、Rvizボタンから起動すると下記のように表示された。
![Screenshot from 2020-12-31 00-23-35.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/80263/c60d174c-c794-933f-f308-c637619052c9.png)

![Screenshot from 2020-12-31 00-24-36.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/80263/794bd809-f4a5-10a6-eab3-3f1104ada8b7.png)


# 設定値について
狭い場所や室内のマップでは、Computing>ndt_mappingで、minimum Scan rangeを0.25などに設定
![Screenshot from 2021-01-05 00-24-03.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/80263/fc71942f-7723-bad3-03b8-2b8cc52bf20c.png)


# 各種エラーについて
下記のようなエラーが出るときがあった。

```
// TF指定がされていない場合
For frame [velodyne]: No transform to fixed frame [world].  TF error: [Lookup would require extrapolation into the past.  Requested time 1609378636.168447000 but the earliest data is at time 1609383678.572532467, when looking up transform from frame [velodyne] to frame [world]]

// MapタブのTFが定義されてない場合
Global Status:Error
Fixed Frame [world] does not exist

// Velodyne座標がWorld座標とリンクしていない場合
Points Raw:
Status: Error
For frame [velodyne]: Fixed Frame [world] does not exist

// Velodyne座標がWorld座標とリンクしていない場合
For frame [velodyne]: No transform to fixed frame [world].  TF error: [Could not find a connection between 'world' and 'velodyne' because they are not part of the same tree.Tf has two or more unconnected trees.]

// Minimum_range
For frame [velodyne]: No transform to fixed frame [world].  TF error: [Lookup would require extrapolation into the future.  Requested time 1609686391.451981000 but the latest data is at time 1609686288.021054000, when looking up transform from frame [velodyne] to frame [world]]
```

https://qiita.com/dendensho/items/959c6d510593546f4020
これはうまくできなかった。

```
rosrun tf static_transform_publisher 0 0 0 0 0 0 map frame 100
```

# フレーム/TFのリンク方法
座標系を、world→map→base_link（車両）→velodyne とリンクさせてあげる必要がある。

* world→map：MapタブでTFを押す
* map→base_link：Computingタブでndt_mappingを押す
* base_link→velodyne：SettingタブでTFを押す


## TFツリーを可視化して確認
```
$ rosrun tf view_frames
```
このコマンドを打つことで、ツリー構造図がPDF出力され分かりやすい。
![Screenshot from 2021-01-02 00-22-29.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/80263/36da245a-f7e1-ebaf-4c72-f29c1b6a44f0.png)

![Screenshot from 2021-01-01 23-52-42.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/80263/487f9898-cc53-8d79-a126-764cbd8411d9.png)

# 最終的に出来上がったマップ
点群が薄いけど、、
![Screenshot from 2021-01-05 00-29-11.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/80263/55ba36ba-a464-aed7-e6f3-0b1b78606194.png)