rosbag2 の解析

これはROS2 Advent Calendar 2022 の9日目の記事です。遅刻した上に内容とっ散らかっていますがご容赦ください。。。

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

はじめに

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

# 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言語¹でやろうと思います。また、おまけとしてROS2ではどのようにrosbagファイルの中身の確認ができるかを簡単に調べてみました。

D言語でrosbagファイル解析

作ったもの : cdr-reader

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

rosbagの形式

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

$ 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コマンドで中身を見ることができます。

$ 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つのテーブルがあります。どういった内容になっているか見てみましょう。

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にはトピックの一覧が入っています。

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を使用します(一昨年解説してた記事)。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 (型の扱いはこちら)

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

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

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

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

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

@final
struct ShapeType {
    @key string<64> color;
    long x;
    long y;
    long size;
};

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

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

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

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

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

// 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の全てのメンバー変数を取得できます。つまり、これを使って

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

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

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を扱いたい時は以下のようにします。

// 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を例にします。

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

module geometry_msgs.msg;

import std_msgs.msg;

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

struct PointStamped {
    Header header;
    Point point;
}

module std_msgs.msg;

import builtin_interfaces.msg;

struct Header {
    Time stamp;
    string frame_id;
}

module builtin_interfaces.msg;

struct Time {
    int sec;
    uint nanosec;
}

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

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

これをコードにします。

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を使ってメタプログラミングします。

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自体を再帰します。

例えば、

PointStamped msg;
deserializa(buffer, msg);

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

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を見てください)、それを利用して最終的に以下のように解析できるようになりました。型が増えても、型定義だけ構造体にすれば解析できます。デシリアライズ関数を1つずつ作る必要はありません。気軽に何かできそうですね。

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;
}

$ 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 を使用(humble以降)
• rviz2を併用
• rosbag2_py を使用 (galactic以降)
• rosbag2_cpp を使用
• foxglove を使用

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

rqt_bag

rqt_bag は ROS1時代からあるrosbag可視化ツールです。ROS2ではhumbleから入りました。rosbagファイルを読み込ませてみると以下のように可視化できます。

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

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

rqt_bag data.bag

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

rviz2

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

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

foxglove

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

ROS2標準のメッセージは問題なく可視化できるようです。カスタムメッセージについては、MCAPに変換するのを試してほしいと書かれていました。10分ぐらいしか触っていないですが、便利に使えそうですね。

rosbag2_py と rosbag2_cpp

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

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

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

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

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

さらに、これらのことはコマンドラインで実行できるため自動化の助けになるでしょう。そこで、本節ではrosbag2_pyの使い方を簡単に説明します。なお、その他の使い方はテストコードが参考になると思います。

rosbagの作成

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

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 している。

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を操作する時に多少ログ出力があって邪魔だったためです。

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ならではですね。

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に保存されている全てのトピック情報を得られるのでそれを使用します。ここではトピック名の文字列のキーにトピックの型のクラスが得られるようなテーブルを作ります。

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

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

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を使用します。デシリアライズのことは一旦置いておいて中身を見てみます。

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は第二引数に入れた型に基づいてデシリアライズする関数です。

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])

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

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

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))

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

1. Mercedes-BenzではROS1でrosbagファイル解析をD言語でやっていたのでROS2でやっても良い。 ↩