Google Cartographerで作った地図でmcl_3dlによる自己位置推定
はじめに
こんにちは.ササキ(@saitosasaki)です.
今回はGoogle Cartographerで作った三次元地図で自己位置推定(ローカリゼーション)パッケージmcl_3dlを動かしました.
mcl_3dlはnavigationパッケージにあるamcl(adaptive Monte Carlo localization)の三次元版でトピックもできるだけ共通化しているそうです.なので代替できるかもと思い,実際に動かしてみました.
https://github.com/at-wat/mcl_3dlgithub.com
mcl_3dlの解説スライド
mcl_3dl: amcl並に軽量な3-D/6-DoFローカリゼーションパッケージ
mcl_3dlの解説スライドの解説動画
ROSCon JP 2018: 5. mcl_3dl amcl並に軽量な3-D 6-DoFローカリゼーションパッケージ on Vimeo
今回データにodomがなかったのですが、オドメトリフリーで動かす場合は同じくat-watさんが作っているneonavigationパッケージが必要なようです。
データはカートグラファーのデモ用のbagファイルを使用.
コンフィグとlaunch弄ったりコマンドポチポチしてるだけでいけました.
実行フロー
1. デモデータのダウンロード
カートグラファーの三次元自己位置推定用デモファイルをダウンロードします
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-13-54-42.bag wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-15-52-20.bag
2. bagファイルの切り取り
これをしないと自分のPCだと点群データが重すぎて,ダウンサンプリング時にコアダンプしました().(繋げるときはpclやCloudCompare等で)
rosbag filter ${HOME}/Downloads/b3-2016-04-05-13-54-42.bag ${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag "t.secs <=1459864582.67"
3. bagファイルからbag.pbstreamファイルを作る
bag.pbstreamはカートグラファーで自己位置推定する時に使う地図や軌道情報形式が入っている専用の形式です.
roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag
4. bagとbag.pbgstreamファイルからpcdファイルを作る
pcdファイルは点群において一般的なファイル形式です .
まずpcdファイルを出力するように設定ファイルを弄ります .
cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua options = { tracking_frame = "base_link", pipeline = { { action = "min_max_range_filter", min_range = 1., max_range = 60., }, { action = "dump_num_points", }, ・・・ { -- add action = "write_pcd", -- add filename = "points.pcd", -- add }, -- add } }
編集したらcatkin_make忘れずに.次コマンドでpcdファイル出力.
roslaunch cartographer_ros assets_writer_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag pose_graph_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag.pbstream
ダウンサンプリング.
pcl_voxel_grid ${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag_points.pcd ${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag_points_down.pcd -leaf 0.1,0.1,0.1
5. 自己位置推定
mcl_3dlのtest.launchを弄ります。
rosbagがpublishするtopicのリマップの追加。
<node pkg="rosbag" type="play" name="playback" args="--clock $(arg bag_file)" if="$(arg use_bag_file)"> <remap from="odom" to="odom_unused" if="$(arg without_odom)" /> <remap from="imu" to="imu/data"/> <!-- add --> <remap from="horizontal_laser_3d" to="cloud"/><!-- add --> </node>
同じくlaunchにtf関係の追加。
<param name="robot_description" textfile="$(find cartographer_ros)/urdf/backpack_3d.urdf" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
最後にlaunch。
roslaunch mcl_3dl test.launch use_neonavigation:=true without_odom:=true use_pointcloud_map:=true map_pcd:=${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag_points_down.pcd use_cad_map:=false use_bag_file:=true bag_file:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag
結果
作られた地図
自己位置推定の様子
色々あんま弄ってないので荒ぶっています。
が,とりあえず今回はここまで。
追記:下gifのようにCartographerのデモデータの点群が不連続なのが良くなさそう?
追記2:Autowareのpoints_concat_filterで複数のpointcloudを統合すると良いっぽい?
参考にしたもの
google-cartographer-ros.readthedocs.io