安価な2DレーザスキャナRPLidar A2とROSでSLAMを試す

# 記事の概要
本記事は、安価な2DレーザスキャナRPLidar A2の動作確認をします。

まずはwindowsで点群地図を作成できることを確認します。
次にUbuntuでROSを用いて点群地図を作成できることを確認します。
最後にHector SLAMを用いてリアルタイムに地図を作成できることを確認します。

動作確認が目的なので、アルゴリズムの解説や精度を高める調整は行いません。


# RPLidar A2とは何か

[RPLidar A2](https://www.slamtec.com/en/Lidar/A2)は安価な2次元レーザスキャナです。
2次元レーザスキャナはSLAMで必要になります。

<img width="40%" alt="IMG_1829.JPG" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/65383072-e46c-224d-06c3-4ee0bcf66696.jpeg">

性能をAMAZONの商品紹介ページから以下に引用します。

- 距離範囲：0.15-12m(反射率70％の白いオブジェクトに基づく)
- 角度範囲：0-360度
- 距離分解能：<0.5mm
- 角度分解能：0.45〜1.35度、通常0.9度
- サンプル期間：0.25ms
- サンプルレート：2000~8000Hz,Typical 4000Hz
- スキャンレート：5~15Hz,Typical 10Hz




### SLAMの種類
ロボットや自動運転車が移動する為には、地図の作成と自分がどこにいるかの自己位置推定が必要です。これを実現する手段がSLAMです。

SLAMには以下の種類があります。

- 2D/2D型
    - ロボット位置：2次元3自由度
    - 地図：2次元 
    - センサ：2Dレーザースキャナ
- 2D/3D型
    - ロボット位置：2次元3自由度
    - 地図：3次元 
    - センサ：2D/3Dレーザスキャナ
- 3D/3D型
    - ロボット位置：3次元6自由度
    - 地図：3次元 
    - センサ：3Dレーザスキャナ

# windowsでの動作確認

## 準備
 
### SDKのインストール
[RPLidarA2サポートページ](https://www.slamtec.com/en/Support#rplidar-a-series)のSDK and FirmwareのRPLIDAR SDKのDownloadをクリックします

<img width="40%" alt="PRLidarのサポートページ.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/c6782405-94ca-d6ff-105f-68146e0e2732.png">


### USBデバイスドライバのインストール
USBをPCに接続し、デバイスマネージャーでPORT番号を確認してください。

<img width="40%" alt="デバイスドライバの認識.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/96d72d8a-ea02-35ec-869e-bd950ccc9961.png">

USBを認識しない場合は、USBデバイスドライバがインストールされていないのかもしれません。その時はSDKのtoolフォルダ内のcp2102_driverフォルダにあるCP210xVCPInstaller_x64.exeを実行し、ドライバをインストール後に、再度試してみてください。

<img width="40%" alt="デバイスドライバのインストール0.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/314c8c3d-fcaa-a805-12ca-0fca07de1f0f.png">


同じドライバは[CP210x USB to UART Bridge VCP Drivers](https://www.silabs.com/products/development-tools/software/usb-to-uart-bridge-vcp-drivers)からもインストールできます。

<img width="40%" alt="デバイスドライバのダウンロード.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/1fcb5747-008b-35ae-90d6-994337735e7d.png">


## 動作確認

### 
[RPLidar toolページ](https://github.com/Slamtec/rplidar_sdk/releases)のframe_grabber.exeをダウンロードします

<img width="40%" alt="Frame_grabberのダウンロード画面.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/289c1190-6457-6010-b459-4826dce3003e.png">

frame_grabber.exeを実行し、デバイスマネージャーで確認したPORT番号に設定し、通信速度を115200にします。

<img width="30%" alt="Frame_grabberの起動画面.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/6971036e-6373-2c83-1313-a5b237d50809.png">

起動後、以下の画面が表示されます。赤い枠で囲ったボタンをクリックするとRPLidarが回転を開始します。

<img width="80%" alt="Frame_grabberの起動.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/082fd021-1357-29e7-5628-d7c74823d5ed.png">

正常に動作すれば、以下のように点群地図を作成します。

<img width="80%" alt="Frame_grabberの実行.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/2a5d7637-6459-abfc-efcf-38d7cdb960de.png">


	
# Ubuntuでの動作確認

次はROSを用いてRPLidarの動作確認をします。

## 準備

https://github.com/robopeak/rplidar_ros.git をコピーして、ビルドします。
ここではビルドにcatkin buildを使用していますが、catkin_makeでも問題ありません。

catkin buildの設定方法は[iRobot Create 2でROSを勉強する（１）準備](https://qiita.com/Kosuke_Matsui/items/95e90b372ccb749cf1a2)をご覧ください。

```bash:
$ cd ~/catkin_ws/src
$ git clone https://github.com/robopeak/rplidar_ros.git
$ cd ~/catkin_ws
$ catkin build rplidar_ros
$ source devel/setup.bash
```

## 動作確認

ターミナルを新規に開いて以下を実行します

```bash:
$ roscore
```

"roslaunch rplidar_ros"を入力してTABをクリックすると、launch候補の一覧が確認できます。

```bash:
$ cd ~/catkin_ws
$ roslaunch rplidar_ros 
rplidar_a3.launch       test_rplidar_a3.launch  view_rplidar_a3.launch
rplidar.launch          test_rplidar.launch     view_rplidar.launch
```

RPLidarが点群地図を作成するのを確認したい場合は、view_rplidar.launchを実行します。

```bash:
$ roslaunch rplidar_ros view_rplidar.launch
```

RVizが起動し、以下の画面が表示されます。

<img width="80%" alt="viewer起動画面.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/74319f93-f4fe-20f5-f973-75096d3d21a3.png">


ここでtopicは/scanになっています。
これは以下のコマンドでリスト一覧を表示させて確認することもできます。

```bash:
$ rostopic list
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg
/scan
/tf
/tf_static
```

RPLidarが取得しているデータは以下のコマンドで確認できます。

```bash:
$ rostopic echo /scan
```

以下のデータが取得できます。
データの詳細はhttp://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html をご覧ください。

もしくは[「RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_008日目_SLAM_GMapping_LiDAR(A1M8)のLiDARスキャンデータの意味を調べる」](https://qiita.com/PINTO/items/9aa737c284dc4e8212f1#-lidar%E3%82%B9%E3%82%AD%E3%83%A3%E3%83%B3%E3%83%87%E3%83%BC%E3%82%BF%E3%81%AE%E6%84%8F%E5%91%B3%E3%82%92%E8%AA%BF%E3%81%B9%E3%82%8B)をご参照ください。

```bash:
header: 
  seq: 996
  stamp: 
    secs: 1576877613
    nsecs: 741686250
  frame_id: "laser"
angle_min: -3.12413907051
angle_max: 3.14159274101
angle_increment: 0.0174532923847
time_increment: 0.000267035182333
scan_time: 0.0958656296134
range_min: 0.15000000596
range_max: 12.0
ranges: [inf, 0.3140000104904175, inf, 0.3109999895095825, 0.30399999022483826, inf, 0.3019999861717224, 0.3059999942779541, 0.3409999907016754, 0.3199999928474426, 0.30799999833106995, 0.3019999861717224, 0.31200000643730164, 0.30799999833106995, 0.296999990940094, 0.2879999876022339, 0.28700000047683716, 0.289000004529953, 0.289000004529953, 0.2879999876022339, inf, 0.2879999876022339, 0.2930000126361847, inf, 0.30399999022483826, 0.3149999976158142, 0.3140000104904175, inf, 0.3100000023841858, inf, 0.3149999976158142, inf, 0.3149999976158142, 0.31299999356269836, inf, 0.3140000104904175, inf, inf, 0.18700000643730164, 0.18400000035762787, 0.18000000715255737, 0.1770000010728836, 0.17399999499320984, 0.17100000381469727, 0.16899999976158142, 0.16599999368190765, 0.164000004529953, 0.16200000047683716, 0.16200000047683716, 0.16500000655651093, 0.16899999976158142, inf, inf, inf, inf, inf, inf, 0.4650000035762787, 0.4180000126361847, 0.4169999957084656, 0.4230000078678131, 0.414000004529953, 0.41100001335144043, 0.41200000047683716, 0.41499999165534973, 0.4180000126361847, 0.42100000381469727, 0.4259999990463257, 0.4309999942779541, 0.4429999887943268, 0.49300000071525574, 0.5099999904632568, 0.5080000162124634, 0.5040000081062317, 0.5009999871253967, 0.5, 0.5, 0.5009999871253967, 0.5070000290870667, 0.5370000004768372, 0.5429999828338623, inf, 0.5680000185966492, 0.5830000042915344, inf, inf, 0.33000001311302185, 0.3240000009536743, 0.3140000104904175, 0.3100000023841858, 0.3059999942779541, 0.3009999990463257, 0.2930000126361847, inf, 0.289000004529953, 0.2849999964237213, 0.28299999237060547, 0.2750000059604645, 0.27300000190734863, 0.26899999380111694, 0.2669999897480011, 0.2619999945163727, inf, 0.25600001215934753, inf, 0.2549999952316284, 0.2529999911785126, 0.24799999594688416, inf, 0.24699999392032623, 0.24199999868869781, 0.24300000071525574, inf, 0.2409999966621399, 0.23999999463558197, inf, 0.26499998569488525, 0.2840000092983246, 0.30000001192092896, inf, inf, inf, inf, 0.3790000081062317, inf, inf, inf, inf, inf, inf, inf, inf, 0.8989999890327454, 1.0420000553131104, inf, 0.609000027179718, 0.6290000081062317, inf, inf, 0.6589999794960022, 0.6359999775886536, 0.5789999961853027, 0.5759999752044678, 0.5830000042915344, inf, 0.5870000123977661, 0.593999981880188, 0.6230000257492065, 0.6650000214576721, inf, 0.4659999907016754, 0.4519999921321869, 0.4480000138282776, inf, 0.38999998569488525, 0.38499999046325684, 0.3869999945163727, 0.3919999897480011, 0.4020000100135803, 0.4090000092983246, 0.4230000078678131, 0.4339999854564667, 0.44200000166893005, 0.4390000104904175, 0.44999998807907104, inf, 0.5509999990463257, 0.5559999942779541, 0.5580000281333923, 0.5519999861717224, 0.5509999990463257, 0.5640000104904175, 0.5680000185966492, 0.5609999895095825, 0.5509999990463257, 0.5450000166893005, 0.5419999957084656, 0.5440000295639038, 0.5429999828338623, 0.5440000295639038, 0.5460000038146973, 0.5509999990463257, 0.5550000071525574, 0.5720000267028809, 0.5849999785423279, inf, inf, inf, 2.565999984741211, 2.553999900817871, 2.549999952316284, 2.5260000228881836, 2.503999948501587, 2.500999927520752, 2.493000030517578, 2.4649999141693115, 2.4779999256134033, 2.446000099182129, 2.259999990463257, 2.1510000228881836, 2.128000020980835, 2.1410000324249268, 2.1579999923706055, 2.138000011444092, 2.1510000228881836, 2.1549999713897705, 2.1530001163482666, 2.186000108718872, 2.3259999752044678, 2.361999988555908, 2.4179999828338623, 2.433000087738037, 2.443000078201294, 2.4519999027252197, 2.4590001106262207, 2.4590001106262207, 2.4679999351501465, 2.4719998836517334, inf, 0.925000011920929, 0.921999990940094, 0.9520000219345093, 0.9539999961853027, 0.9890000224113464, 0.9990000128746033, 1.0010000467300415, 0.9980000257492065, 0.9819999933242798, 0.9369999766349792, 0.9350000023841858, 0.9929999709129333, 0.9929999709129333, 1.069000005722046, 1.0700000524520874, 1.0709999799728394, 1.0670000314712524, 1.062999963760376, 1.0759999752044678, 1.0839999914169312, 1.0880000591278076, 1.2120000123977661, 1.2009999752044678, inf, inf, 1.1929999589920044, inf, 3.1110000610351562, 3.046999931335449, 3.0160000324249268, 2.9509999752044678, 2.8980000019073486, 2.7880001068115234, 2.7279999256134033, 2.693000078201294, 2.630000114440918, 2.6059999465942383, 2.572000026702881, 2.565999984741211, 2.5450000762939453, 2.5, 2.4670000076293945, 2.4489998817443848, 2.4140000343322754, 2.375999927520752, 2.3550000190734863, 2.3399999141693115, 2.322000026702881, 2.306999921798706, 2.2820000648498535, 2.259999990463257, 2.244999885559082, 2.2219998836517334, 2.2019999027252197, 2.190000057220459, 2.1760001182556152, 2.1570000648498535, 2.1440000534057617, 2.174999952316284, inf, inf, 0.5090000033378601, 0.5059999823570251, 0.503000020980835, 0.5, 0.49900001287460327, 0.4950000047683716, 0.49399998784065247, 0.492000013589859, 0.47699999809265137, 0.4749999940395355, 0.48399999737739563, inf, 0.48100000619888306, inf, 0.6389999985694885, inf, 0.4830000102519989, 0.4819999933242798, 0.5080000162124634, 0.48100000619888306, 0.4880000054836273, 0.4950000047683716, 0.5139999985694885, 0.48399999737739563, 0.5049999952316284, inf, inf, 0.5709999799728394, inf, 0.5, 0.6159999966621399, 0.6110000014305115, 0.6159999966621399, 0.6079999804496765, 0.6060000061988831, 0.6069999933242798, inf, inf, inf, 0.8220000267028809, inf, inf, inf, inf, 0.5230000019073486, 0.5139999985694885, 0.5239999890327454, 0.5220000147819519, inf, inf, inf, inf, inf, inf, 0.6489999890327454, 0.6359999775886536, 0.6230000257492065, 0.6119999885559082, 0.6000000238418579, 0.5889999866485596, inf, inf, 0.3919999897480011, 0.3970000147819519, 0.37700000405311584, 0.3720000088214874, 0.37700000405311584, 0.3659999966621399, 0.3659999966621399, 0.3630000054836273, inf, inf, inf, inf, inf, inf, inf, inf, 0.3140000104904175, 0.3149999976158142]
intensities: [0.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 0.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 0.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 0.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0]
---
```



# Hector SLAMの動作確認
Hector SLAMはダルムシュタット工科大学が開発した、Scan-Matching +拡張カルマンフィルタのアルゴリズを採用したSLAMです。loop closure機能は無く、オドメトリも使用しません。
オドメトリ情報が不要なので、RPLidar単体での動作確認をするのに適しています。

作業手順は主に以下の動画を参照しました。

[RPLidar and Hector SLAM for Beginners | ROS Tutorial #8](https://www.youtube.com/watch?v=Qrtz0a7HaQ4)

## 準備

https://github.com/tu-darmstadt-ros-pkg/hector_slam.git をコピーして、ビルドします。

```bash:
$ cd ~/catkin_ws/src
$ git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git
$ cd ~/catkin_ws
$ catkin build hector_slam
$ source devel/setup.bash
```

Ubuntu PCでは正常にビルドできたのですが、Jetson Nanoではビルドエラーになってしまいました。
現在、原因を調査中です。

## ファイルの修正
[Special mention to NickL77](https://github.com/NickL77/RPLidar_Hector_SLAM)に従い、以下の修正を行います。

### mapping_default.launchの修正

catkin_ws/src/rplidar_hector_slam/hector_slam/hector_mapping/launch/mapping_default.launchを修正します。

#### 54行目の修正

tf broadcasterについてmap navをbase_link laserに変更します。

【修正前】

```html:
<!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->
```


【修正後】

```html:
<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100" />
```
 

#### 5行目、6行目の修正
base_frameとodom_frameをbase_linkに変更します。

【修正前】

```html:
<arg name="base_frame" default="base_footprint"/>
<arg name="odom_frame" default="nav"/>
```


【修正後】

```html:
<arg name="base_frame" default="base_link"/>
<arg name="odom_frame" default="base_link"/>
```


### tutorial.launchの修正

catkin_ws/src/rplidar_hector_slam/hector_slam/hector_slam_launch/launch/tutorial.launchを修正します

#### 7行目の修正
use_sim_timeをfalseに設定することで、リアルタイム動作にします。

【修正前】

```html:
<param name="/use_sim_time" value="true"/>
```


【修正後】

```html:
<param name="/use_sim_time" value="false"/>
```


## 動作確認

ターミナルを新規に開いて以下を実行します。

```bash:
$ roscore
```


ターミナルを新規に開き、パッケージrplidar_rosのrplidar.launchを実行し、RPLidarを起動させます。

```bash:
$ cd ~/catkin_ws
$ roslaunch rplidar_ros rplidar.launch
```


ターミナルを新規に開き、パッケージhector_slamのtutorial.launchを実行します。

```bash:
$ cd ~/catkin_ws
$ roslaunch hector_slam tutorial.launch
```

RVizが起動して、以下の画面が表示されます。
RPLidarを移動させるとリアルタイムに地図を更新します。
緑の線は移動の軌道ですが、上下に動かしたせいか、異常な跳躍をしています。

<img width="80%" alt="HECTOR_SLAM.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/042dcaed-87d5-031b-1ad5-1e581a0b210e.png">

# 今後の予定
- Gmappingを試す
- SLAMのアルゴリズムを理解する
    - データだけを取得して、ROSを用いずにSLAMを実装する 
- iRobot Create2のオドメトリを併用して精度の高い地図を作成する
- Jetson Nanoで同様のことを行う
    - RPLidar起動中に電源が落ちてしまう問題を解決しないといけない
    - Hector SLAMのビルドに失敗する

# 参考
- [RPLidar and Hector SLAM for Beginners | ROS Tutorial #8](https://www.youtube.com/watch?v=Qrtz0a7HaQ4)
- [ネコとロボティクス「低価格 Lidar RPLIDAR A2 を使ってみる」](http://takk.hatenablog.com/entry/2018/04/20/233502)
- [ROSのLidarSLAMまとめ](https://qiita.com/nnn112358/items/814c0fb0d2075eb71da0)
- [rplider A2M8をROSで動かす](https://qiita.com/ReoNagai/items/b5f54ca57d052e2a184d)
- [RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_008日目_SLAM_GMapping_LiDAR(A1M8)](https://qiita.com/PINTO/items/9aa737c284dc4e8212f1)