はじめに
こんにちは。
本記事はROS2 アドベントカレンダーの12日目です。遅れて申し訳ありません。
前回は@smakitaさんの「WSL に ROS2 をインストールします」、
次回は@youtalkさんの「QoSプロファイルの実践と課題」です。
今回は、以前ROS2で3D-SLAMできそうなソフトウェアを探したんですがなさそうで、pcl(point cloud library)でNDT(Normal Distributions Transform) scan matchingによるマップマッチングによる自己位置推定のコードがあったので、それを基に勉強がてらマッピングとマップマッチングによる自己位置推定のコードを書いて動かしました！
ちなみに、ROS2で2D-SLAMはROBOTIS社のロボットturtlebot3のROS2チュートリアルで実機・simulationともに簡単にできます。こちらのSLAMはgoogle cartographerを使っています。なお、これによる3D-SLAMの移植はされていません。
http://emanual.robotis.com/docs/en/platform/turtlebot3/overview/
また、gmappingも移植している人がいて、その移植版を使っている人(リンク先Qiita記事)もいるようです。
https://github.com/Project-MANAS/slam_gmapping
他にもslam_toolbox等があります。
環境
ubuntu：18.04
ros1：melodic
ros2：dashing
PCスペック
メインメモリ：16GB
クロック数：3.5GHz(The governor "performance")
キャッシュメモリ：8MB
コア：4コア
NDT scan matchingとは
NDT scan matchingについて詳しくは以下を参照ください。
gggggraziegrazie NDT( Normal Distributions Transform )
http://graziegrazie.hatenablog.com/entry/2018/07/01/005613
NDTスキャンマッチング 第1回3D勉強会@PFN 2018年5月27日
https://www.slideshare.net/YukiKitsukawa/ndt-13dpfn-2018527
説明は一番上のgraizegrazieさんさんのブログ記事から引用します。
スキャンマッチングの説明。
NDT[1]とは、２つの点群のマッチングをさせる手法の１つです。点群マッチングとは、ある点群（参照点群と呼ぶ）に対し、ある点群（入力点群と呼ぶ）がどれだけ重なっているか、重ねるためには点群をどの様に移動させるべきかを判断するための手法です。
NDTの説明
これに対してNDTは、ボクセルやグリッドを使って点群が存在する空間を等間隔に分割するし、ボクセル又はグリッド毎に求めた正規分布を使って点群の表現、マッチングを行うため、計算量を削減することができます。
pclのndtのソースは以下です。ソースに参考文献も書いてあります。
https://github.com/PointCloudLibrary/pcl/blob/master/registration/include/pcl/registration/impl/ndt.hpp
ソース
プログラムは以下のソースを参考に書きました。
https://github.com/OUXT-Polaris/pcl_apps
書いたコードはGithubにあります。
マッピング
https://github.com/rsasaki0109/pcl_apps/tree/master/pcl_apps/src/mapping
マッチング(こっちは基のコードを微修正しただけ)
https://github.com/rsasaki0109/pcl_apps/tree/master/pcl_apps/src/matching
実験
マッピング
設定
ndtのresolutionは5.0(屋外環境での値の目安は5~10mくらい)、
https://gitlab.com/autowarefoundation/autoware.ai/core_perception/blob/master/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp#L116
入力点群のvoxel_gridのサイズは以下を参考に0.2にしました。
https://gitlab.com/autowarefoundation/autoware.ai/core_perception/blob/master/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp#L121
実行手順
rosbagデータはAutowareのROS1のdemoデータを使います。
ROS2でROS1のbagデータを再生するにはrosbag2に加え、rosbag2_bag_v2をインストールする必要があります。aptで入ります。
ちなみに3D LiDAR SLAMと言ったらvelodyneが有名ですが、velodyneのROS2ドライバーは現状以下の様らしいです。
ros2のvelodyneドライバー、unstableすぎる
— 片岡大哉 @ 技術書典 い23 (@hakuturu583) October 28, 2019
30秒以上もたない
また、他のドライバーでは以下が見つかりました(動作未確認)
demoデータのダウンロード
wget http://db3.ertl.jp/autoware/sample_data/sample_moriyama_150324.tar.gz
tar zxfv sample_moriyama_150324.tar.gz
ノードの起動
ros2 launch pcl_apps ndt_mapping.launch.py
(ちなみにros2 run pcl_apps ndt_mapping_node ndt_mapping/input:=/points_raw
でも起動できますが、ros dashingではros2 runでパラメータを渡せないので面倒です。次の世代のros eloquentだとできるようになります)
位置姿勢初期値を指定します。
ros2 topic pub ndt_mapping/initial_pose geometry_msgs/PoseStamped '{ pose: {position: {x: 0, y: 0}, orientation: {z: 0, w: 1}}}' --once
rosbag再生。
ros2 bag play -s rosbag_v2 sample_moriyama_150324.bag
結果
mappingした結果がこちらです。
大丈夫そうですね！
マップマッチングによる自己位置推定
マップマッチングは、自己位置推定パッケージmcl_3dlのデモデータを使って自己位置推定します。
設定
以下を参考にndtのresolutionは0.2,入力点群のvoxel_gridのサイズは0.1にしました。
https://github.com/at-wat/mcl_3dl/blob/master/config/test_localization.yaml
実行手順
demoデータのダウンロード
wget https://openspur.org/~atsushi.w/dataset/mcl_3dl/short_test.bag
rviz起動
rviz2 -d src/pcl_apps/pcl_apps/config/matching.rviz
nodeの起動
ros2 launch pcl_apps ndt_matching.launch.py
位置姿勢初期値を指定します。
ros2 topic pub ndt_matching/initial_pose geometry_msgs/PoseStamped '{header: {stamp: {sec: 1476334344, nanosec: 561867202}, frame_id: "map"}, pose: {position: {x: 0.075144559145, y: -0.0197359323502}, orientation: {z: 0.999897136204, w: 0.0143428383376}}}'
rosbag再生
ros2 bag play -s rosbag_v2 short_test.bag
結果
実行すると、以下のようになります。
(ちょっと微妙な気が)・・・まあまあですね！
今回はとりあえずここまでとします。