ssk tech blog

自律移動ロボットの話を書きます

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パッケージが必要なようです。

GitHub - at-wat/neonavigation: A 2-D/3-DOF seamless global/local mobile robot motion planner package for ROS

データはカートグラファーのデモ用の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

結果

作られた地図 f:id:ssk0109:20181209135240p:plain 自己位置推定の様子 f:id:ssk0109:20181209133909p:plain f:id:ssk0109:20181209212420g:plain 色々あんま弄ってないので荒ぶっています。
が,とりあえず今回はここまで。
追記:下gifのようにCartographerのデモデータの点群が不連続なのが良くなさそう? f:id:ssk0109:20190101021026g:plain
追記2:Autowareのpoints_concat_filterで複数のpointcloudを統合すると良いっぽい?

参考にしたもの

google-cartographer-ros.readthedocs.io

関連記事

ssk0109.hatenablog.com