ROSBAGから、SLAMのTFの結果をcsvにまとめる。

目的

ROSのTFからSLAM(gmapping/cartographerなど)の自己位置推定結果を受信する。
ROSのTFからSLAMの結果を受信するよりも良い方法があったため、ここに記載する。

実装

rostopicコマンドを使う

$ rostopic echo /tf -p -b 2017-08-30-00-00-00.bag > tf_`date "+%Y%m%d_%H%M%S"`.csv

引数
-p matlab/octaveライクなplotting formatで出力する
-b bag fileを指定する。

結果：

tf_20170830_000000.csvのような、テキストファイルが出力される。

time,field.transforms0.header.seq,field.transforms0.header.stamp,field.transforms0.header.frame_id,field.transforms0.child_frame_id,field.transforms0.transform.translation.x,field.transforms0.transform.translation.y,field.transforms0.transform.translation.z,field.transforms0.transform.rotation.x,field.transforms0.transform.rotation.y,field.transforms0.transform.rotation.z,field.transforms0.transform.rotation.w
1503488850592379207,0,1503488850590262955,odom,base_footprint,0.0,-0.0,0.0,0.0,0.0,-0.0,1.0
1503488850615588518,0,1503488850615016206,base_link,base_scan,-0.25,0.0,1.53,-0.0906412823601,0.158088607453,0.489071347702,0.852995525769
1503488850624114299,0,1503488850623585564,odom,base_footprint,0.0,-0.0,0.0,0.0,0.0,-0.0,1.0
1503488850636991246,0,1503488850736799565,base_footprint,base_link,0.0,0.0,0.1,0.0,0.0,0.0,1.0

LibreOffice/Excelなどで
field.transforms0.header.frame_idと
field.transforms0.child_frame_idで
抽出したいtfをフィルタリングし、
field.transforms0.transform.translation.x、
field.transforms0.transform.translation.y
で散布図を描くとSLAMの結果が可視化できる。