diff --git a/rosbag2_cpp/CMakeLists.txt b/rosbag2_cpp/CMakeLists.txt index 2fd3cd672a..208cd1bf00 100644 --- a/rosbag2_cpp/CMakeLists.txt +++ b/rosbag2_cpp/CMakeLists.txt @@ -36,6 +36,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) @@ -65,6 +66,7 @@ add_library(${PROJECT_NAME} SHARED ament_target_dependencies(${PROJECT_NAME} ament_index_cpp pluginlib + rclcpp rcpputils rcutils rmw @@ -103,7 +105,9 @@ install( ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) -ament_export_dependencies(pluginlib +ament_export_dependencies( + pluginlib + rclcpp rmw rmw_implementation rosbag2_storage diff --git a/rosbag2_cpp/include/rosbag2_cpp/reader.hpp b/rosbag2_cpp/include/rosbag2_cpp/reader.hpp index 42db6ab660..ddac0fa72b 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/reader.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/reader.hpp @@ -20,6 +20,9 @@ #include #include +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" + #include "rosbag2_cpp/converter_options.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" #include "rosbag2_cpp/visibility_control.hpp" @@ -110,6 +113,28 @@ class ROSBAG2_CPP_PUBLIC Reader final */ std::shared_ptr read_next(); + /** + * Read next message from storage. Will throw if no more messages are available. + * The message will be serialized in the format given to `open`. + * + * Expected usage: + * if (writer.has_next()) message = writer.read_next(); + * + * \return next message in non-serialized form + * \throws runtime_error if the Reader is not open. + */ + template + MessageT read_next() + { + MessageT msg; + auto bag_message = read_next(); + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + rclcpp::Serialization serialization; + serialization.deserialize_message(&extracted_serialized_msg, &msg); + + return msg; + } + /** * Ask bagfile for its full metadata. * diff --git a/rosbag2_cpp/include/rosbag2_cpp/writer.hpp b/rosbag2_cpp/include/rosbag2_cpp/writer.hpp index 790afe43d4..d8b0d63a31 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/writer.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/writer.hpp @@ -21,6 +21,10 @@ #include #include +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/time.hpp" + #include "rosbag2_cpp/converter_options.hpp" #include "rosbag2_cpp/visibility_control.hpp" #include "rosbag2_cpp/writers/sequential_writer.hpp" @@ -112,7 +116,7 @@ class ROSBAG2_CPP_PUBLIC Writer final /** * Write a message to a bagfile. - * Opposing a call to \sa write without topic metadata, the topic will be created. + * The topic will be created if it has not been created already. * * \param message to be written to the bagfile * \param topic_name the string of the topic this messages belongs to @@ -126,6 +130,45 @@ class ROSBAG2_CPP_PUBLIC Writer final const std::string & type_name, const std::string & serialization_format = "cdr"); + /** + * Write a serialized message to a bagfile. + * The topic will be created if it has not been created already. + * + * \param message rclcpp::SerializedMessage The serialized message to be written to the bagfile + * \param topic_name the string of the topic this messages belongs to + * \param type_name the string of the type associated with this message + * \param time The time stamp of the message + * \throws runtime_error if the Writer is not open. + */ + void write( + const rclcpp::SerializedMessage & message, + const std::string & topic_name, + const std::string & type_name, + const rclcpp::Time & time); + + /** + * Write a non-serialized message to a bagfile. + * The topic will be created if it has not been created already. + * + * \param message MessageT The serialized message to be written to the bagfile + * \param topic_name the string of the topic this messages belongs to + * \param type_name the string of the type associated with this message + * \param time The time stamp of the message + * \throws runtime_error if the Writer is not open. + */ + template + void write( + const MessageT & message, + const std::string & topic_name, + const rclcpp::Time & time) + { + rclcpp::SerializedMessage serialized_msg; + + rclcpp::Serialization serialization; + serialization.serialize_message(&message, &serialized_msg); + return write(serialized_msg, topic_name, rosidl_generator_traits::name(), time); + } + writer_interfaces::BaseWriterInterface & get_implementation_handle() const { return *writer_impl_; diff --git a/rosbag2_cpp/package.xml b/rosbag2_cpp/package.xml index 44f3dbf468..7aca741f1c 100644 --- a/rosbag2_cpp/package.xml +++ b/rosbag2_cpp/package.xml @@ -12,6 +12,7 @@ ament_index_cpp pluginlib + rclcpp rcutils rcpputils rmw diff --git a/rosbag2_cpp/src/rosbag2_cpp/writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writer.cpp index b10ab13fc0..579a01506a 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writer.cpp @@ -21,6 +21,9 @@ #include #include +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/time.hpp" + #include "rosbag2_cpp/info.hpp" #include "rosbag2_cpp/writer_interfaces/base_writer_interface.hpp" @@ -28,6 +31,8 @@ #include "rosbag2_storage/storage_options.hpp" #include "rosbag2_storage/topic_metadata.hpp" +#include "rmw/rmw.h" + namespace rosbag2_cpp { @@ -97,4 +102,24 @@ void Writer::write( write(message); } +void Writer::write( + const rclcpp::SerializedMessage & message, + const std::string & topic_name, + const std::string & type_name, + const rclcpp::Time & time) +{ + auto serialized_bag_message = std::make_shared(); + serialized_bag_message->topic_name = topic_name; + serialized_bag_message->time_stamp = time.nanoseconds(); + + // temporary store the payload in a shared_ptr. + // add custom no-op deleter to avoid deep copying data. + serialized_bag_message->serialized_data = std::shared_ptr( + const_cast(&message.get_rcl_serialized_message()), + [](rcutils_uint8_array_t * /* data */) {}); + + return write( + serialized_bag_message, topic_name, type_name, rmw_get_serialization_format()); +} + } // namespace rosbag2_cpp diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_api.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_api.cpp index 6c9d8cf07a..22a8272f62 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_api.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_api.cpp @@ -18,6 +18,7 @@ #include #include +#include "rclcpp/clock.hpp" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" @@ -74,6 +75,14 @@ TEST(TestRosbag2CPPAPI, minimal_writer_example) bag_message->topic_name = "/my/other/topic"; writer.write(bag_message, "/my/other/topic", "test_msgs/msg/BasicTypes"); + // yet another alternative, writing the rclcpp::SerializedMessage directly + writer.write( + serialized_msg, "/yet/another/topic", "test_msgs/msg/BasicTypes", + rclcpp::Clock().now()); + + // writing a non-serialized message + writer.write(test_msg, "/a/ros2/message", rclcpp::Clock().now()); + // close on scope exit } @@ -92,9 +101,23 @@ TEST(TestRosbag2CPPAPI, minimal_writer_example) EXPECT_EQ(test_msg, extracted_test_msg); } - ASSERT_EQ(2u, topics.size()); + ASSERT_EQ(4u, topics.size()); EXPECT_EQ("/my/test/topic", topics[0]); EXPECT_EQ("/my/other/topic", topics[1]); + EXPECT_EQ("/yet/another/topic", topics[2]); + EXPECT_EQ("/a/ros2/message", topics[3]); + + // close on scope exit + } + + // alternative reader + { + rosbag2_cpp::Reader reader; + reader.open(rosbag_directory.string()); + while (reader.has_next()) { + TestMsgT extracted_test_msg = reader.read_next(); + EXPECT_EQ(test_msg, extracted_test_msg); + } // close on scope exit }