やりたいこと
ROS2で記録されたrosbagファイルを読んで中身のデータをCSV等に変換したい.
いちいちplayしてechoするのは色々な意味でめんどくさいので避けたい.
できればROS2の環境に依存するのも避けたい.
手法
rosbagsというpythonライブラリがあるのでこれを利用する.
https://gitlab.com/ternaris/rosbags
導入は普通にpipでOK
pip install rosbags
基本的な使い方は
https://ternaris.gitlab.io/rosbags/topics/rosbag2.html
を参照
このページのサンプルコードの場合,タイムスタンプがtimestamp,データがmsgに入っている.
msgはインスタンス的な構造になっていて,例えば画像(sensor_msgs/msg/Image)の場合こんな感じ
sensor_msgs__msg__Image(header=std_msgs__msg__Header(stamp=builtin_interfaces__msg__Time(sec=1505454387, nanosec=719656343, __msgtype__='builtin_interfaces/msg/Time'), frame_id='front', __msgtype__='std_msgs/msg/Header'), height=540, width=720, encoding='bayer_rggb8', is_bigendian=0, step=720, data=array([255, 255, 255, ..., 31, 26, 31], dtype=uint8), __msgtype__='sensor_msgs/msg/Image')
ここで例えばmsg.heightみたいに書けば720という値が取得できる.
vars(msg)とすることで辞書型に変換できて,
{'header': std_msgs__msg__Header(stamp=builtin_interfaces__msg__Time(sec=1505454387, nanosec=719656343, __msgtype__='builtin_interfaces/msg/Time'), frame_id='front', __msgtype__='std_msgs/msg/Header'),
'height': 540,
'width': 720,
'encoding': 'bayer_rggb8',
'is_bigendian': 0,
'step': 720,
'data': array([255, 255, 255, ..., 31, 26, 31], dtype=uint8),
'__msgtype__': 'sensor_msgs/msg/Image'}
list(vars(msg))とすれば変数名一覧が取得できる.
['header',
'height',
'width',
'encoding',
'is_bigendian',
'step',
'data',
'__msgtype__']
なので(たぶんもっときれいな書き方があるんだろうが)とりあえずこんな感じで書くとデータの中身をざっくり取り出せる.
for i in range(len(vars(msg))):
print(vars(msg)[list(vars(msg))[i]])
構造がネストしている場合はもう少し頑張る必要がある.
独自定義のトピックを読む場合
メッセージ定義ファイル(.msg)を用意する
/hoge/msg以下にまとめて配置してあるとすると,
from rosbags.rosbag2 import Reader
from rosbags.typesys import get_types_from_msg, register_types
from pathlib import Path
from glob import glob
msgl=glob("/hoge/msg/**/*.msg",recursive=True)
add_types = {}
for i in range(len(msgl)):
add_types.update(get_types_from_msg(Path(msgl[i]).read_text(),メッセージタイプ名 ))
register_types(add_types)
メッセージタイプ名はわかっているならべた書き(文字列)でいい
大量に処理したいなら本来ROSがmsgファイルのコンパイルに使うCmakefileの中のproject()部分を読むなどすれば自動化可能.
Limitation
metadata.yamlが存在しない場合には読めないらしい.
もしかしたら回避策があるのかもしれないが,とりあえずここだけ別途ROS2環境を用意してreindexして対処.
ros2 bag reindex -s sqlite3 hoge/bag/
ここで-sオプションがなくてもreindex自体はできるが,後々rosbagsが読むときにエラーを吐くので-sオプションは必須.
rosbagsにはROS1のbagをROS2のbagに変換する機能とかもあるらしい…