rosbagをC++で扱う

こんにちはKN_Appleです。

rosbagをC++で扱うときにはまったのでその時のメモです。
以下実行環境です。

• Ubuntu18.04
• ROS Melodic

きっかけ

rosbagを扱うコードを検索すると出てくるのはだいたいPythonのコードでした。
確かにPythonは便利ですよね、catkin_makeなしで書いては実行ができますし。

しかし、公式ページによると、C++のAPIも用意されているもよう。

「なるほど、やってみよう」と思い、コピペで動かしてみたけど動かず。
今回はそれを解決するところまでを残すため、記事にしました。

動かなかった原因

以下、動かなかった公式ページのコードです。

bag_write.cpp

#include <rosbag/bag.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>

rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Write);

std_msgs::String str;
str.data = std::string("foo");

std_msgs::Int32 i;
i.data = 42;

bag.write("chatter", ros::Time::now(), str);
bag.write("numbers", ros::Time::now(), i);

bag.close();

bag_read.cpp

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Read);

std::vector<std::string> topics;
topics.push_back(std::string("chatter"));
topics.push_back(std::string("numbers"));

rosbag::View view(bag, rosbag::TopicQuery(topics));

foreach(rosbag::MessageInstance const m, view){
    std_msgs::String::ConstPtr s = m.instantiate<std_msgs::String>();
    if (s != NULL)
        std::cout << s->data << std::endl;

    std_msgs::Int32::ConstPtr i = m.instantiate<std_msgs::Int32>();
    if (i != NULL)
        std::cout << i->data << std::endl;
}

bag.close();

よく見ましょう。C++です。

...main関数ないやんけ！！！！
pythonに慣れすぎて気づかず１週間溶かしました。　（泣）

さらにもう一つ、bag_write.cppのコードですが、main関数で囲んでも動きません。
bag.write()の部分ですが、ros::Time::now()を使うには事前にNodeHandleを宣言するか、ros::start()を行う必要があるらしいです。（ここは詳しくないのでどなたか教えていただけるとありがたいです）

まあ、rosbagの書き込みに関してはPublisherかSubscriberのどちらかと一緒に書くのが良いでしょう。
これらを修正することで、無事C++でもrosbagの読み書きをすることができました。

結論

以上のの問題点を修正し、パッケージ化したものを以下のリポジトリに置きました。
使っていただけるとありがたいです。

また、間違いの指摘やPRなどあればお願いします。