CIR-KIT/fifth_robot_pkg

Google Cartographer 導入

Closed this issue · 17 comments

tanacchi commented

takuyani/SLAM-Robot_Codeのビルド方法

先駆者様のリポジトリからいろいろと探ろうと思います
リポジトリそのものがsrc内にあるべきファイル群なので次のような手順を踏みます
1.
mkdir slambot_ws ;cd slambot_ws
2.
git clone git@github.com:takuyani/SLAM-Robot_Code.git src ;cd src
3.
catkin_init_workspace ;cd ../
4.
catkin_make

これでビルドは通ります。

tanacchi commented

offline_cartographer を使ってみた（odom 不使用）

commit ID
ec5e1263cc278ccab4c968f6bf5197ef2b71918f

手順

  1. おもむろに
    roslaunch fifth_robot_description teleop_keyboard.launch

  2. bagfile を録る
    デモ用の bagfile で使ってそうだったのが
    clock とimu と pointcloud2 のトピックしかなかったのでそれを参考に

    rosbag record /points2 /clock /imu

  3. offline_cartographer を起動
    roslaunch fifth_robot_description offline_cartographer.launch bagfile:=<2で録ったbagfile>

結果

なんかできてしまった
3d_without_tf_points2 bag_xray_xy
＊上から見た図

こ れ は ひ ど い
地図の概形っぽいのは見えるんだけどなあ
imuが悪さしてるのかしら

tanacchi commented

offline_cartographer を使ってみた（odom 使用）

commit ID
b37d3c05634abe79ff2c97a90807d306332acc02

手順は上とだいたい同じ
3d_without_tf_with_odom bag_xray_xz
＊横から見た図

? ? ?
odomを受け取るようにしただけでも
だいぶマシにはなったっぽい
やっぱりimuのせいなのか？

tanacchi commented

走り方を工夫してみた

上：直進のみ 下：カーブのみ
3d_only_back bag_xray_yz
3d_only_curve bag_xray_yz

上からだときれいに見えなくもないが、横から見るとなかなかひどい
やはり imu のキャリブレーションかフィルタリングが必要かと
only_back_side

tanacchi commented

シミュレータが重たいので、imu のデータが飛んでるからこうなってるのかと思い
points2 の数を減らしたり、imu の update_rate を上げてみたり、切り返しをゆっくりにしてみたりしたけど変化なし

3d_high_rate_imu bag_xray_xz

切り返しの数だけ跳んでるからimuなんだろうな

tanacchi commented

imu データをフィルタリングしてみた

imu データの内、roll と pitch の回転角速度をフィルタリングした

やっとまともなのができた
3d_without_roll_and_pitch bag_xray_xy]
↑上から見た図
3d_without_roll_and_pitch bag_xray_yz
↑横から見た図
実機のセンサを使ったらまた変わるのかもしれない

根本的な解決のためには urdf をもう少し厳密に書かなきゃいけないのかもしれない

tanacchi commented

上の再現方法

commit ID
94ce3028821610df0b14038d960f16f94da51b8e

  1. シミュレーション環境を立ち上げる
    roslaunch fifth_robot_description teleop_keyboard.launch enable_rviz:=false

  2. bagfile を録る
    roslaunch fifth_robot_description bag_recorder.launch

  3. シミュレーション環境で適当に走らせる

  4. bag を録り終えたら、imu のフィルタリングと odom のremap をする
    (もっと賢いやり方にするつもり)
    roslaunch fifth_robot_description bag_player.launch bagfile:=<bagfile_name.bag>
    rosbag record --all -O filtered_bagfile.bag

  5. offline_cartographer_node でマッピング
    roslaunch fifth_robot_description offline_cartographer.launch bagfile:=filtered_bagfile.bag

  6. filtered_bagfile.bag と同じディレクトリにそれらしきものができているかと思います。
    plyファイルは３次元点群を表現するためのものです

  7. assets_writer を走らせる。点群がなめらかになっているのはわかるけど、
    実際何やってるかは微妙にわかんない。本来 *.pbstream ファイルができているはずなんだけど
    何らかの不具合で *.pb ファイルしかできていない
    Extracting trajectory/poses #332
    roslaunch fifth_robot_description assets_writer.launch bagfile:=/home/tanacchi/filtered_bagfile.bag pbfile:=/home/tanacchi/filtered_bagfile.bag.pb
    ↑複数行に渡ってるように見えるけどトリプルクリックで全選択できます

filtered_imu bag_xray_xy
まだキレイとは言えんな。。

tanacchi commented

この時点で、地図の出来がパラメータ次第で全く異なることが分かった

imu のフィルタリングは依然として必要

tanacchi commented

で、実際にnavigation させる時にどうやって使うの？っていうところが今後の課題

tanacchi commented

先駆者様たちの launch ファイルから見様見真似で動かしてみます
自分が次に動けるのは 9/7(金) 以降になります

YuePanEdward commented

Thanks a lot for your summary.

  • 👍1
tanacchi commented

Cartographer を使う際は、きちんと公式にある手順を踏んでインストールしてください。

apt でも入ってしまってしかも微妙に動いちゃうから気づくのが遅れてしまった

tanacchi commented

可能性として出てきたのが、
Cartographer は既知の地図を元に navigation するのに適していないということ

cartographer_node 等で pbstream ファイル（３次元地図および自己位置データ）を
生成することに成功したものの
それを読み込んで 自己位置推定 をするという機能が見当たらない

cartographer によって、従来でいう map とwaypoint の生成は可能かもしれないが
実際に navigation する際には amcl と move_base を使うことになりそう

amcl がLaserScan （２D情報）を取得するため
結局は PointCloud2(3D) から LaserScan(2D) への変換を噛ませることになるかもしれない

幸い、変換の際に利用する pointcloud_to_laserscan_node が、
”３次元で見た時の輪郭を２次元に落としこむ”ような処理をしてくれているため
3DLiDAR の恩恵を全く受けられないというわけではなさそうだ。
（白：LaserScan その他：PointCloud2）
pointcloud_to_laserscan

今後のアプローチとしては、

  1. Cartographer で「マップ情報とセンサ情報から自己位置推定をする」機能を探してみる
    • -load_state_filename というオプションで pbstream ファイルを読み込める
  2. Cartographer に加え、新たに 3d_navigationというパッケージを利用してみる
  3. cartographer_pbstream_to_ros_map で GridMap を生成し、その後は従来通りの手法をとる
    • GridMap の生成には成功している

の３パターンが考えられる

tanacchi commented

確実性を重視して、3 → 2 → 1 の順番で手を付けたいと思う

いずれの手段となっても、3D LiDAR の意味は十分にある

tanacchi commented

screenshot from 2018-09-11 14-47-15

PointCloud2 -> LaserScan の変換をしたうえで gmapping で地図を作りました。
こっちのほうがキレイじゃねえか

今までの、経路上にできる謎空間はなくなった。

調べてみたところ、pcl（point cloud library）で位置補正し、
move_baseと連携して navigation するみたいなパッケージがあるので
それも調べてみます

tanacchi commented

今回の Google Cartographer の研究の集大成です。
for_cartographer bag_xray_xy_all
汚そうで実はかなりキレイなんですコレ
マップの輪郭はそう見えますが。

つくチャに向けての開発にあたって、導入は難しいということで
一旦 Google Cartographer の方はこれにて切り上げることにします。

もしここから waypoint 的なものが取り出せることがわかれば、
再始動します

gaoethan commented

It should be perfect on condition that it's written in English, or the others have to employ google translation :)

  • 👍1