ros2において、自作パッケージでrosbagを実行する方法です。
ros2の情報はまだまだ少ないので、記事投稿を通して少しでも盛り上げていきたいです。
環境
-
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を読み出して時間同期かけながら読み込むとか、それを使ったアノテーションツールとか面白いと思います。需要があるかはわかりませんが。