RPLidar A1 M8で部屋の地図を作成しよう!

# 本記事の目的
本記事では細かい理論面は一旦置いて, 実際にLiDARを使用して地図作成する事を目的としています. また扱うSLAMはGmappingとHector SLAMです.

# 環境
本記事は以下の環境で実験しています.

| 項目 | バージョン |
|:-:|:-:|
| Ubuntu | 18.04 |
| ROS | Melodic |
| RPLidar | A1 M8 |

UbuntuやROSの環境構築は別途他のサイトをご覧ください.

# RPLidarのパッケージをインストール
以下の手順に従ってRPLidarに必要なパッケージをインストールしましょう.

```bash:
$ cd ~/catkin_ws/src
$ git clone https://github.com/Slamtec/rplidar_ros.git
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
```
これでRPLidarを動かすために必要なパッケージはインストール出来たので, 以下の手順に従って実際にPRLidarを動かしてみましょう!
# 実際に動かそう
まずはRPLidarをPCに接続します.
<img width="300" alt="S__13295618" src="https://user-images.githubusercontent.com/52307432/97778398-3ca14100-1bba-11eb-8ee8-62815baebb2a.jpg">
次にRPLidarのUSBデバイス接続の権限を与えます.

```bash:
$ sudo chmod 666 /dev/ttyUSB0
```
roscoreを起動してRPLidarの点群データをrvizで見て観ましょう.

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

以下の画像のように点群データが得られていれば成功です!
<img width="600" alt="Screenshot from 2020-10-31 04-14-39" src="https://user-images.githubusercontent.com/52307432/97778175-c2bc8800-1bb8-11eb-8cf2-536d49f0db21.png">
実際の点群データは/scanトピックで見る事が出来ます

```bash:
$ rostopic echo /scan
```
以下のようなデータ型を確認できるはずです.

```bash:
-
header: 
  seq: 774
  stamp: 
    secs: 1608805712
    nsecs: 759049379
  frame_id: "laser"
angle_min: -3.12413907051
angle_max: 3.14159274101
angle_increment: 0.0174532923847
time_increment: 0.000378991797334
scan_time: 0.136058062315
range_min: 0.15000000596
range_max: 12.0
ranges: [0.13300000131130219, 0.13500000536441803, 0.13600000739097595, 0.13899999856948853, 0.14300000667572021, 0.1459999978542328, 0.14900000393390656, 0.15199999511241913, 0.15600000321865082, 0.15700000524520874, 0.16599999368190765, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.1289999932050705, 0.12999999523162842, 0.13099999725818634, 0.13199999928474426, 0.13300000131130219, 0.1340000033378601, 0.1340000033378601, inf, inf, inf, inf, inf, inf, inf, 2.806999921798706, 2.86299991607666, 2.936000108718872, inf, inf, inf, inf, inf, inf, inf, inf, inf, 2.444000005722046, 2.4170000553131104, 2.3919999599456787, 2.385999917984009, 2.3559999465942383, 2.3450000286102295, 2.322000026702881, 2.319000005722046, 2.296999931335449, 2.2990000247955322, 2.315999984741211, 2.3940000534057617, 2.2850000858306885, 2.2750000953674316, 2.2679998874664307, 2.2780001163482666, 2.2720000743865967, 2.2809998989105225, inf, 0.28200000524520874, inf, 0.2720000147819519, inf, 0.1899999976158142, 0.1850000023841858, inf, 0.17399999499320984, inf, 0.15800000727176666, 0.15700000524520874, 0.1550000011920929, 0.15199999511241913, 0.1509999930858612, 0.15199999511241913, 0.15399999916553497, inf, 0.14300000667572021, 0.1420000046491623, 0.14100000262260437, 0.13899999856948853, 0.14000000059604645, 0.13899999856948853, 0.1379999965429306, inf, 0.13199999928474426, 0.12999999523162842, 0.12999999523162842, 0.12999999523162842, 0.12999999523162842, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.16500000655651093, inf, inf, inf, inf, inf, 0.13699999451637268, 0.13899999856948853, 0.13899999856948853, 0.1379999965429306, 0.13899999856948853, 0.13899999856948853, 0.14000000059604645, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.15199999511241913, 0.15000000596046448, inf, 0.17299999296665192, 0.16599999368190765, 0.16099999845027924, 0.15800000727176666, 0.15299999713897705, 0.15199999511241913, 0.14900000393390656, 0.14800000190734863, 0.1469999998807907, 0.1469999998807907, 0.1469999998807907, 0.14800000190734863, 0.15000000596046448, 0.15299999713897705, 0.1550000011920929, 0.1589999943971634, 0.16099999845027924, 0.16599999368190765, 0.1679999977350235, inf, 0.16899999976158142, 0.1850000023841858, 0.1850000023841858, inf, inf, 0.1770000010728836, 0.17499999701976776, 0.17499999701976776, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.1289999932050705, 0.1289999932050705, 0.13099999725818634, 0.12999999523162842, 0.13199999928474426]
intensities: [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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.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, 0.0, 47.0, 0.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.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, 0.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 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, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0]
-
```
# Gmappingで地図を作成しよう
GmappingはFastSLAM2.0をベースとしたSLAMで, LiDARとオドメトリ情報から自己位置と地図を更新しています. FastSLAM2.0の理論面に関しては

**MoriKenさん**

- **[Navigation Stack を理解する - 4.3 gmapping(格子ベースFast SLAM): 原理をみる(応用編)](https://qiita.com/MoriKen/items/0f2550a2adbdcd3da04e)**

が非常に分りやすい解説をしてくれているので, そちらをご覧ください.

それでは実際にGmappingを使用して部屋の地図を作成していこうと思います.
以下に示すファイルを順に実行するとrvizで地図生成の様子が見れます.

```html:rplidar.launch
<?xml version="1.0"?>
<launch>

  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
    <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
    <param name="serial_baudrate"     type="int"    value="115200"/>
    <param name="frame_id"            type="string" value="laser"/>
    <param name="inverted"            type="bool"   value="false"/>
    <param name="angle_compensate"    type="bool"   value="true"/>
    <param name="scan_mode"        type="string" value="Standard"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="baselink_to_laserlink" args="0 0 0 0 0 0 base_link laser 100" /> 
  <node pkg="tf" type="static_transform_publisher" name="odom_to_baselink" args="0 0 0 0 0 0 odom base_link 100"/>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />

</launch>
```

```html:gmapping.launch
<launch>
    <param name="use_sim_time" value="true"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <param name="base_frame" value="base_link"/>
      <param name="odom_frame" value="odom"/>
      <param name="map_frame" value="map"/>
      <param name="map_update_interval" value="2.0"/>
      <param name="maxUrange" value="6.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="minimumScore" value="0.0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.2"/>
      <param name="temporalUpdate" value="1.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="100"/>
      <param name="xmin" value="-10.0"/>
      <param name="ymin" value="-10.0"/>
      <param name="xmax" value="10.0"/>
      <param name="ymax" value="10.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>
</launch>
```
上記2つのファイルを保存したら順に実行します.

```bash:
$ roslaunch rplidar.launch
$ roslaunch gmapping.launch
```
以下の画像のように地図が生成されれば成功です!
![Screenshot from 2020-12-24 07-10-07](https://user-images.githubusercontent.com/52307432/103096341-fced5780-4646-11eb-9537-7bb1cf67c2ac.png)
地図生成中にターミナルでは以下のような時系列データが流れています.

```bash:
m_count 183
Average Scan Matching Score=232.213
neff= 100
Registering Scans:Done
update frame 1472
update ld=0 ad=0
Laser Pose= 0 0 0.00872684

```

作成した地図を以下のコマンドで保存しましょう.

```bash:
$ rosrun map_server map_saver -f gmapping
```
すると以下のように地図データが保存されます.
![gmapping](https://user-images.githubusercontent.com/52307432/103096339-fbbc2a80-4646-11eb-8328-aa6606392a7f.png)

# Hector SLAMで地図を作成しよう
Hector SLAMはLiDARベースSLAMアルゴリズムであり, 特徴として計算負荷の少ないデータ処理方法によって高速にレーザーレンジファインダのデータを処理することで, オドメトリ情報なしで精度の高いSLAMを実現している事が挙げられます. オドメトリ情報が必要ないことからもHector SLAMはドローンの自律制御に用いるSLAMとして適しています.

それでは実際にhector_slamを使用して部屋の地図を作成していこうと思います. 
まずはhector_slamに必要なパッケージをインストール以下の手順でインストールします.

```bash:
$ cd ~/catkin_ws/src
$ git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
```
ここでhector_slamではいくつかのlaunchファイルを修正する必要があります. 基本的には以下のgithubを参考に修正を行いました.

https://github.com/NickL77/RPLidar_Hector_SLAM

ただし上記githubの修正だけでは上手く動作しないので修正点に関して詳細に説明します.

## 修正点1
hector_slam/hector_mapping/launch/mapping_default.launchファイル内の5, 6, 54行目を以下のように修正します.

```html:

<!-- 5行目 -->
<arg name="base_frame" default="base_link"/>

<!-- 6行目 -->
<arg name="odom_frame" default="base_link"/>

<!-- 54行目 -->
<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100" />
```
## 修正点2
hector_slam/hector_slam_launch/launch/tutorial.launchファイル内の7行目を以下のように修正します.

```html:
<!-- 7行目 -->
<param name="/use_sim_time" value="false"/>
```
## 修正点3
hector_slam/hector_slam_launch/launch/tutorial.launchファイル内の14行目を以下のように修正します.

```html:
<!-- 14行目 -->
<include file="$(find hector_geotiff_launch)/launch/geotiff_mapper.launch">
```

# 地図作成
以下の手順通り実行してください.

```bash:
$ roslaunch rplidar_ros rplidar.launch
$ roslaunch hector_slam_launch tutorial.launch
```
以下の画像のように地図が生成されれば成功です!
![Screenshot from 2020-12-24 05-36-54](https://user-images.githubusercontent.com/52307432/103092673-5c456a80-463b-11eb-98cc-0a94b511637a.png)

作成した地図を以下のコマンドで保存しましょう.

```bash:
$ rosrun map_server map_saver -f hectorslam
```
すると以下のように地図データが保存されます.
![hectorslam](https://user-images.githubusercontent.com/52307432/103092924-3371a500-463c-11eb-81c4-f54ee67959d7.png)
また個人的にはhector_slamではtrajectory情報も取得することが出来る事に驚きました.

```bash:
$ rostopic echo /trajectory
```
上記コマンドで以下のtrajectory情報が取得できます.

```bash:
-
header: 
      seq: 0
      stamp: 
        secs: 1608817265
        nsecs: 805512208
      frame_id: "/map"
    pose: 
      position: 
        x: 23.3810806274
        y: -70.6297912598
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: -0.00920321632247
        w: 0.999957649508
-
```
下の画像の緑色の線が軌跡を意味します. 
![Screenshot from 2020-12-24 05-40-47](https://user-images.githubusercontent.com/52307432/103092675-5d769780-463b-11eb-96ac-2c912921a569.png)

# まとめ
GmappingとHector SLAMで部屋の地図を作成しましたが, 正直私の部屋くらいの広さだと, どちらが有効なのかまではよく分かりませんでした. 広域の環境でどのような差が生まれるのか今後検証していく予定です. また今回使用したコードは以下のGithubのリポジトリにあるので適宜ご参照ください.

https://github.com/Ramune6110/2D-Mapping

# 今後の予定
- 地図精度を上げるためのパラメータ調整についても追加でまとめたいと思います.