rosbag2 の解析

これは[ROS2 Advent Calendar 2022](https://qiita.com/advent-calendar/2022/ros2) の9日目の記事です。遅刻した上に内容とっ散らかっていますがご容赦ください。。。

諸々作業したリポジトリ : https://github.com/nonanonno/rosbag-trial

# はじめに

ROS2においてトピックのデータをダンプしたり、それを再生するのに[rosbag2](https://github.com/ros2/rosbag2)の機能を使うと思います。コマンドとしては以下のような感じです。

```bash
# Record
$ ros2 bag record -o data.bag /chatter
# Replay
$ ros2 bag play data.bag
```

しかしながら、このrosbagファイルの中身はどうなっているのか？ `ros2 bag play` + `ros2 topic echo`以外の方法で中身を見ることができるのか？ `ros2 bag record`以外の方法で rosbagファイルを作ることができるのか？ という部分が気になります。そこで、本記事では rosbagファイルの中身の解析をD言語[^1]でやろうと思います。また、おまけとしてROS2ではどのようにrosbagファイルの中身の確認ができるかを簡単に調べてみました。

# D言語でrosbagファイル解析

作ったもの : [cdr-reader](https://github.com/nonanonno/rosbag-trial/tree/main/cdr-reader)


rosbagファイルの解析をするとしたら通常は後述の`rosbag2_py`を使えば良いと思いますが、例えば、大量のデータがあってそれをPythonでやるのは性能的に厳しかったり、そもそも動的型付け言語に苦手意識があったりすると別の言語でやりたくなると思います。とは言えC++でこの手の解析をやるのは色々大変です。というわけで、コンパイラ言語で高速なコードを書けてかつ強力なメタプログラミングができるD言語でrosbagを解析してみようと思います。



## rosbagの形式

まず、rosbagファイルがどういう形式になっているかを確認します。rosbagファイルを見てみると以下のようなフォルダ構成になっていると思います。

```shell
$ ls data.bag
data.bag_0.db3  metadata.yaml
```

このうち`metadata.yaml`はトピック一覧などが書かれた補助情報です。rosbag本体は`data.bag_0.db3`になりますが、この本体がどういった形式になっているかは`metadata.yaml`に書かれているのでまずはそちらを参照することになります。とは言え、今はほぼほぼ`sqlite3`一択でしょう。

ではrosbag本体である`data.bag_0.db3`の中身を見てみます。先程書いた通り、これは`sqlite3`のデータベースファイルになっています。なので`sqlite3`コマンドで中身を見ることができます。

```shell
$ sqlite3 data.bag/data.bag_0.db3
SQLite version 3.37.2 2022-01-06 13:25:41
Enter ".help" for usage hints.
sqlite> .tables
messages  topics
sqlite> .schema
CREATE TABLE topics(id INTEGER PRIMARY KEY,name TEXT NOT NULL,type TEXT NOT NULL,serialization_format TEXT NOT NULL,offered_qos_profiles TEXT NOT NULL);
CREATE TABLE messages(id INTEGER PRIMARY KEY,topic_id INTEGER NOT NULL,timestamp INTEGER NOT NULL, data BLOB NOT NULL);
CREATE INDEX timestamp_idx ON messages (timestamp ASC);
```

`topics`と`messages`の2つのテーブルがあります。どういった内容になっているか見てみましょう。

```shell
sqlite> SELECT * FROM topics;
1|/number|std_msgs/msg/Int32|cdr|
2|/chatter|std_msgs/msg/String|cdr|
3|/point|geometry_msgs/msg/Point|cdr|
4|/point_stamped|geometry_msgs/msg/PointStamped|cdr|
5|/image|sensor_msgs/msg/Image|cdr|
6|/channel|sensor_msgs/msg/ChannelFloat32|cdr|
```

`topics`にはトピックの一覧が入っています。

```shell
sqlite> SELECT * FROM messages LIMIT 10;
1|1|0|
2|2|1000000|
3|3|2000000|
4|4|3000000|
5|5|4000000|
6|6|5000000|
7|3|100000000|
8|4|101000000|
9|5|102000000|
10|3|200000000|
```

`messages`には全てのトピックの全てのメッセージが格納されています。(BLOBはバイナリなので表示されていない)

これは簡単ではありますがリレーショナルデータベースの形になっています。`messages`の`topic_id`は`topics`の`id`に対応しており、例えば7番目のレコードは`/point`トピックです。

ここで注意しなければならないのは、BLOBとして格納されているバイナリデータがCDR(Common Data Representation)になっているということ、このバイナリデータをどのようにデシリアライズすれば良いのかはデータベースにも`metadata.yaml`にも含まれていないということです。型名はわかるので、ROS2の環境ではTypesupportを使用します([一昨年解説してた記事](https://qiita.com/nonanonno/items/332a585fe8c9930692bd#typesupport))。ROS2ではない環境であったり、ROS2がサポートしていない言語ではTypesupportは使えないので頑張って自前でデシリアライズを定義する必要があります。

では、CDRの仕様などを見つつやっていきましょう。

## CDRの仕様

仕様書としては以下のものを参照します。

- https://www.omg.org/spec/DDSI-RTPS/2.3/PDF (10章)
- https://www.omg.org/cgi-bin/doc?formal/02-06-51 (型の扱いはこちら)

ざっくりと要約すると、以下のような形式になっています。

- ヘッダー + 本文という形式
- ヘッダーにはエンディアンなどの情報が入っている。(エンディアンが自分の環境と合ってなかったら読み込み側が入れ替えて解釈する)
- 本文には値がバイナリでそのまま入っている。(型が何か、などの情報は一切ない)
- アライメントを気をつける必要がある。

ドキュメントから引用すると以下のような説明があります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/73600/555aa93f-80c5-e2e8-e96d-da8b8abe0317.png)


最初の4byteがヘッダーです。例えば、{0x00, 0x01, 0x00, 0x00} となっていたらリトルエンディアンです。

ヘッダーの次が本文です。ROS2の文脈では10.7節を見れば良いと思います。例では以下のようなIDLをシリアライズしています。

```idl
@final
struct ShapeType {
    @key string<64> color;
    long x;
    long y;
    long size;
};
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/73600/d4bd548d-5ad7-3a0f-07f5-ff563a2653db.png)


文字列の長さは、`\0`終端含めて5です。そのため、4byte整数で文字列の長さが格納され、次の5byteで文字が入ります。次に数字を入れていくのですが、ここでアライメントが出てきます。アライメントはコンピュータが読みやすいメモリレイアウトにするためのもので、4byteのデータであれば4の倍数番地から始まり、8byteであれば8の倍数番地から始まる必要があります。今回の例では4byteなので、4の倍数の16から始まるようにパディングが入れられます。

ルールとしてはわりとそれだけで、アライメントのルールであったり各型がどのように表現されるのかの確認が必要だったりといったところになります。(可変長配列であれば、まず長さがありその後にデータが並ぶなど)

## rosbag読み込み(データベース部分)

sqlite3を扱う`d2sqlite3`というパッケージを使って読み込みます(main文は省略)。

```d
import d2sqlite3;

auto db = Database("data.bag/data.bag_0.db3");

for(Row r; db.execute("SELECT * FROM topics")){
    writeln(r);
}
// ↓のように表示される。
// [1, /number, std_msgs/msg/Int32, cdr, null]
// [2, /chatter, std_msgs/msg/String, cdr, null]
// [3, /point, geometry_msgs/msg/Point, cdr, null]
// [4, /point_stamped, geometry_msgs/msg/PointStamped, cdr, null]
// [5, /image, sensor_msgs/msg/Image, cdr, null]
// [6, /channel, sensor_msgs/msg/ChannelFloat32, cdr, null]
```

`Row`が各レコードを表現します。これをそのまま使うのはちょっと面倒なので構造体にマップします。メタプログラミングの時間です。

```d
// topics テーブル
struct Topic {
    long id;
    string name;
    string type;
    string serialization_format;
    string offered_qos_profiles;
}

T mapOR(T)(Row r) {
    T v;
    static foreach (i, m; __traits(allMembers, T)) {
        __traits(getMember, v, m) = r[i].as!(typeof(__traits(getMember, v, m)));
    }
    return v;
}
```

`mapOR`は`T`をテンプレート引数として取るテンプレート関数で、`Row`を任意の構造体にマップします。`static foreach`や`__traits`など、D言語ユーザー以外には見慣れなさそうな表現がありますが、これらはコンパイル時に処理される部分です。`__traits(allMembers, T)` で構造体Tの全てのメンバー変数を取得できます。つまり、これを使って

```d
for(Row r; db.execute("SELECT * FROM topics")){
    auto t = mapOR!Topic(r);
    ...
}
```

と記述すると、mapORはコンパイル時に以下のようなコードに展開されます。(`!`はテンプレート引数を渡す表現。C++の`mapOR<Topic>(r)`だと考えて良い)

```d
Topic mapOR(Row r) {
    Topic v;
    v.id = r[0].as!(typeof(v.id));
    v.name = r[1].as!(typeof(v.name));
    v.serialization_format = r[2].as!(typeof(v.serialization_format));
    v.offered_qos_profiles = r[3].as!(typeof(v.offered_qos_profiles));
}
```

もちろん`Topic`ではない構造体にも適用できます。`messages`を扱いたい時は以下のようにします。

```d
// messages テーブル
struct Message {
    long id;
    long topic_id;
    long timestamp;
    void[] data;
}

foreach(r; db.execute("SELECT * FROM messages")) {
    auto m = mapOR!Message(r);
    ...
}
```

これでデータベースの処理ができました。次に各メッセージを処理していきます。

## rosbag読み込み(デシリアライズ)

流石に何も補助情報がない状況ではCDRのデシリアライズはできません。そのため、ROS2のインターフェースに対応する型をD言語でも表現しておいてそれを使います。`geometry_msgs/msg/PointStamped`を例にします。

まず構造体定義です。機械的にやっていきます。

```d
module geometry_msgs.msg;

import std_msgs.msg;

struct Point {
    double x;
    double y;
    double z;
}

struct PointStamped {
    Header header;
    Point point;
}
```

```d
module std_msgs.msg;

import builtin_interfaces.msg;

struct Header {
    Time stamp;
    string frame_id;
}
```

```d
module builtin_interfaces.msg;

struct Time {
    int sec;
    uint nanosec;
}
```

次にデシリアライズ関数です。CDRのところでさっくり仕様を説明しましたが、この仕様に基づいてデシリアライズを行うには以下のような手順が必要です。

1. ヘッダーを読む(今回はエンディアン変化はないので単に無視する)
2. 型に応じてアライメントを調整する
3. 型に応じたバイト数を読み取る
4. 型に応じたバイト数、インデックスを進める
5. 2に戻る。

これをコードにします。

```d
private class CDRBuffer {
    int index;
    void[] data;

    this(void[] data) {
        this.data = data[4 .. $];
        index = 0;
    }

    void consume(T)(out T v) {
        assert(index < data.length);
        static if (is(T == string)) {
            int len;
            consume(len);
            v = (cast(char[]) data[index .. index + len]).to!string;
            index += len;
        }
        else {
            index = ceiling(index, T.sizeof);
            v = *(cast(T*) data[index .. index + T.sizeof]);
            index += T.sizeof;
        }
    }
}

private int ceiling(int x, int n) {
    return ((x + n - 1) / n) * n;
}
```

`this(...)`はコンストラクタで、`this.data = data[4 .. $]`でヘッダー文の4byteを読み飛ばしています。 `[4 .. $]`はスライス構文で、Pythonの `[4:]`に相当するものです。(`$`はこの文脈では配列の長さになる)

`consume`は手順の2から4に対応します。ここでもメタプログラミングをやっています。`static if` の文はテンプレートの型の確認した上で、コンパイル時にコード切り替えを行います。C++のSFINAEで達成することをやっているイメージです。

- `T`が`string` → 文字列の長さを取るために `consume`を再帰、取得した長さ文文字列を読み取る、indexを進める
- `T`が`string`ではない(`int`などのプリミティブを想定) → 型に応じてアライメントを取る、型に応じたバイト数分読み取る、indexを進める

本当は動的配列の場合(`static if (isDynamicArray!(T))`が使える)など他のケースの考慮も必要ですが、とりあえずはこれで進めます。

次に必要なのは、トピックの型のメンバー変数を全て取得してそれを`consume`に入れるという操作です。ここでも前述の`static foreach`を使ってメタプログラミングします。

```d
private void deserialize(T)(CDRBuffer buffer, out T v) {
    static foreach (m; __traits(allMembers, T)) {
        static if (isAggregateType!(typeof(__traits(getMember, v, m)))) {
            deserialize(buffer, __traits(getMember, v, m));
        }
        else {
            buffer.consume(__traits(getMember, v, m));
        }
    }
}
```

`__traits(allMembers, T)`は前に説明した通りです。分岐のところが特徴的で、`isAggregateType!(typeof(__traits(getMember, v, m)))`によりそのメンバー変数の型が構造体かどうかを判定しています。`consume`に入れられるのは文字列かプリミティブ型だけなので、メンバー変数の型構造体であれば`deserialize`自体を再帰します。

例えば、

```d
PointStamped msg;
deserializa(buffer, msg);
```

と呼び出した場合、これはコンパイル時に以下のようになります。

```d
void deserialize(CDRBuffer buffer, out PointStamped v) {
    deserialize(buffer, v.header);
    deserialize(buffer, v.point);
}

void deserialize(CDRBuffer buffer, out Header v) {
    deserialize(buffer, v.stamp);
    buffer.consume(v.frame_id);
}

void deserialize(CDRBuffer buffer, out Time v) {
    buffer.consume(v.sec);
    buffer.consume(v.nanosec);
}

void deserialize(CDRBuffer buffer, out Point v) {
    buffer.consume(v.x);
    buffer.consume(v.y);
    buffer.consume(v.z);
}
```

こういうコードを手で1つずつ作るのは大変です。このあたりをメタプログラミングで処理できるのはD言語の良いところですね。

若干のヘルパー関数を加えつつ([rosbag2.d](https://github.com/nonanonno/rosbag-trial/blob/main/cdr-reader/source/rosbag2.d)を見てください)、それを利用して最終的に以下のように解析できるようになりました。型が増えても、型定義だけ構造体にすれば解析できます。デシリアライズ関数を1つずつ作る必要はありません。気軽に何かできそうですね。

```d
import std.algorithm;
import std.stdio;
import std.format;

import d2sqlite3;
import tabletool;

import rosbag2;

import geometry_msgs.msg;

void main(string[] args) {
    const file = args[1];

    auto db = Database(file);
    Topic[long] topics;
    long[string] topics_ids;

    foreach (Row r; db.execute("SELECT * FROM topics")) {
        auto t = mapOR!Topic(r);
        topics[t.id] = t;
        topics_ids[t.name] = t.id;
    }
    writeln(tabulate(topics.values, Config(Justify.Left)));

    writeln("=============");

    PointStamped[] msgs;
    foreach (r; db.execute(
            "SELECT * FROM messages WHERE topic_id = %s LIMIT 10".format(
            topics_ids["/point_stamped"]))) {
        auto message = r.mapOR!Message;

        PointStamped data;
        deserialize(message, data);
        msgs ~= data;
    }
    writeln("Displaying /point_stamped");

    msgs.each!writeln;
}
```

```shell
$ dub -q -- data.bag/data.bag_0.db3 
id name           type                           serialization_format offered_qos_profiles
-- -------------- ------------------------------ -------------------- --------------------
6  /channel       sensor_msgs/msg/ChannelFloat32 cdr                                      
5  /image         sensor_msgs/msg/Image          cdr                                      
4  /point_stamped geometry_msgs/msg/PointStamped cdr                                      
3  /point         geometry_msgs/msg/Point        cdr                                      
2  /chatter       std_msgs/msg/String            cdr                                      
1  /number        std_msgs/msg/Int32             cdr                                      
=============
Displaying /point_stamped
PointStamped(Header(Time(0, 0), "test\0"), Point(0, 0, 1))
PointStamped(Header(Time(0, 100000000), "test\0"), Point(0.5, 0.479426, 0.877583))
PointStamped(Header(Time(0, 200000000), "test\0"), Point(1, 0.841471, 0.540302))
PointStamped(Header(Time(0, 300000000), "test\0"), Point(1.5, 0.997495, 0.0707372))
PointStamped(Header(Time(0, 400000000), "test\0"), Point(2, 0.909297, -0.416147))
PointStamped(Header(Time(0, 500000000), "test\0"), Point(2.5, 0.598472, -0.801144))
PointStamped(Header(Time(0, 600000000), "test\0"), Point(3, 0.14112, -0.989992))
PointStamped(Header(Time(0, 700000000), "test\0"), Point(3.5, -0.350783, -0.936457))
PointStamped(Header(Time(0, 800000000), "test\0"), Point(4, -0.756802, -0.653644))
PointStamped(Header(Time(0, 900000000), "test\0"), Point(4.5, -0.97753, -0.210796))
```




# ROS2でのrosbagファイル解析(おまけ)

ざっくり調べてみると、ROS2では以下の方法でrosbagファイルの中身を見ることができるようです。

- [rqt_bag](https://github.com/ros-visualization/rqt_bag/tree/humble) を使用(humble以降)
- `rviz2`を併用
- [rosbag2_py](https://github.com/ros2/rosbag2/tree/rolling/rosbag2_py) を使用 (galactic以降)
- [rosbag2_cpp](https://github.com/ros2/rosbag2/tree/rolling/rosbag2_cpp) を使用
- [foxglove](https://foxglove.dev/) を使用

他にもあるかもしれませんが私が見かけたことがあるのはこのあたりです。それぞれ見ていきます。

## rqt_bag

[rqt_bag](https://github.com/ros-visualization/rqt_bag/tree/humble) は ROS1時代からあるrosbag可視化ツールです。ROS2ではhumbleから入りました。rosbagファイルを読み込ませてみると以下のように可視化できます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/73600/9e97527e-6548-6608-d507-96629448fbf2.png)

一番左がタイムラインで、数値のプロットと生データとしての表示、画像の表示ができます。本記事の目的からは逸れますが`ros2 bag record` や `ros2 bag play`といったことをこのツールから行えるようです。

使う時は以下のようなコマンドを実行します。

```shell
rqt_bag data.bag
```

あるいは、`rqt_bag` を起動してメニューから読み込ませます。とりあえずrosbagの中身を確認したい、という用途には十分そうです。ただし、可視化の種類が少ない(画像のもので全て。増やすにはプラグイン作成が必要)のと、画面のレイアウトの保存ができず、rosbagを読み込む度にプロットパネルなどを新たに開く必要があるなど、個人的には機能不足を感じました。まあグラフにプロットできればけっこう事足りるのかもしれませんね。

## rviz2

rviz2はROS2の可視化ツールで、使われている方も多いと思います。リアルタイムでの可視化に使われることが多いと思いますが、`ros2 bag play` でrosbagファイルを再生できるのでrosbagデータをrviz2で可視化することもできます。以下の動画は画像と`geometry_msgs/msg/PointStamped`を可視化したものです。

![output-palette.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/73600/f5aefbac-a750-5050-7bcc-42eade53e4b2.gif)

3D画面での可視化に良いでしょう。TFも解釈してくれます。humble以降の`ros2 bag play` ではスペースキーによる再生の一時停止などもできるようになったので以前より見やすくなったと思います。あるいはrqt_bagと組み合わせても良いと思います。

## foxglove

[foxglove](https://foxglove.dev/)が開発しているオープンソースの可視化ツールのFoxglove Studioを使うことでrosbagファイルの可視化ができます。ブラウザアプリでインストールなしで使えます(デスクトップアプリもある)。軽く試してみた図が以下になります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/73600/9502b55d-5509-6315-8a59-15d0b5ad6988.png)

ROS2標準のメッセージは問題なく可視化できるようです。カスタムメッセージについては、[MCAPに変換するのを試してほしい](http://docs.ros.org/en/humble/How-To-Guides/Visualizing-ROS-2-Data-With-Foxglove-Studio.html)と書かれていました。10分ぐらいしか触っていないですが、便利に使えそうですね。

## rosbag2_py と rosbag2_cpp

[rosbag2_py](https://github.com/ros2/rosbag2/tree/rolling/rosbag2_py)はrosbagファイルを扱うためのPythonライブラリです。Pythonスクリプトでrosbagファイルを作成したり読み込んだりできます。galactic以降で利用可能で、C++ライブラリである`rosbag2_cppの`バインディングになっています。そのため、APIドキュメントは[rosbag2_cpp](https://github.com/ros2/rosbag2/tree/rolling/rosbag2_cpp)の方を参照してください。

rosbagファイルをスクリプトで読み書きすることはあまりないかもしれませんが、これによりいくつかできそうなことがあります。作成の観点では例えば以下のようなことが思いつきます。

- 既存のrosbagファイルから必要なトピックだけを抜き出したrosbagファイルを作成する
- 複数のrosbagフィアルをマージする
- CSVなどのデータからrosbagファイルを作成する
    - 特に長時間に渡るデータとなると、CSVを読み込んでパブシッシュするノードを作ってそれを`ros2 bag record`するというやり方が厳しくなるでしょう

読み込みの観点では、例えば以下のようなことが思いつきます。

- rosbagファイルの統計情報をPythonで集計する
- rosbagファイルの信号をmatplotlibでグラフにする
- CSVなど、rosbagではない別の形式に変換する

さらに、これらのことはコマンドラインで実行できるため自動化の助けになるでしょう。そこで、本節ではrosbag2_pyの使い方を簡単に説明します。なお、その他の使い方は[テストコード](https://github.com/ros2/rosbag2/tree/rolling/rosbag2_py/test)が参考になると思います。

### rosbagの作成

作成には `rosbag2_py.SequentialWriter` を使用します。以下は `std_msgs/msg/String`型の`/chatter`トピックを含むrosbagファイルを作成する例です。

```python
import rosbag2_py
from rclpy.serialization import serialize_message

from std_msgs.msg import String


writer = rosbag2_py.SequentialWriter()
writer.open(
    rosbag2_py.StorageOptions(
        uri="data.bag",
        storage_id="sqlite3",
    ),
    rosbag2_py.ConverterOptions(
        input_serialization_format="cdr",
        output_serialization_format="cdr",
    ),
)

topic_name = "/chatter"

writer.create_topic(
    rosbag2_py.TopicMetadata(
        name=topic_name,
        type="std_msgs/msg/String",
        serialization_format="cdr",
    ),
)

for i in range(10):
    msg = String(data=f"Hello {i}")
    writer.write(topic_name, serialize_message(msg), i * 1000 * 1000 * 1000)
```

やっていることは以下の通りです。

1. `SequentialWriter`インスタンスを作成する
2. 形式(`sqlite3` と `cdr` で良い)を指定してrosbagファイルを開く
3. `create_topic`でトピックを登録する
4. メッセージを作成し、シリアライズしてから`write`で書き込む

`StorageOptions`は見た目通りとして、`ConverterOptions`はちょっと疑問に思うかもしれません。私もちゃんと理解はしていませんが、`cdr`以外の形式のrosbagファイルを`cdr`として読み込みたい、などといった時に使うのではないかと想像しています。また、その他注意点として`TopicMetadata`に渡す`type`が`std_msgs.msg.String`というクラスではなく文字列であること、`write`関数にはシリアライズしたデータを渡すことなどがあります。これらは前述のデータベース内での表現の仕方を思い出していただければ理解できると思います。

この例では1つのトピックだけでしたが、複数のトピックの場合でも同様に`create_topic`で登録して`write`で書くといった形になります。以下のように、時間通りの順番で書き込まなくても大丈夫なようです。インデックス貼られていたしたぶん`ros2 bag play`の時は`SELECT * FROM messages ORDER BY timestamp` している。

```python
for i in range(10):
    msg = String(data=f"Hello {i}")
    writer.write("/chatter", serialize_message(msg), i * 1000 * 1000 * 1000)

for i in range(10):
    msg = Int32(data=i)
    writer.write("/number", serialize_message(msg), int((i + 0.5) * 1000 * 1000 * 1000))
```

### rosbagの読み込み

読み込みには`rosbag2_py.SequentialReader`を使用します。Jupyter notebook で可視化することを試してみます。以下はJuptyterの各セルだと解釈してください。

とりあえずimportです。loggingの設定を行っていますが、これはrosbagを操作する時に多少ログ出力があって邪魔だったためです。

```python
import rosbag2_py
from rclpy.serialization import deserialize_message
import importlib
import matplotlib.pyplot as plt
import rclpy.logging

# To suppress logging output
rclpy.logging.set_logger_level("rosbag2_storage", rclpy.logging.LoggingSeverity.FATAL)
```

次にヘルパー関数を定義します。`load_bag`を定義していますが、やっていることは前節とほぼ同じで、`SequentialWriter`ではなく`SequentailReader`を使っているだけです。`get_type_from_str`はもう少し特殊です。rosbagからは`std_msgs/msg/String`といった文字列による型名しか得られないので、その情報を使って`importlib`により動的にimportする関数です。このあたりを動的に処理できるのはPythonならではですね。

```python
def get_rosbag_options(path: str):
    storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3")
    converter_options = rosbag2_py.ConverterOptions(
        input_serialization_format="cdr", output_serialization_format="cdr"
    )
    return storage_options, converter_options


def load_bag(path: str):
    reader = rosbag2_py.SequentialReader()
    reader.open(*get_rosbag_options(path))
    return reader


def get_type_from_str(type_str: str):
    # dynamic load message package
    pkg = importlib.import_module(".".join(type_str.split("/")[:-1]))
    return eval(f"pkg.{type_str.split('/')[-1]}")
```

次はトピック一覧を取得します。`get_all_topics_and_types`でrosbagに保存されている全てのトピック情報を得られるのでそれを使用します。ここではトピック名の文字列のキーにトピックの型のクラスが得られるようなテーブルを作ります。

```python
type_table = {}
for meta in bag.get_all_topics_and_types():
    type_table[meta.name] = get_type_from_str(meta.type)
```

見てみましょう。6個のトピックが入っています。

```python
for k,v in type_table.items():
    print(k,"\t",v)
```

```
/channel 	 <class 'sensor_msgs.msg._channel_float32.ChannelFloat32'>
/image 	 <class 'sensor_msgs.msg._image.Image'>
/point 	 <class 'geometry_msgs.msg._point.Point'>
/chatter 	 <class 'std_msgs.msg._string.String'>
/point_stamped 	 <class 'geometry_msgs.msg._point_stamped.PointStamped'>
/number 	 <class 'std_msgs.msg._int32.Int32'>
```

rosbagからメッセージを1つずつ取得するには`read_next`を使用します。デシリアライズのことは一旦置いておいて中身を見てみます。

```python
for _ in range(10):
    message = bag.read_next()
    print(
        message[0],  # topic name
        # message[1], # topic data (cdr)
        message[2],  # timestamp
    )
```

```
/number 0
/chatter 1000000
/point 2000000
/image 3000000
/point 100000000
/image 101000000
/point 200000000
/image 201000000
/point 300000000
/image 301000000
```

`read_next`で得られるメッセージはトピック名、トピックデータ(バイナリ)、タイムスタンプのTupleになっています。(画像のバイナリが大変なことになるのでトピックデータは省略)

では、このうちの`/point`トピックの可視化をしてみます。先程使用した`read_next`は副作用のある関数なので`seek`関数で先頭に戻します。そして、`/point`だけを取得できるようなフィルターを設定し、前に作った`type_table`を使って型情報を取得し、デシリアライズします。`deserialize_message`は第二引数に入れた型に基づいてデシリアライズする関数です。

```python
bag.seek(0)
bag.set_filter(rosbag2_py.StorageFilter(topics=["/point"]))

pts = []

while bag.has_next():
    message = bag.read_next()
    pts.append(deserialize_message(message[1], type_table[message[0]]))

plt.plot([p.x for p in pts], [p.y for p in pts])
```

これで以下のようなグラフが得られます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/73600/c130e207-c10b-53c8-7358-6b07e392f316.png)


他のトピックについても同様です。例えば画像の場合は以下のようになります。OpenCVだとエンコーディングがBGRなのでRGBに変換するといった追加処理は必要ですが。

```python
import cv2
import cv_bridge

bridge = cv_bridge.CvBridge()

bag.seek(0)
bag.set_filter(rosbag2_py.StorageFilter(topics=["/image"])
)

message = bag.read_next()

image_ros = deserialize_message(message[1], type_table[message[0]])

image = bridge.imgmsg_to_cv2(image_ros)

plt.imshow(cv2.cvtColor(image, cv2.COLOR_BGR2RGB))
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/73600/7f5720dc-4e4b-6d26-5fc8-ba685addf062.png)


ここでは rosbag2_py で説明しましたが、おそらくrosbag2_cpp でも同様な読み書きができるでしょう。動的型付けでないことによる制約などはあると思いますが。


[^1]: [Mercedes-BenzではROS1でrosbagファイル解析をD言語でやっていた](http://dconf.org/2019/talks/cehreli.html)のでROS2でやっても良い。