Autoware.AIに2D Lidar( RPLIDAR A3)を使用

Autoware.AIでVelodyne VLP-16様な高価な3D Lidarが必須ですが、安価な2D Lidar( RPLIDAR A3)を使用できるかを試します。

# 目次
1. [Autoware.AIのインストールとROSBAG Demoの実行](https://qiita.com/k-koh/items/90647b1cb140b4b75c91)
2. [Autoware.AIに2D Lidar( RPLIDAR A3)を使用](https://qiita.com/k-koh/items/0e492a0ae032a5ab034d)

# 動作環境

- MacBookPro A1708 (Core i7, 16GB RAM, 512GB SSD)
- AsRock DeskMini GTX1060(Z370)
- Core i9-9900 (3.10GHz 8core 16thread)
- メモリー32GB
- NVIDIA GeForce GTX 1060 Mobile(6GB GDDR5)
- Ubuntu 18.04.5
- ROS Melodic
- Qt Creator 4.9.2
- Autoware 1.14.0
- RPLIDAR A3

# 2D Lidar( RPLIDAR A3)のデータをROSBAGに保存

- RPLIDAR ROS packageのインストール

```
cd ~/catkin_ws/src
git clone https://github.com/Slamtec/rplidar_ros.git
cd ~/catkin_ws
catkin_make
```

- RPLIDAR ROS packageのnode.cppを改造

RPLIDARのscanデータと共にscanデータをPointCloud2形式に変換して出力するように修正します。

PointCloud2 Topicのframe_idとtopic_nameをAutowareに合わせて修正します。

frame_id : `velodyne`
topic_name : `points_raw`

- RPLIDARのデータを記録

車の上に載せって走行したRPLIDARのScanデータをROSBAGに記録

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/fd1546aa-cd5c-571b-5695-8cfdcf2c3bae.png)

```
$ rosbag record -a
```

```
$ rosbag info 2020-09-01-22-44-42.bag
path:        2020-09-01-22-44-42.bag
version:     2.0
duration:    1:14s (74s)
start:       Sep 01 2020 22:44:42.32 (1598967882.32)
end:         Sep 01 2020 22:45:56.63 (1598967956.63)
size:        240.8 MB
messages:    674885
compression: none [311/311 chunks]
types:       rosgraph_msgs/Log       [acffd30cd6b6de30f120938c17c593fb]
             sensor_msgs/LaserScan   [90c7ef2dc6895d81024acba2ac42f369]
             sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics:      /points_raw      984 msgs    : sensor_msgs/PointCloud2
             /rosout       336083 msgs    : rosgraph_msgs/Log       (3 connections)
             /rosout_agg   336834 msgs    : rosgraph_msgs/Log      
             /scan            984 msgs    : sensor_msgs/LaserScan

```

# ROSBAGのPointCloud2データから地図を作成

BLAMで地図を作成します。

- BLAMをインストール

[Ubuntu18.04 でBLAMをインストール](https://qiita.com/k-koh/items/fa87e9eae3aa4203ba2c)を参考してインストールします。

- 地図を作成

blam/internal/src/blam_example/launch/test_offline.launchでscan_topic名をROSBAGのtopic名に変更します。

`velodyne_points` → `points_raw`

```
$ cd blam
$ ./blam_offline.sh 2020-09-01-22-44-42.bag

NODES
  /blam/
    blam_slam (blam_slam/blam_slam_offline)

ROS_MASTER_URI=http://localhost:11311

process[blam/blam_slam-1]: started with pid [14250]
[ INFO] [1598973632.507347948]: /blam/blam_slam/BlamSlam: Registering log callbacks.
[ INFO] [1598973632.509999720]: STARTING LoadBagfile
[ INFO] [1598973632.510012012]: /blam/blam_slam/BlamSlamOffline: Processing the following topics:
	/points_raw
[ INFO] [1598973632.622944827]: /blam/blam_slam/BlamSlamOffline: Bag start time    = 1598967882.530024 	 Bag end time    = 1598967956.633434
[ INFO] [1598973632.622991533]: /blam/blam_slam/BlamSlamOffline: Replay start time =         0.000000 	 Replay end time = 4294967296.000000
[ INFO] [1598973632.661777337]: STARTING ProcessBagfile
[ INFO] [1598973632.661799079]: /blam/blam_slam/BlamSlamOffline: Sorting messages.
[ INFO] [1598973632.666309510]: /blam/blam_slam/BlamSlamOffline: Processing messages.
[ INFO] [1598973632.666323489]: Processing 1 out of Total 984

[ INFO] [1598973633.955234963]: /blam/blam_slam/BlamSlamOffline: Finished processing bag file, 5747.575541 percent of real-time.
[ INFO] [1598973634.997859155]: PCD file prefix is: blam/pcd_offline/map_
[ INFO] [1598973634.998685363]: Saving as ASCII PCD
[ INFO] [1598973634.999621759]: Listening for incoming data on topic /blam/blam_slam/octree_map
[ INFO] [1598973636.508294270]: Received 1394 data points in frame world with the following fields: x y z
[ INFO] [1598973636.508377475]: Data saved to blam/pcd_offline/map_0.pcd
```

- 作成した地図を確認

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/6b4d57dd-a949-0161-2612-ff4253738703.png)


# Autoware.AIでROSBAGから経路生成

- Autoware.AIを起動

```
$ roslaunch runtime_manager runtime_manager.launch
```

- ROSBAGをロード

ROSBAGを選択してplay → Pause で一時停止

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/bc8dc411-2582-0cbd-1654-132dbaa1348a.png)

- Setup TabでTFとVehicle Modelを設定

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/781bbe47-8304-bb7a-a48b-f4913259757a.png)

- Map Tabでblamから生成した地図とTFを設定

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/1b6fb917-2142-92ce-7d81-4b672ed72d0d.png)

- Sensing Tabでvoxel_grid_filterを選択

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/4c27df28-d94f-2f81-b159-7ddd150dbc47.png)

- Computing Tab

ndt_matchingのappでInitial Posを選択

ndt_matching、vel_pose_connect、waypoint_saverを選択

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/cb689289-ab70-d6d8-0b15-2a60fa27d229.png)


![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/228cc643-43d8-acc4-5123-3bbdf3e135ec.png)

- Rosbag再生開始

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/b07a995a-9ccc-7700-af65-3cac41b9955d.png)


- Rosbag再生完了、経路生成結果

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/cb3ea7e0-e907-2854-c7ac-2556f8ec9629.png)


[![Autoware.AIに2D Lidar( RPLIDAR A3)で経路生成](http://img.youtube.com/vi/V50hAqestuQ/0.jpg)](http://www.youtube.com/watch?v=V50hAqestuQ)