Octomapで遊んでみた #ROS

私自身，ナビゲーションやSLAM等の知識はほぼ無く初心者ですが，最近自動運転やらなにやらで話題で，勉強してみたいとは思いつつ，確率ロボティクスでいきなり勉強をしようとすると式がいっぱいで難しい…．私のスタンスとしてはやはり手を動かしながら進めつつ理論を追うのがはかどるので，初心者ではありますが，今回ちょっと色々試しながら遊んでみたいと思います．今回はOctomapを使ってみたので，試し方をメモ書き程度に残しておこうと思います．

Octomapとは

センサ情報から簡単に3次元で環境認識を行うことができるライブラリです(URL)．3次元での占有グリッドマップを作成することが出来ます．MoveItでは障害物回避に使われたりしていますね．

検証

今回はGazebo上でTurtlebotを使用してOctomapで遊んでみることにします．環境はUbuntu 14.04，indigoです．以下のレポジトリで各ノードを立ち上げるlaunchファイルを作成したので，もしよろしければお試しいただければと思います．一応ubuntu16.04,kineticでも確認しましたが概ね動くと思います.

インストール

github上にもインストール方法は記載しましたが，一応こちらにも書いていくことにします．indigoでのインストール方法になりますが，それ以外のディストリビューションを使っている場合は適宜入れ替えてインストールしてください．

Octomap及びRTAB-mapのインストール
もしインストールされていないようでしたら，以下のコマンドによりインストールしてください．

$ sudo apt-get install ros-kinetic-octomap ros-kinetic-octomap-rviz-plugins ros-kinetic-rtabmap ros-indigo-kinetic-ros

後述しますが，Octomapを使用するにあたって，センサから得られる点群情報により地図を作成していくのですが，そのときに点群及びセンサの座標が必要になります．なのでそのまま点群情報を与えても駄目なわけで，そのときに使えるのがRTAB-mapになります．

Turtlebotのインストール
こちらも同じくインストールされていないようでしたら以下のコマンドでインストールして下さい．

$ sudo apt-get install ros-kinetic-turtlebot*

今回では使わないものが多く含まれていますがまぁこの際全部インストールしちゃいます．

ROSのワークスペースにソースコードを持ってくる ROSはインストール済みさらにはワークスペースも作成済みという設定で話を進めます．

$ cd ~/catkin_ws/src
$ wstool init
$ wstool set turtlebot_octomap https://github.com/RyuYamamoto/turtlebot_octomap.git --git
$ wstool update turtlebot_octomap

実行環境構築が完了したら，以下のコマンドで実行してみます．

$ roslaunch turtlebot_octomap turtlebot_build_map.launch

そうすると，GazeboとRvizが表示されると思います(以下のような感じです)．

　launchのディレクトリないには，turtlebot_build_map.launchとturtlebot_rtabmap.launchが存在し,turtlebot_rtabmap.launchでRTAB-mapの起動，turtlebot_build_map.launchで先ほどのlaunchファイルの読み込みと，gazebo，octomap_server，Rvizの起動を行っています.
　上記のlaunchファイルを起動した際，teleopも起動しておりここではlogitech.launchを起動していますが，ここは環境に応じて適宜入れ替えて下さい．
　起動できたら自由に動かして地図を作成してみましょう．適当に動かしてみるとこんな感じで地図が作れます．

　ちなみに結構重いと思います．私の環境ではCorei7で動かしていましたが時折カクカクすることがありました．

以下，WillowGarageのモデルで地図生成を行っている動画です．

内容

　Octomapで地図を構築するためにはcloud_inトピックに点群情報を入れてあげることが必要です．ですが，例えばKinectを使用していればKinectで得られる点群情報(PointCloud2型)をそのまま入れればいいと思われますが，地図作成を行う場合は不十分で，実際には各点群の座標の情報が必要になってきます．何かしらの方法で座標情報が付与された点群を入れてあげることで，地図作成が可能になります．
　いろいろググると，RTAB-mapを使用するらしいです(参考:rtabmap_ros)．RTAB-mapはグラフ構造を用いたSLAM手法であり，センサから得られるRGB画像とDepth画像から3次元地図を作成します．このとき得られる地図の各点群はそれぞれ座標をもっており，この情報から随時octomap_serverにより占有グリッドマップに変換していきます．
　RTAB-mapを立ち上げると，/rtabmap/cloud_mapという名のトピックがでてきます．このトピックからRTAB-mapで構築される地図情報を得ることが出来ます．そして，この情報をOctomapのcloud_inトピックへremapしてあげることでグリッドマップを作成できます．

<remap from="cloud_in" to="/rtabmap/cloud_map" />

ちなみにKinectが手元にある場合は，以下のコマンドを実行することでKinect単体で地図生成を行うことが可能になります．

$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch rtabmap_ros rgbd_mapping.launch rtabmapviz:=false rviz:=true
$ rosrun octomap_server octomap_server_node cloud_in:=/rtabmap/cloud_map

まとめ

　とりあえずOctomapで地図作成をすることができました．今後どんな理論で動いているのか気になるので，論文等と伏せて勉強していきたいです(論文はこちら)．
　あとはせっかく地図生成やったのでナビゲーションもやってみたいですねぇ．

参考

追記(2019/03/25)

turtlebot3用のlaunchファイルを追加しました。詳しくはREADMEを参照してください。