環境

ubuntu18.04LTS
ros2 dashing

ソースコード

my_rosbag2.h

#ifndef MY_ROSBAG2_H
#define MY_ROSBAG2_H

// ros
#include <rclcpp/rclcpp.hpp>
#include <rosbag2_transport/rosbag2_transport.hpp>
#include <std_msgs/msg/string.hpp>

namespace my_rosbag2
{

class MyRosbag2 : public rclcpp::Node
{
public:
    explicit MyRosbag2(const std::string &node_name, const rclcpp::NodeOptions &options);
    virtual ~MyRosbag2();

private:
    std::shared_ptr<rosbag2_transport::Rosbag2Transport> trans;
};

}// my_rosbag2

#endif// MY_ROSBAG2_H

自作クラスへ、rclcpp::Nodeクラスを継承します。

また、メンバ変数にrosbag2_transport::Rosbag2Transportのスマートポインタを持たせます。

my_rosbag2.cpp

#include "my_rosbag2/my_rosbag2.h"

using namespace my_rosbag2;

MyRosbag2::MyRosbag2(const std::string &node_name, const rclcpp::NodeOptions &options)
:rclcpp::Node(node_name, options)
,trans(std::make_shared<rosbag2_transport::Rosbag2Transport>())
{
    // rosbag2のための設定
    rosbag2_transport::StorageOptions soptions;
    soptions.storage_id = "sqlite3";
    soptions.uri = "/home/user/ros2/rosbag2";
    rosbag2_transport::RecordOptions roptions;
    roptions.rmw_serialization_format = "cdr";
    roptions.all = true;
    // roptions.topics.push_back("/chatter");

    // 保存開始
    trans->record(soptions, roptions);
}

MyRosbag2::~MyRosbag2()
{
}

コンストラクタで設定を済ませ、そのまま保存を開始します。

rosbag2_transport::RecordOptionsのallをtrueにすると、流れているすべてのトピックを保存します。

個別に保存するには、rosbag2_transport::RecordOptions::topicsに1つ以上トピック名を指定します。

優先度は　topics > all です。

MyRosbag2クラスが破棄されるタイミングで、スマートポインタで確保しているtransportも破棄されますので、デストラクタが自動実行され、rosbagが停止します。

main.cpp

// ros2
#include <rclcpp/rclcpp.hpp>

// std
#include <memory>

// original
#include "my_rosbag2/my_rosbag2.h"

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions options;
    auto node = std::make_shared<my_rosbag2::MyRosbag2>("MyRosbag2", options);

    while (rclcpp::ok())
    {
        rclcpp::spin_some(node);
    }

    rclcpp::shutdown();
    return 0;
}

トピックを受信するため、spinを忘れずに。

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(my_rosbag2)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(rosbag2_transport REQUIRED)
find_package(std_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

include_directories(
  include
)

add_executable(${PROJECT_NAME}
  # main
  src/my_rosbag2/main.cpp
  # my_rosbag2 
  include/my_rosbag2/my_rosbag2.h
  src/my_rosbag2/my_rosbag2.cpp
)

target_link_libraries(${PROJECT_NAME}
)

ament_target_dependencies(${PROJECT_NAME}
  rclcpp
  rclcpp_components
  rosbag2_transport
  std_msgs
)

install(TARGETS
  ${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

rosbag2_transportを使うため、find_packageによるパッケージの検索と、ament_target_dependenciesで依存関係を指定します。

この構成で実行すると、metadata.yamlとrosbag2.db3が作られます。

rosbag2.db3がデータベース本体で、トピックの内容が保存されています。

もちろん、SQLiteを使えば中を見ることが出来ます。

$ sqlite3 rosbag2.db2
sqlite> .tables
messages  topics
sqlite> select * from topics;
1|/rosout|rcl_interfaces/msg/Log|cdr
2|/parameter_events|rcl_interfaces/msg/ParameterEvent|cdr
3|/chatter|std_msgs/msg/String|cdr

metadata.yamlには、ros1でのrosbag infoのような情報が書き込まれています。

metadata.yaml

rosbag2_bagfile_information:
  version: 1
  storage_identifier: sqlite3
  relative_file_paths:
    - rosbag2.db3
  duration:
    nanoseconds: 3000344351
  starting_time:
    nanoseconds_since_epoch: 1580308466937462649
  message_count: 4
  topics_with_message_count:
    - topic_metadata:
        name: /chatter
        type: std_msgs/msg/String
        serialization_format: cdr
      message_count: 4

今後は、rosbag2.db3を読み出して時間同期かけながら読み込むとか、それを使ったアノテーションツールとか面白いと思います。需要があるかはわかりませんが。

github リポジトリ

ソースコード(GitHub)

ROS2で自作パッケージでrosbag recordを実行する

環境

ソースコード

github リポジトリ