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

本記事の目的

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

環境

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

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

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

RPLidarのパッケージをインストール

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

$ 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に接続します.

次にRPLidarのUSBデバイス接続の権限を与えます.

$ sudo chmod 666 /dev/ttyUSB0

roscoreを起動してRPLidarの点群データをrvizで見て観ましょう.

$ roscore
$ roslaunch rplidar_ros view_rplidar.launch

以下の画像のように点群データが得られていれば成功です!

実際の点群データは/scanトピックで見る事が出来ます

$ rostopic echo /scan

以下のようなデータ型を確認できるはずです.

-
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): 原理をみる(応用編)

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

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

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>

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つのファイルを保存したら順に実行します.

$ roslaunch rplidar.launch
$ roslaunch gmapping.launch

以下の画像のように地図が生成されれば成功です!

地図生成中にターミナルでは以下のような時系列データが流れています.

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

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

$ rosrun map_server map_saver -f gmapping

すると以下のように地図データが保存されます.

Hector SLAMで地図を作成しよう

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

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

$ 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を参考に修正を行いました.

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

修正点1

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

<!-- 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行目を以下のように修正します.

<!-- 7行目 -->
<param name="/use_sim_time" value="false"/>

修正点3

hector_slam/hector_slam_launch/launch/tutorial.launchファイル内の14行目を以下のように修正します.

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

地図作成

以下の手順通り実行してください.

$ roslaunch rplidar_ros rplidar.launch
$ roslaunch hector_slam_launch tutorial.launch

以下の画像のように地図が生成されれば成功です!

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

$ rosrun map_server map_saver -f hectorslam

すると以下のように地図データが保存されます.

また個人的にはhector_slamではtrajectory情報も取得することが出来る事に驚きました.

$ rostopic echo /trajectory

上記コマンドで以下のtrajectory情報が取得できます.

-
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
-

下の画像の緑色の線が軌跡を意味します.

まとめ

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

今後の予定

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