5章.SLAMを使ってみる

本章の流れ

自作ロボットを自動航行する上で、本章では私が使ったLiDARと実際に試したSLAMを取り上げ、実際に動作させる所までを書きます。

1.SLAMについて
 1.1.SLAMとは
 1.2.SLAMとTFの関係
2.使うLiDAR
 2.1.RPLiDAR A1を使ってみる
 2.2.Camsense X1を使ってみる
 2.3.両者を使ってみた感想
3.使ったSLAM
 3.1.gmapping_slam
 3.2.hector_slam
 3.3.cartgrapher
4.今までの道のり
5.参考文献

1.SLAMについて

　自作ロボットを自動航行させる際にとても重要なテーマとなる点がこの”SLAM”だと思っております。そのためボリュームある内容ですが、こちらで3つのSLAMが扱えるようになれます。

1.1.SLAMとは

　SLAMとは”Simultaneous Localization and Mapping”の略で、環境地図を生成します。この環境地図があれば、現在レーザースキャンしている距離と照らし合わせ、自分の位置を認識することが出来るようになり、自動航行が出来るようになります。そこで今回はこの環境地図の生成にスポットを当てて記事を書きます。

1.2.SLAMとTFの関係

SLAMとTFの関係について、私の勝手な解釈で考え易くした図になりますが、以下の様に表してみました。

【TFの考え方】
またこちらの図について詳細を記述します。
①map原点の中にodom原点が存在し、SLAMによってodom原点を基準にしてmapが生成される。SLAMを使ってmapが生成している時は、odomとmapの位置は一致している。SLAMによって出来上がったmapを参照しながら、odom原点がmap上のどこに位置するか調整しながら自分の位置を認識するのがAMCL（自己位置推定）の役割になる。

②odom原点の中にbase_link原点が存在し、これらを繋ぎ合わせる方法として以下の様な方法がある
　・モータエンコーダによってodom原点から機体(base_link)の移動量を算出する(エンコーダ値とホイール径から計算するノードを作る)
　・IMUセンサやGPSセンサなどでodom原点から機体の移動量を算出する(RealsenseT265やZED,ZED2等)
　・SLAMの中にはodomとbase_linkを繋いでくれるものもある（cartographer、orb_slam等）
　これらの方法を複合的に使ったり、状況に応じて使い分けたりして位置精度を向上・補償する技術が沢山生まれています。

③base_link原点の中にlaser_link原点があり、LiDARが機体に固定されているならばstatic_transform_publisherを使ってbase_linkとlaser_linkをつなぎ合わせてしまうのがお薦めです。

2.使うLiDAR

今回以下の2種類のLiDARを使ってみました。

2.1.RPLIDAR A1を使ってみる

以下によってRPLiDAR A1を動かせるようになります。
 RPLiDAR A1でSLAMを動かす場合は、こちらを参考にスキャン出来るようにしてみて下さい。

3章.LiDARを動かす

2.2.Camsense X1を使ってみる

以下によってCamsense X1を動かせるようになります。
 Camsense X1でSLAMを動かす場合は、こちらを参考にスキャン出来るようにしてみて下さい。

Camsense X1を使ってROS上でレーザースキャンしてみる

2.3.両者を使ってみた感想

RPLIDAR A1とCamsense X1を動かしたところ、モータの力と回転数に値段通りの差があり、RPLIDAR A1の方が元気よく回っており、/scanの周期に以下の差が出ました。　

 RPLIDAR A1の/scan tpoic周期 : 7.92Hz
 Camsense X1の/scan tpoic周期 : 5.16Hz

なお測定距離や精度については、しっかりとまだ確認しておりません。

3.使ったSLAM

　上記CamsemseX1とRPLiDAR A1を使って、私がSLAMを使用した環境はubuntu18.04LTSとなっており、ROS Melodicで確認しております。しかしPCによる個体差もあると思いますのでご了承下さい。

3.1.gmapping_slam

gmapping_slamはこちらの素晴らしい記事を参考にしながら、自分の環境で確実に動くように修正してみました。

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

こちらと同様にまずはgmappingをビルドします

gmappingをビルド

$ cd catkin_ws/src
$ git clone https://github.com/ros-perception/slam_gmapping.git
$ cd ..
$ catkin_make

そして/catkin_ws/src/slam_gmapping/gmapping/launchの中に gmapping.launch を作成し、以下を記述します。

gmapping.launchの記述内容

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

    <param name="use_sim_time" value="false"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <param name="base_frame" value="base_link"/>
      <param name="odom_frame" value="base_link"/>
      <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>

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

また/catkin_ws/src/rplidar_ros/launchの中にある rplidar.launch を編集します。

rplidar.launchの編集

<launch>
  <!-- run rplidarNode -->
  <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"/><!--A1/A2 -->
    <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
    <param name="frame_id"            type="string" value="laser"/>
    <param name="inverted"            type="bool"   value="false"/>
    <param name="angle_compensate"    type="bool"   value="true"/>
  </node>

  <!-- run static_transform_publisher -->
  <node pkg="tf" type="static_transform_publisher" name="baselink_to_laser" args="0.08 0 0.2 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"/>
</launch>

ここで注目すべき点は下方にあるstatic_transform_publisherの使い方です。
上記で示した通り、base_linkとlaserを繋ぎ合わせるノードは必ず必要になり、機体(base_link)とLiDAR(laser)の位置関係をbaselink_to_laserで繋ぎ合わせています。
しかしもう一方のodom_to_baselinkはodomを出力できていない場合の暫定処置で、base_linkを無理やりodomに固定しているため、今後自動航行する場合には向いていません。この場合、LiDARを移動させてしまうと自分が移動したことを認識できていない（odom → base_linkが動かない）ため、地図がぐちゃぐちゃになってしまいます。そのためodomを出力できるノードが動いているのであれば、この行をコメントアウトして下さい。
　ちなみにodomの出力については、　qiita 1章.ロボットのROS対応　で書かせてもらいました ”5.SPRESENSEでのArduinoスケッチ” の下方でodomを出力するコードを記述していますので時間があればご確認下さい。

もしodomを出力せずに自動航行するのであれば、cartographerを使ってodomを出力出来るようにする事をお薦めします。

同様にCamsense X1を起動するlaunchを作成します.
/catkin_ws/src/camsense_driver/launchの中にcamsense.launchを作成し、以下の内容を記述します。

camsense.launchの記述内容

<launch>
  <arg name="port" default="/dev/ttyUSB0" />
  <arg name="offset" default="16.0" />

  <!-- run camsense_driver -->
  <node pkg="camsense_driver" type="camsense_publisher" name="camsense_publisher" output="screen">
    <param name="port" value="$(arg port)"/>
    <param name="offset" value="$(arg offset)"/>
    <param name="frame_id" value="laser"/>
  </node>

  <!-- run static_transform_publisher -->
  <node pkg="tf" type="static_transform_publisher" name="baselink_to_laser" args="0.43 0 0.20 3.14159 0 0 base_link laser 30" />   

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

</launch>

ではまずRPLIDAR A1を使ってgmapping_slamを実施してみます。各launchの起動ごと別のターミナルで実行して下さい。

rplidar.launchの起動

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch rplidar_ros rplidar.launch

gmapping.launchの起動

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch gmapping gmapping.launch 

ここでrviz上に現れた画面にmapを付け足すため以下を行って下さい。

これによってmapが生成されたと思います。

では次にCamsense X1を使ってgmapping_slamを実施してみます。各launchの起動ごと別のターミナルで実行して下さい。

camsense.launchの起動

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch camsense_driver camsense.launch

gmapping.launchの起動

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch gmapping gmapping.launch 

CaMSENSE x1でmapを生成した際の画像も添付します。

やっぱり価格なりにレーザーの反射が突き抜けて戻って来れない箇所が幾つかあるようです。

3.2.hector_slam

hector_slamについてはMatsuiさんの素晴らしい以下の記事を参考にさせて頂き、gmappingと同じ環境でhector_slamも使えるようにしてみました。
安価な2DレーザスキャナRPLidar A2とROSでSLAMを試す
この記事にある通り、catkin_ws/src/rplidar_hector_slam/hector_slam/hector_mapping/launch/mapping_default.launchを修正します。

mapping_default.launchの修正

<launch>
  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
  <arg name="base_frame" default="base_link"/>
  <arg name="odom_frame" default="odom"/>

  <arg name="pub_map_odom_transform" default="true"/>
  <arg name="scan_subscriber_queue_size" default="5"/>
  <arg name="scan_topic" default="scan"/>
  <arg name="map_size" default="2048"/>

  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

    <!-- Frame names -->
    <param name="map_frame" value="map" />
    <param name="base_frame" value="$(arg base_frame)" />
    <param name="odom_frame" value="$(arg odom_frame)" />

    <!-- Tf use -->
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>

    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="$(arg map_size)"/>
    <param name="map_start_x" value="0.5"/>
    <param name="map_start_y" value="0.5" />
    <param name="map_multi_res_levels" value="2" />

    <!-- Map update parameters -->
    <param name="update_factor_free" value="0.4"/>
    <param name="update_factor_occupied" value="0.9" />    
    <param name="map_update_distance_thresh" value="0.4"/>
    <param name="map_update_angle_thresh" value="0.06" />
    <param name="laser_z_min_value" value = "-1.0" />
    <param name="laser_z_max_value" value = "1.0" />

    <!-- Advertising config --> 
    <param name="advertise_map_service" value="true"/>    
    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
    <param name="scan_topic" value="$(arg scan_topic)"/>

    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
  </node>

</launch>

また/catkin_ws/src/hector_slam/hector_slam_launch/launch/tutorial.launchも以下の様に修正しました

tutorial.launchの修正

<launch>

  <arg name="geotiff_map_file_path" default="$(find hector_geotiff)/maps"/>
  <param name="/use_sim_time" value="false"/>

  <node pkg="rviz" type="rviz" name="rviz"
    args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>

  <include file="$(find hector_mapping)/launch/mapping_default.launch"/>

  <include file="$(find hector_geotiff_launch)/launch/geotiff_mapper.launch">
    <arg name="trajectory_source_frame_name" value="scanmatcher_frame"/>
    <arg name="map_file_path" value="$(arg geotiff_map_file_path)"/>
  </include>

</launch>

ではRPLIDAR A1を使ってhector_slamを実施してみます。各launchの起動ごと別のターミナルで実行して下さい。

rplidar.launchの起動

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch rplidar_ros rplidar.launch

tutorial.launchの起動

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch hector_slam_launch tutorial.launch  

ではCamsense X1を使ってhector_slamを実施してみます。各launchの起動ごと別のターミナルで実行して下さい。

camsense.launchの起動

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch camsense_driver camsense.launch

tutorial.launchの起動

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch hector_slam_launch tutorial.launch  

これによってmapが生成されたと思います。
hector_slamでは、mapとodomがscanmatcher_frameによって繋がり、LiDARが移動しても追従できるようになりました。このmapとodomの繋ぎ方についてはまた今度確認していきたいと思います。

3.3.cartgrapher

cartgrapherについては以下の記事にある素晴らしいもので簡単に実装できることがわかりました！
2D Mapping using Google Cartographer and RPLidar with Raspberry Pi

一応cartographer自体もインストールしておきましょう。

cartographerのインストール

$sudo apt install ros-melodic-cartographer-ros

このあとgbot_coreをインストールします。

gbot_coreの入手

$ cd ~/catkin_ws/src
$ git clone https://github.com/Andrew-rw/gbot_core.git
$ cd ..
$ catkin_make

これだけで私の場合はRPLiDAR A1でcartgrapherが動いてしまいました！

gbot.launchの起動（RPLiDAR_A1でcartgrapher起動）

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch gbot_core gbot.launch  

visualization.launchの起動（rvizでcartgrapherを表示）

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch gbot_core visualization.launch  

$\huge{素晴らしい！！}$

ではCamsense X1でも動かせるよう、/catkin_ws/src/gbot_core/launchの中にcamsense_x1.launchを作成し、以下を記述します。

camsense_x1.launchの記述内容

<launch>
  <!-- Load robot description and start state publisher-->
   <param name="robot_description" textfile="$(find gbot_core)/urdf/head_2d.urdf" />
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

  <!-- Start RPLIDAR sensor node which provides LaserScan data  -->
   <arg name="port" default="/dev/ttyUSB0" />
   <arg name="offset" default="16.0" />

  <!-- run camsense_driver -->
   <node pkg="camsense_driver" type="camsense_publisher" name="camsense_publisher" output="screen">
     <param name="port" value="$(arg port)"/>
     <param name="offset" value="$(arg offset)"/>
     <param name="frame_id" value="laser"/>
   </node>

  <!-- Start Google Cartographer node with custom configuration file-->
   <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args="
          -configuration_directory
              $(find gbot_core)/configuration_files
          -configuration_basename gbot_lidar_2d.lua" output="screen">
   </node>

  <!-- Additional node which converts Cartographer map into ROS occupancy grid map. Not used and can be skipped in this case -->
   <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

   <node pkg="rviz" type="rviz" name="show_rviz" args="-d $(find gbot_core)/rviz/demo.rviz"/>    

</launch>

さぁ！Camsense X1でcartographerを動かしてみます。

camsense_x1.launchの起動（Camsense_X1でcartgrapher起動）

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch gbot_core camsense_x1.launch  

なんと！激安LiDARでもここまで出来るとは！！

このcartographerによってLiDARだけでもodomを出力することができます。
確かに出力する/scanの量が少ないからなのかズレが発生する場面があったり、問題点は色々出てきますが、$\huge{この価格で試せるなら嬉しい限り！}$
それでも大きな地図を作成しようと思うと、これらでは厳しくなってくるのでodom→base_link間の位置を正確に出力出来る様な工夫が必要なことも分かってくると思います。

$\huge{これでSLAMが3つ使えるようになれたら、}$
　$\huge{ ROSが楽しくなってくる！}$
のではないでしょうか。

4.今までの道のり

作って分かった「ROSを使う前のロボットと、ROSを使った後のロボットの変化」

目次： ROSを使った自動航行ロボットをつくるまでの道のり　ー序章　概要ー
1章.ロボットのROS対応
2章.ロボットを操作する
3章.LiDARを動かす
前章 ：4章.urdfを作成する

5.参考文献

RPLidar A1 M8で部屋の地図を作成しよう!
安価な2DレーザスキャナRPLidar A2とROSでSLAMを試す
2D Mapping using Google Cartographer and RPLidar with Raspberry Pi