ROSで動かすロボットカー作り(SLAMをできるようにしてみた編その2)

前回はgmappingを使ってSLAMをやろうとするところまでで終わりました

supernove.hatenadiary.jp

このブログの中では地図が更新されなくて多分机の上で動かすだけだと更新されないと思ってました。

が、これは大きな勘違いで正確にはodometryのデータを一つも用意してなかったのが原因です。

gmappingではロボットの移動距離の情報としてodometryをロボット側で配信する必要がありますが、それができてませんでした。

ただ、今作っているロボットにはロータリーエンコーダーとかを使ってモーターの回転数取れればいいのですが、なるべく楽したいのでLidarセンサーだけで地図を作成する方法を模索しました。

今回の成果物もいつも通り以下のレポジトリの backlog/chapter6ブランチで切っています。

github.com

用意するもの

前回と同様のものですが、一応載せておきます

動作環境

今回もRaspberry Pi上で試すのでRaspberry Piの動作環境を紹介します

LidarセンサーだけでSLAMをやる方法

ググったらいくつか候補が出てきたので試しました。

候補1：gmapping + laser_scan_matcher

gmapping単体だとodomデータが足りなくて地図が更新されませんが、laser_scan_matcherを使ってLidarのデータの変化量から移動距離を算出します。

sudonull.com

この方法ならセンサーを用意したり自分でロジックを書かなくても地図が更新されるようになるみたいです。

ただ、Raspberry Pi OSだとlaser_scan_matcherを使うために必要な依存パッケージが多いのとビルドエラーが出まくっているので今回は却下しました。

候補2：hector_slam

gmappingの代わりにhector_slamを使う方法が候補にありました。

これならodomデータがなくてもパッケージ単体で地図を生成できるらしいです。

試しに動かしてみたらセンサー単体を動かすだけでも簡単に地図を更新できるようになったので今回はこの方法で地図を生成してみました。

hector_slamをインストール

まずは依存パッケージをインストールします。

sudo apt install libflann-dev libeigen3-dev libglvnd-dev

ワークスペースのディレクトリ直下のsrcディレクトリ内にhector_slamのソースコードをクローンします。

git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam

ビルドを実行します。

catkin build

パッケージを追加したのでセットアップスクリプトを実行します。

source devel/setup.bash

これで必要な物は揃いました。gmappingのときと同じぐらい簡単にインストールが終わりました。

とりあえず動かしてみる

インストールはできたので試しに机の上でセンサーを動かしてみます。

今回は以下のlaunchファイルを使います。

ファイル名は hector_slam.launchとします。

<launch>

  <!-- X2L -->
  <node name="ydlidar_node"  pkg="ydlidar_ros"  type="ydlidar_node" output="screen" respawn="false" >
    <param name="port"         type="string" value="/dev/ydlidar"/>
    <param name="baudrate"         type="int" value="115200"/>
    <param name="frame_id"     type="string" value="base_link"/>
    <param name="resolution_fixed"    type="bool"   value="true"/>
    <param name="auto_reconnect"    type="bool"   value="true"/>
    <param name="reversion"    type="bool"   value="false"/>
    <param name="angle_min"    type="double" value="-180" />
    <param name="angle_max"    type="double" value="180" />
    <param name="range_min"    type="double" value="0.1" />
    <param name="range_max"    type="double" value="12.0" />
    <param name="ignore_array" type="string" value="" />
    <param name="frequency"    type="double" value="8"/>
    <param name="samp_rate"    type="int"    value="3"/>
    <param name="isSingleChannel"    type="bool"   value="true"/>
  </node>

  <!-- tf -->
  <node pkg ="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0.0 0.0 0.0 /map /nav 40"/>
  <node pkg ="tf" type="static_transform_publisher" name="odom_to_base_link" args="0.0 0.0 0.0 0.0 0.0 0.0 /nav /base_footprint 40"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.2245 0.0 0.2 0.0 0.0 0.0 /base_footprint /base_link 40" />
  
  <!-- hector_slam -->
  <include file="$(find hector_mapping)/launch/mapping_default.launch" />
  <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch" />
</launch>

gmappingのときは直でパラメータを指定してましたが、hector_slamではデフォルトで用意されているlaunchファイルを呼び出すようにしています。

ちなみに上記のlaunchファイルは以下の記事を参考にX2L向けにアレンジしました

archit0994.wixsite.com

実際に動かしてみます。

以下のコマンドでlaunchファイルを動かします。

roslaunch my_rober hector_slam.launch

そして可視化は以下のコマンドでrvizを起動します。(前回の同じ設定ファイルを読み込んでますが、他のSLAMパッケージでも使えるのでファイル名を変えてます)

rosrun rviz rviz -d $(find $(pwd) -name slam.rviz)

起動したらセンサーをいろいろ動かしてみるとそれに合わせて地図が更新されていることが確認できます。

navigationパッケージのセットアップ

無事に動作することが分かったところで実際に地図を作成していきます。

ただ今のままだとロボットを走らせて地図を作成してもその地図を保存する手段がありません。

地図を保存するためには navigationパッケージの map_serverを使用します。

そこで地図を作成する前にnavigationパッケージをセットアップしていきます。

まずは以下のコマンドで依存ライブラリをインストールします。

sudo apt install libbullet-dev libsdl-image1.2-dev libsdl-dev

ワークスペースディレクトリ直下のsrcディレクトリ上でnavigationパッケージとその依存パッケージをクローンします。

git clone -b melodic-devel https://github.com/ros-planning/navigation.git
git clone -b melodic-devel https://github.com/ros/geometry2.git
git clone -b ros1 https://github.com/ros-planning/navigation_msgs.git

ビルドを実行します。

catkin build

hector_slamと同様にセットアップスクリプトを実行します。

source devel/setup.bash

地図を作成する

いよいよ地図を作成していきます。

まずは以下の記事を参考にJoystickでロボットを動かす準備をします。

supernove.hatenadiary.jp

地図を作成する場所にロボットを設置したら以下のコマンドでマッピング用のノードを起動します

roslaunch my_rober hector_slam.launch

これでゲームコントローラーでロボットを操作しながら地図を作成します。

一通り地図を作成したら以下のコマンドで地図のデータを保存します。

 rosrun map_server map_saver

作成した地図はこんな感じです。

Lidarのみで作成しているだけに結構ぐちゃぐちゃした地図になってますね…

まとめ

今回はhector_slamを使ってSLAMに挑戦しました。

Lidarだけでマッピングできると思ってやってましたが、やっぱり急な方向転換に弱いせいか精度は今ひとつでしたね。

今度は精度の高い地図を作成するためにどうすればいいか試行錯誤していこうと思います。