From 7214706e490b69fc2e551abd91f07f4f04c4f0ac Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Mon, 29 Oct 2018 15:56:30 +0100 Subject: [PATCH 01/22] GH-118 Make rosbag2::Writer use converters - Use converters in Writer::write() when input rmw serialization format is different from desired storage serialization format - Add new field in rosbag2::StorageOptions to keep track of the rmw format given by the user to store the message in --- rosbag2/CMakeLists.txt | 6 ++ rosbag2/include/rosbag2/converter.hpp | 8 +- rosbag2/include/rosbag2/storage_options.hpp | 1 + rosbag2/include/rosbag2/writer.hpp | 20 ++++- rosbag2/src/rosbag2/converter.cpp | 26 ++++-- rosbag2/src/rosbag2/sequential_reader.cpp | 5 +- rosbag2/src/rosbag2/writer.cpp | 33 +++++-- rosbag2/test/rosbag2/test_writer.cpp | 90 +++++++++++++++++++ .../rosbag2_transport/rosbag2_transport.cpp | 2 +- .../rosbag2_transport_python.cpp | 2 + .../test/rosbag2_transport/mock_writer.hpp | 5 +- .../rosbag2_transport_test_fixture.hpp | 2 +- 12 files changed, 175 insertions(+), 25 deletions(-) create mode 100644 rosbag2/test/rosbag2/test_writer.cpp diff --git a/rosbag2/CMakeLists.txt b/rosbag2/CMakeLists.txt index 6fbccde5f..62b61ddc2 100644 --- a/rosbag2/CMakeLists.txt +++ b/rosbag2/CMakeLists.txt @@ -125,6 +125,12 @@ if(BUILD_TESTING) target_link_libraries(test_ros2_message rosbag2) ament_target_dependencies(test_ros2_message test_msgs) endif() + + ament_add_gmock(test_writer + test/rosbag2/test_writer.cpp) + if(TARGET test_writer) + target_link_libraries(test_writer rosbag2) + endif() endif() ament_package() diff --git a/rosbag2/include/rosbag2/converter.hpp b/rosbag2/include/rosbag2/converter.hpp index d4c862d0e..ae177dc31 100644 --- a/rosbag2/include/rosbag2/converter.hpp +++ b/rosbag2/include/rosbag2/converter.hpp @@ -36,6 +36,9 @@ namespace rosbag2 { + +struct ConverterTypeSupport; + class ROSBAG2_PUBLIC Converter { public: @@ -43,7 +46,6 @@ class ROSBAG2_PUBLIC Converter Converter( const std::string & input_format, const std::string & output_format, - const std::vector & topics_and_types, std::shared_ptr converter_factory = std::make_shared()); @@ -60,11 +62,13 @@ class ROSBAG2_PUBLIC Converter std::shared_ptr convert(std::shared_ptr message); + void add_topic(const std::string & topic, const std::string & type); + private: std::shared_ptr converter_factory_; std::unique_ptr input_converter_; std::unique_ptr output_converter_; - std::map topics_and_types_; + std::map topics_and_types_; }; } // namespace rosbag2 diff --git a/rosbag2/include/rosbag2/storage_options.hpp b/rosbag2/include/rosbag2/storage_options.hpp index dc8e37e3f..b07f439b4 100644 --- a/rosbag2/include/rosbag2/storage_options.hpp +++ b/rosbag2/include/rosbag2/storage_options.hpp @@ -25,6 +25,7 @@ struct StorageOptions public: std::string uri; std::string storage_id; + std::string rmw_serialization_format; }; } // namespace rosbag2 diff --git a/rosbag2/include/rosbag2/writer.hpp b/rosbag2/include/rosbag2/writer.hpp index 609ef12b8..044bc8d20 100644 --- a/rosbag2/include/rosbag2/writer.hpp +++ b/rosbag2/include/rosbag2/writer.hpp @@ -18,9 +18,12 @@ #include #include +#include "rosbag2_storage/metadata_io.hpp" #include "rosbag2_storage/storage_factory.hpp" #include "rosbag2_storage/storage_factory_interface.hpp" #include "rosbag2_storage/storage_interfaces/read_write_interface.hpp" +#include "rosbag2/converter.hpp" +#include "rosbag2/serialization_format_converter_factory.hpp" #include "rosbag2/storage_options.hpp" #include "rosbag2/types.hpp" #include "rosbag2/visibility_control.hpp" @@ -45,8 +48,12 @@ class ROSBAG2_PUBLIC Writer public: explicit Writer( - std::unique_ptr factory = - std::make_unique()); + std::unique_ptr storage_factory = + std::make_unique(), + std::shared_ptr converter_factory = + std::make_shared(), + std::unique_ptr metadata_io = + std::make_unique()); virtual ~Writer(); /** @@ -54,8 +61,10 @@ class ROSBAG2_PUBLIC Writer * This must be called before any other function is used. * * \param options Options to configure the storage + * \param input_serialization_format The serialization format corresponding to the currently + * used rmw */ - virtual void open(const StorageOptions & options); + virtual void open(const StorageOptions & options, const std::string & input_serialization_format); /** * Create a new topic in the underlying storage. Needs to be called for every topic used within @@ -76,8 +85,11 @@ class ROSBAG2_PUBLIC Writer private: std::string uri_; - std::unique_ptr factory_; + std::unique_ptr storage_factory_; + std::shared_ptr converter_factory_; std::shared_ptr storage_; + std::unique_ptr metadata_io_; + std::unique_ptr converter_; }; } // namespace rosbag2 diff --git a/rosbag2/src/rosbag2/converter.cpp b/rosbag2/src/rosbag2/converter.cpp index ecefd07ea..bc6366b87 100644 --- a/rosbag2/src/rosbag2/converter.cpp +++ b/rosbag2/src/rosbag2/converter.cpp @@ -28,16 +28,18 @@ namespace rosbag2 { +struct ConverterTypeSupport +{ + const rosidl_message_type_support_t * type_support; + const rosidl_message_type_support_t * introspection_type_support; +}; + Converter::Converter( const std::string & input_format, const std::string & output_format, - const std::vector & topics_and_types, std::shared_ptr converter_factory) : converter_factory_(converter_factory) { - for (const auto & topic_with_type : topics_and_types) { - topics_and_types_.insert({topic_with_type.name, topic_with_type.type}); - } input_converter_ = converter_factory_->load_converter(input_format); output_converter_ = converter_factory_->load_converter(output_format); if (!input_converter_) { @@ -58,9 +60,9 @@ Converter::~Converter() std::shared_ptr Converter::convert( std::shared_ptr message) { - auto ts = get_typesupport(topics_and_types_[message->topic_name], "rosidl_typesupport_cpp"); - auto introspection_ts = - get_typesupport(topics_and_types_[message->topic_name], "rosidl_typesupport_introspection_cpp"); + auto ts = topics_and_types_[message->topic_name].type_support; + auto introspection_ts = topics_and_types_[message->topic_name].introspection_type_support; + auto allocator = rcutils_get_default_allocator(); std::shared_ptr allocated_ros_message = allocate_ros2_message(introspection_ts, &allocator); @@ -72,4 +74,14 @@ std::shared_ptr Converter::convert( return output_message; } +void Converter::add_topic(const std::string & topic, const std::string & type) +{ + ConverterTypeSupport type_support; + type_support.type_support = get_typesupport(type, "rosidl_typesupport_cpp"); + type_support.introspection_type_support = + get_typesupport(type, "rosidl_typesupport_introspection_cpp"); + + topics_and_types_.insert({topic, type_support}); +} + } // namespace rosbag2 diff --git a/rosbag2/src/rosbag2/sequential_reader.cpp b/rosbag2/src/rosbag2/sequential_reader.cpp index f13f00b70..5e003d10b 100644 --- a/rosbag2/src/rosbag2/sequential_reader.cpp +++ b/rosbag2/src/rosbag2/sequential_reader.cpp @@ -46,8 +46,11 @@ SequentialReader::open(const StorageOptions & options, const std::string & rmw_s converter_ = std::make_unique( storage_serialization_format, rmw_serialization_format, - storage_->get_all_topics_and_types(), converter_factory_); + auto topics = storage_->get_all_topics_and_types(); + for (const auto & topic_with_type : topics) { + converter_->add_topic(topic_with_type.name, topic_with_type.type); + } } } diff --git a/rosbag2/src/rosbag2/writer.cpp b/rosbag2/src/rosbag2/writer.cpp index e927ba527..437ed6d80 100644 --- a/rosbag2/src/rosbag2/writer.cpp +++ b/rosbag2/src/rosbag2/writer.cpp @@ -17,35 +17,48 @@ #include #include #include +#include -#include "rosbag2_storage/metadata_io.hpp" #include "rosbag2/info.hpp" #include "rosbag2/storage_options.hpp" namespace rosbag2 { -Writer::Writer(std::unique_ptr factory) -: factory_(std::move(factory)) +Writer::Writer( + std::unique_ptr storage_factory, + std::shared_ptr converter_factory, + std::unique_ptr metadata_io) +: storage_factory_(std::move(storage_factory)), + converter_factory_(std::move(converter_factory)), + metadata_io_(std::move(metadata_io)), + converter_(nullptr) {} Writer::~Writer() { if (!uri_.empty()) { - rosbag2_storage::MetadataIo metadata_io; - metadata_io.write_metadata(uri_, storage_->get_metadata()); + metadata_io_->write_metadata(uri_, storage_->get_metadata()); } storage_.reset(); // Necessary to ensure that the writer is destroyed before the factory } -void Writer::open(const StorageOptions & options) +void Writer::open(const StorageOptions & options, const std::string & input_serialization_format) { - storage_ = factory_->open_read_write(options.uri, options.storage_id); + storage_ = storage_factory_->open_read_write(options.uri, options.storage_id); if (!storage_) { throw std::runtime_error("No storage could be initialized. Abort"); } uri_ = options.uri; + + std::string output_rmw_format = options.rmw_serialization_format; + if (output_rmw_format != input_serialization_format) { + converter_ = std::make_unique( + input_serialization_format, + output_rmw_format, + converter_factory_); + } } void Writer::create_topic(const TopicMetadata & topic_with_type) @@ -54,6 +67,10 @@ void Writer::create_topic(const TopicMetadata & topic_with_type) throw std::runtime_error("Bag is not open. Call open() before writing."); } + if (converter_) { + converter_->add_topic(topic_with_type.name, topic_with_type.type); + } + storage_->create_topic(topic_with_type); } @@ -63,7 +80,7 @@ void Writer::write(std::shared_ptr message) throw std::runtime_error("Bag is not open. Call open() before writing."); } - storage_->write(message); + storage_->write(converter_ ? converter_->convert(message) : message); } } // namespace rosbag2 diff --git a/rosbag2/test/rosbag2/test_writer.cpp b/rosbag2/test/rosbag2/test_writer.cpp new file mode 100644 index 000000000..cd4c451d9 --- /dev/null +++ b/rosbag2/test/rosbag2/test_writer.cpp @@ -0,0 +1,90 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rosbag2/writer.hpp" +#include "rosbag2_storage/bag_metadata.hpp" +#include "rosbag2_storage/topic_with_type.hpp" + +#include "mock_converter.hpp" +#include "mock_converter_factory.hpp" +#include "mock_storage.hpp" +#include "mock_storage_factory.hpp" + +using namespace testing; // NOLINT + +class WriterTest : public Test +{ +public: + WriterTest() + { + storage_factory_ = std::make_unique>(); + storage_ = std::make_shared>(); + converter_factory_ = std::make_shared>(); + + EXPECT_CALL(*storage_factory_, open_read_write(_, _)).WillOnce(Return(storage_)); + + writer_ = std::make_unique( + std::move(storage_factory_), converter_factory_); + } + + std::unique_ptr> storage_factory_; + std::shared_ptr> storage_; + std::shared_ptr> converter_factory_; + std::unique_ptr writer_; +}; + +TEST_F(WriterTest, + write_uses_converters_to_convert_serialization_format_if_input_and_output_format_are_different) { + std::string storage_serialization_format = "rmw1_format"; + std::string input_format = "rmw2_format"; + + auto format1_converter = std::make_unique>(); + auto format2_converter = std::make_unique>(); + EXPECT_CALL(*format1_converter, serialize(_, _, _)).Times(1); + EXPECT_CALL(*format2_converter, deserialize(_, _, _)).Times(1); + + EXPECT_CALL(*converter_factory_, load_converter(storage_serialization_format)) + .WillOnce(Return(ByMove(std::move(format1_converter)))); + EXPECT_CALL(*converter_factory_, load_converter(input_format)) + .WillOnce(Return(ByMove(std::move(format2_converter)))); + + auto message = std::make_shared(); + message->topic_name = "test_topic"; + rosbag2::StorageOptions options; + options.rmw_serialization_format = storage_serialization_format; + writer_->open(options, input_format); + writer_->create_topic({"test_topic", "test_msgs/Primitives"}); + writer_->write(message); +} + +TEST_F(WriterTest, write_does_not_use_converters_if_input_and_output_format_are_equal) { + std::string storage_serialization_format = "rmw_format"; + + EXPECT_CALL(*converter_factory_, load_converter(storage_serialization_format)).Times(0); + + auto message = std::make_shared(); + message->topic_name = "test_topic"; + rosbag2::StorageOptions options; + options.rmw_serialization_format = storage_serialization_format; + writer_->open(options, storage_serialization_format); + writer_->create_topic({"test_topic", "test_msgs/Primitives"}); + writer_->write(message); +} diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp index e18830be3..6ba44e4b8 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp @@ -63,7 +63,7 @@ void Rosbag2Transport::record( const StorageOptions & storage_options, const RecordOptions & record_options) { try { - writer_->open(storage_options); + writer_->open(storage_options, rmw_get_serialization_format()); auto transport_node = setup_node(); diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp index 69022dc92..27e179a1c 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp @@ -43,6 +43,8 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * storage_options.uri = std::string(uri); storage_options.storage_id = std::string(storage_id); + // TODO(botteroa-si): get this from CLI. + storage_options.rmw_serialization_format = "cdr"; record_options.all = all; if (topics) { diff --git a/rosbag2_transport/test/rosbag2_transport/mock_writer.hpp b/rosbag2_transport/test/rosbag2_transport/mock_writer.hpp index e20b60e48..55fca3bb9 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_writer.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_writer.hpp @@ -25,9 +25,12 @@ class MockWriter : public rosbag2::Writer { public: - void open(const rosbag2::StorageOptions & options) override + void open( + const rosbag2::StorageOptions & options, + const std::string & input_serialization_format) override { (void) options; + (void) input_serialization_format; } void create_topic(const rosbag2::TopicMetadata & topic_with_type) override diff --git a/rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp b/rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp index ff1750c14..5b2863d4e 100644 --- a/rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp +++ b/rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp @@ -57,7 +57,7 @@ class Rosbag2TransportTestFixture : public Test { public: Rosbag2TransportTestFixture() - : storage_options_({"uri", "storage_id"}), play_options_({1000}), + : storage_options_({"uri", "storage_id", "rmw_serialization_format"}), play_options_({1000}), reader_(std::make_shared()), writer_(std::make_shared()), info_(std::make_shared()) {} From 770cc4d77354b2c5e40b5637232d4cbc48f9b7bd Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Tue, 30 Oct 2018 11:06:04 +0100 Subject: [PATCH 02/22] GH-118 Add --encoding option to ros2 bag record --- ros2bag/ros2bag/verb/record.py | 10 ++++++++-- .../src/rosbag2_transport/rosbag2_transport_python.cpp | 9 +++++---- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index fd80fe086..35bd6f9bf 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -42,6 +42,10 @@ def add_arguments(self, parser, cli_name): # noqa: D102 parser.add_argument( '-s', '--storage', default='sqlite3', help='storage identifier to be used, defaults to "sqlite3"') + parser.add_argument( + '-e', '--encoding', default='', + help='rmw serialization format in which the messages are saved, defaults to the' + ' rmw currently in use') def create_bag_directory(self, uri): try: @@ -61,9 +65,11 @@ def main(self, *, args): # noqa: D102 self.create_bag_directory(uri) if args.all: - rosbag2_transport_py.record(uri=uri, storage_id=args.storage, all=True) + rosbag2_transport_py.record( + uri=uri, storage_id=args.storage, encoding=args.encoding, all=True) elif args.topics and len(args.topics) > 0: - rosbag2_transport_py.record(uri=uri, storage_id=args.storage, topics=args.topics) + rosbag2_transport_py.record( + uri=uri, storage_id=args.storage, encoding=args.encoding, topics=args.topics) else: self._subparser.print_help() diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp index 27e179a1c..c1b0b1e55 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp @@ -26,15 +26,17 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * rosbag2_transport::StorageOptions storage_options{}; rosbag2_transport::RecordOptions record_options{}; - static const char * kwlist[] = {"uri", "storage_id", "all", "topics", nullptr}; + static const char * kwlist[] = {"uri", "storage_id", "encoding", "all", "topics", nullptr}; char * uri = nullptr; char * storage_id = nullptr; + char * encoding = nullptr; bool all = false; PyObject * topics = nullptr; - if (!PyArg_ParseTupleAndKeywords(args, kwargs, "ss|bO", const_cast(kwlist), + if (!PyArg_ParseTupleAndKeywords(args, kwargs, "sss|bO", const_cast(kwlist), &uri, &storage_id, + &encoding, &all, &topics)) { @@ -43,8 +45,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * storage_options.uri = std::string(uri); storage_options.storage_id = std::string(storage_id); - // TODO(botteroa-si): get this from CLI. - storage_options.rmw_serialization_format = "cdr"; + storage_options.rmw_serialization_format = std::string(encoding).empty() ? "cdr" : encoding; record_options.all = all; if (topics) { From fd39d6eeb13ebfcd18389617cd6af03535d5e449 Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Tue, 30 Oct 2018 13:46:25 +0100 Subject: [PATCH 03/22] GH-118 Associate to each topic its rmw_serialization_format - Add 'serialization_format' field to TopicMetadata - Add 'serialization_forat' column in 'topics' table in sqlite storage - Remove 'storage_format' from BagMetadata and use the TopicMetadata field directly, instead - the field 'rmw_serialization_format' has been moved from rosbag2::StorageOptions to rosbag2_transport::RecordOptions, because it's a topic property rather than a storage one. - Currently all topics in a bag file must have the same serialization format - The tests have been updated accordingly --- rosbag2/include/rosbag2/storage_options.hpp | 1 - rosbag2/include/rosbag2/writer.hpp | 5 +- rosbag2/src/rosbag2/sequential_reader.cpp | 17 ++++++- rosbag2/src/rosbag2/writer.cpp | 10 ++-- rosbag2/test/rosbag2/test_info.cpp | 12 +++-- .../test/rosbag2/test_sequential_reader.cpp | 2 +- rosbag2/test/rosbag2/test_writer.cpp | 10 ++-- .../include/rosbag2_storage/bag_metadata.hpp | 1 - .../rosbag2_storage/topic_with_type.hpp | 3 +- .../src/rosbag2_storage/metadata_io.cpp | 5 +- .../test_metadata_serialization.cpp | 10 ++-- .../sqlite/sqlite_storage.cpp | 34 ++++++++----- .../sqlite/storage_test_fixture.hpp | 5 +- .../sqlite/test_sqlite_storage.cpp | 48 ++++++++++-------- rosbag2_tests/resources/test/metadata.yaml | 3 +- rosbag2_tests/resources/test/test.db3 | Bin 12288 -> 12288 bytes .../test_rosbag2_info_end_to_end.cpp | 19 ++++--- .../rosbag2_transport/record_options.hpp | 1 + .../src/rosbag2_transport/formatter.cpp | 3 +- .../src/rosbag2_transport/recorder.cpp | 2 +- .../rosbag2_transport/rosbag2_transport.cpp | 8 +-- .../rosbag2_transport_python.cpp | 2 +- .../test/rosbag2_transport/mock_writer.hpp | 4 +- .../rosbag2_transport_test_fixture.hpp | 2 +- .../test/rosbag2_transport/test_formatter.cpp | 8 +-- .../test/rosbag2_transport/test_info.cpp | 26 +++++----- .../test/rosbag2_transport/test_play.cpp | 4 +- .../rosbag2_transport/test_play_timing.cpp | 3 +- .../test/rosbag2_transport/test_record.cpp | 2 +- .../rosbag2_transport/test_record_all.cpp | 2 +- 30 files changed, 147 insertions(+), 105 deletions(-) diff --git a/rosbag2/include/rosbag2/storage_options.hpp b/rosbag2/include/rosbag2/storage_options.hpp index b07f439b4..dc8e37e3f 100644 --- a/rosbag2/include/rosbag2/storage_options.hpp +++ b/rosbag2/include/rosbag2/storage_options.hpp @@ -25,7 +25,6 @@ struct StorageOptions public: std::string uri; std::string storage_id; - std::string rmw_serialization_format; }; } // namespace rosbag2 diff --git a/rosbag2/include/rosbag2/writer.hpp b/rosbag2/include/rosbag2/writer.hpp index 044bc8d20..8444f5b00 100644 --- a/rosbag2/include/rosbag2/writer.hpp +++ b/rosbag2/include/rosbag2/writer.hpp @@ -64,7 +64,10 @@ class ROSBAG2_PUBLIC Writer * \param input_serialization_format The serialization format corresponding to the currently * used rmw */ - virtual void open(const StorageOptions & options, const std::string & input_serialization_format); + virtual void open( + const StorageOptions & options, + const std::string & input_serialization_format, + const std::string & output_serialization_format); /** * Create a new topic in the underlying storage. Needs to be called for every topic used within diff --git a/rosbag2/src/rosbag2/sequential_reader.cpp b/rosbag2/src/rosbag2/sequential_reader.cpp index 5e003d10b..b1a590aa3 100644 --- a/rosbag2/src/rosbag2/sequential_reader.cpp +++ b/rosbag2/src/rosbag2/sequential_reader.cpp @@ -19,6 +19,8 @@ #include #include +#include "rosbag2/info.hpp" + namespace rosbag2 { @@ -41,7 +43,20 @@ SequentialReader::open(const StorageOptions & options, const std::string & rmw_s if (!storage_) { throw std::runtime_error("No storage could be initialized. Abort"); } - auto storage_serialization_format = storage_->get_metadata().serialization_format; + auto topics = storage_->get_metadata().topics_with_message_count; + if (topics.empty()) { + return; + } + + // Currently a bag file can only be played if all topics have the same serialization format. + auto storage_serialization_format = topics[0].topic_with_type.serialization_format; + for (const auto & topic : topics) { + if (topic.topic_with_type.serialization_format != storage_serialization_format) { + throw std::runtime_error("Topics with different rwm serialization format have been found. " + "All topics must have the same serialization format."); + } + } + if (rmw_serialization_format != storage_serialization_format) { converter_ = std::make_unique( storage_serialization_format, diff --git a/rosbag2/src/rosbag2/writer.cpp b/rosbag2/src/rosbag2/writer.cpp index 437ed6d80..4153d19f1 100644 --- a/rosbag2/src/rosbag2/writer.cpp +++ b/rosbag2/src/rosbag2/writer.cpp @@ -44,7 +44,10 @@ Writer::~Writer() storage_.reset(); // Necessary to ensure that the writer is destroyed before the factory } -void Writer::open(const StorageOptions & options, const std::string & input_serialization_format) +void Writer::open( + const StorageOptions & options, + const std::string & input_serialization_format, + const std::string & output_serialization_format) { storage_ = storage_factory_->open_read_write(options.uri, options.storage_id); if (!storage_) { @@ -52,11 +55,10 @@ void Writer::open(const StorageOptions & options, const std::string & input_seri } uri_ = options.uri; - std::string output_rmw_format = options.rmw_serialization_format; - if (output_rmw_format != input_serialization_format) { + if (output_serialization_format != input_serialization_format) { converter_ = std::make_unique( input_serialization_format, - output_rmw_format, + output_serialization_format, converter_factory_); } } diff --git a/rosbag2/test/rosbag2/test_info.cpp b/rosbag2/test/rosbag2/test_info.cpp index 051fad5de..72c96a2cb 100644 --- a/rosbag2/test/rosbag2/test_info.cpp +++ b/rosbag2/test/rosbag2/test_info.cpp @@ -34,7 +34,6 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada "rosbag2_bagfile_information:\n" " version: 1\n" " storage_identifier: sqlite3\n" - " serialization_format: cdr\n" " relative_file_paths:\n" " - some_relative_path\n" " - some_other_relative_path\n" @@ -48,10 +47,12 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada " - topic_and_type:\n" " name: topic1\n" " type: type1\n" + " serialization_format: rmw1\n" " message_count: 100\n" " - topic_and_type:\n" " name: topic2\n" " type: type2\n" + " serialization_format: rmw2\n" " message_count: 200"); std::ofstream fout( @@ -65,7 +66,6 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada auto read_metadata = info.read_metadata(temporary_dir_path_); EXPECT_THAT(read_metadata.storage_identifier, Eq("sqlite3")); - EXPECT_THAT(read_metadata.serialization_format, Eq("cdr")); EXPECT_THAT(read_metadata.relative_file_paths, Eq(std::vector({"some_relative_path", "some_other_relative_path"}))); EXPECT_THAT(read_metadata.duration, Eq(std::chrono::nanoseconds(100))); @@ -76,17 +76,21 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada EXPECT_THAT(read_metadata.topics_with_message_count, SizeIs(2u)); auto actual_first_topic = read_metadata.topics_with_message_count[0]; - rosbag2_storage::TopicInformation expected_first_topic = {{"topic1", "type1"}, 100}; + rosbag2_storage::TopicInformation expected_first_topic = {{"topic1", "type1", "rmw1"}, 100}; EXPECT_THAT(actual_first_topic.topic_with_type.name, Eq(expected_first_topic.topic_with_type.name)); EXPECT_THAT(actual_first_topic.topic_with_type.type, Eq(expected_first_topic.topic_with_type.type)); + EXPECT_THAT(actual_first_topic.topic_with_type.serialization_format, + Eq(expected_first_topic.topic_with_type.serialization_format)); EXPECT_THAT(actual_first_topic.message_count, Eq(expected_first_topic.message_count)); auto actual_second_topic = read_metadata.topics_with_message_count[1]; - rosbag2_storage::TopicInformation expected_second_topic = {{"topic2", "type2"}, 200}; + rosbag2_storage::TopicInformation expected_second_topic = {{"topic2", "type2", "rmw2"}, 200}; EXPECT_THAT(actual_second_topic.topic_with_type.name, Eq(expected_second_topic.topic_with_type.name)); EXPECT_THAT(actual_second_topic.topic_with_type.type, Eq(expected_second_topic.topic_with_type.type)); + EXPECT_THAT(actual_second_topic.topic_with_type.serialization_format, + Eq(expected_second_topic.topic_with_type.serialization_format)); EXPECT_THAT(actual_second_topic.message_count, Eq(expected_second_topic.message_count)); } diff --git a/rosbag2/test/rosbag2/test_sequential_reader.cpp b/rosbag2/test/rosbag2/test_sequential_reader.cpp index ee92973a8..3dbbecdcd 100644 --- a/rosbag2/test/rosbag2/test_sequential_reader.cpp +++ b/rosbag2/test/rosbag2/test_sequential_reader.cpp @@ -59,7 +59,7 @@ class SequentialReaderTest : public Test void set_storage_serialization_format(const std::string & format) { rosbag2_storage::BagMetadata metadata; - metadata.serialization_format = format; + metadata.topics_with_message_count.push_back({{"", "", format}, 1}); EXPECT_CALL(*storage_, get_metadata()).WillOnce(Return(metadata)); } diff --git a/rosbag2/test/rosbag2/test_writer.cpp b/rosbag2/test/rosbag2/test_writer.cpp index cd4c451d9..aace2fe88 100644 --- a/rosbag2/test/rosbag2/test_writer.cpp +++ b/rosbag2/test/rosbag2/test_writer.cpp @@ -69,9 +69,8 @@ TEST_F(WriterTest, auto message = std::make_shared(); message->topic_name = "test_topic"; rosbag2::StorageOptions options; - options.rmw_serialization_format = storage_serialization_format; - writer_->open(options, input_format); - writer_->create_topic({"test_topic", "test_msgs/Primitives"}); + writer_->open(options, input_format, storage_serialization_format); + writer_->create_topic({"test_topic", "test_msgs/Primitives", ""}); writer_->write(message); } @@ -83,8 +82,7 @@ TEST_F(WriterTest, write_does_not_use_converters_if_input_and_output_format_are_ auto message = std::make_shared(); message->topic_name = "test_topic"; rosbag2::StorageOptions options; - options.rmw_serialization_format = storage_serialization_format; - writer_->open(options, storage_serialization_format); - writer_->create_topic({"test_topic", "test_msgs/Primitives"}); + writer_->open(options, storage_serialization_format, storage_serialization_format); + writer_->create_topic({"test_topic", "test_msgs/Primitives", ""}); writer_->write(message); } diff --git a/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp b/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp index 098ab0339..c61198f2d 100644 --- a/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp +++ b/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp @@ -37,7 +37,6 @@ struct BagMetadata int version = 1; // upgrade this number when changing the content of the struct size_t bag_size = 0; // Will not be serialized std::string storage_identifier; - std::string serialization_format; std::vector relative_file_paths; std::chrono::nanoseconds duration; std::chrono::time_point starting_time; diff --git a/rosbag2_storage/include/rosbag2_storage/topic_with_type.hpp b/rosbag2_storage/include/rosbag2_storage/topic_with_type.hpp index c70f65306..69a7a22f1 100644 --- a/rosbag2_storage/include/rosbag2_storage/topic_with_type.hpp +++ b/rosbag2_storage/include/rosbag2_storage/topic_with_type.hpp @@ -24,8 +24,9 @@ struct TopicMetadata { std::string name; std::string type; + std::string serialization_format; }; -} +} // namespace rosbag2_storage #endif // ROSBAG2_STORAGE__TOPIC_WITH_TYPE_HPP_ diff --git a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp index 5c18f3b59..6f440d3ad 100644 --- a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp +++ b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp @@ -43,6 +43,7 @@ struct convert Node node; node["name"] = topic.name; node["type"] = topic.type; + node["serialization_format"] = topic.serialization_format; return node; } @@ -50,6 +51,8 @@ struct convert { topic.name = node["name"].as(); topic.type = node["type"].as(); + topic.serialization_format = node["serialization_format"].as(); + return true; } }; @@ -117,7 +120,6 @@ struct convert Node node; node["version"] = metadata.version; node["storage_identifier"] = metadata.storage_identifier; - node["serialization_format"] = metadata.serialization_format; node["relative_file_paths"] = metadata.relative_file_paths; node["duration"] = metadata.duration; node["starting_time"] = metadata.starting_time; @@ -130,7 +132,6 @@ struct convert { metadata.version = node["version"].as(); metadata.storage_identifier = node["storage_identifier"].as(); - metadata.serialization_format = node["serialization_format"].as(); metadata.relative_file_paths = node["relative_file_paths"].as>(); metadata.duration = node["duration"].as(); metadata.starting_time = node["starting_time"] diff --git a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp index 9e3cc8bd7..42ca65e93 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp @@ -48,22 +48,20 @@ TEST_F(MetadataFixture, test_writing_and_reading_yaml) BagMetadata metadata{}; metadata.version = 1; metadata.storage_identifier = "sqlite3"; - metadata.serialization_format = "cdr"; metadata.relative_file_paths.emplace_back("some_relative_path"); metadata.relative_file_paths.emplace_back("some_other_relative_path"); metadata.duration = std::chrono::nanoseconds(100); metadata.starting_time = std::chrono::time_point(std::chrono::nanoseconds(1000000)); metadata.message_count = 50; - metadata.topics_with_message_count.push_back({{"topic1", "type1"}, 100}); - metadata.topics_with_message_count.push_back({{"topic2", "type2"}, 200}); + metadata.topics_with_message_count.push_back({{"topic1", "type1", "rmw1"}, 100}); + metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2"}, 200}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); EXPECT_THAT(read_metadata.version, Eq(metadata.version)); EXPECT_THAT(read_metadata.storage_identifier, Eq(metadata.storage_identifier)); - EXPECT_THAT(read_metadata.serialization_format, Eq(metadata.serialization_format)); EXPECT_THAT(read_metadata.relative_file_paths, Eq(metadata.relative_file_paths)); EXPECT_THAT(read_metadata.duration, Eq(metadata.duration)); EXPECT_THAT(read_metadata.starting_time, Eq(metadata.starting_time)); @@ -76,6 +74,8 @@ TEST_F(MetadataFixture, test_writing_and_reading_yaml) Eq(expected_first_topic.topic_with_type.name)); EXPECT_THAT(actual_first_topic.topic_with_type.type, Eq(expected_first_topic.topic_with_type.type)); + EXPECT_THAT(actual_first_topic.topic_with_type.serialization_format, + Eq(expected_first_topic.topic_with_type.serialization_format)); EXPECT_THAT(actual_first_topic.message_count, Eq(expected_first_topic.message_count)); auto actual_second_topic = read_metadata.topics_with_message_count[1]; auto expected_second_topic = metadata.topics_with_message_count[1]; @@ -83,5 +83,7 @@ TEST_F(MetadataFixture, test_writing_and_reading_yaml) Eq(expected_second_topic.topic_with_type.name)); EXPECT_THAT(actual_second_topic.topic_with_type.type, Eq(expected_second_topic.topic_with_type.type)); + EXPECT_THAT(actual_second_topic.topic_with_type.serialization_format, + Eq(expected_second_topic.topic_with_type.serialization_format)); EXPECT_THAT(actual_second_topic.message_count, Eq(expected_second_topic.message_count)); } diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp index b6119790f..3659456c9 100644 --- a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp @@ -139,7 +139,8 @@ void SqliteStorage::initialize() std::string create_table = "CREATE TABLE topics(" \ "id INTEGER PRIMARY KEY," \ "name TEXT NOT NULL," \ - "type TEXT NOT NULL);"; + "type TEXT NOT NULL," \ + "serialization_format TEXT NOT NULL);"; database_->prepare_statement(create_table)->execute_and_reset(); create_table = "CREATE TABLE messages(" \ "id INTEGER PRIMARY KEY," \ @@ -153,8 +154,9 @@ void SqliteStorage::create_topic(const rosbag2_storage::TopicMetadata & topic) { if (topics_.find(topic.name) == std::end(topics_)) { auto insert_topic = - database_->prepare_statement("INSERT INTO topics (name, type) VALUES (?, ?)"); - insert_topic->bind(topic.name, topic.type); + database_->prepare_statement( + "INSERT INTO topics (name, type, serialization_format) VALUES (?, ?, ?)"); + insert_topic->bind(topic.name, topic.type, topic.serialization_format); insert_topic->execute_and_reset(); topics_.emplace(topic.name, static_cast(database_->get_last_insert_id())); } @@ -179,11 +181,13 @@ void SqliteStorage::prepare_for_reading() void SqliteStorage::fill_topics_and_types() { - auto statement = database_->prepare_statement("SELECT name, type FROM topics ORDER BY id;"); - auto query_results = statement->execute_query(); + auto statement = database_->prepare_statement( + "SELECT name, type, serialization_format FROM topics ORDER BY id;"); + auto query_results = statement->execute_query(); for (auto result : query_results) { - all_topics_and_types_.push_back({std::get<0>(result), std::get<1>(result)}); + all_topics_and_types_.push_back( + {std::get<0>(result), std::get<1>(result), std::get<2>(result)}); } } @@ -213,30 +217,32 @@ rosbag2_storage::BagMetadata SqliteStorage::get_metadata() { rosbag2_storage::BagMetadata metadata; metadata.storage_identifier = "sqlite3"; - metadata.serialization_format = "cdr"; // TODO(greimela) Determine format (i.e. data encoding) metadata.relative_file_paths = {database_name_}; metadata.message_count = 0; metadata.topics_with_message_count = {}; auto statement = database_->prepare_statement( - "SELECT name, type, COUNT(messages.id), MIN(messages.timestamp), MAX(messages.timestamp) " + "SELECT name, type, serialization_format, COUNT(messages.id), MIN(messages.timestamp), " + "MAX(messages.timestamp) " "FROM messages JOIN topics on topics.id = messages.topic_id " "GROUP BY topics.name;"); auto query_results = statement->execute_query< - std::string, std::string, int, rcutils_time_point_value_t, rcutils_time_point_value_t>(); + std::string, std::string, std::string, int, rcutils_time_point_value_t, + rcutils_time_point_value_t>(); rcutils_time_point_value_t min_time = INT64_MAX; rcutils_time_point_value_t max_time = 0; for (auto result : query_results) { metadata.topics_with_message_count.push_back( { - {std::get<0>(result), std::get<1>(result)}, - static_cast(std::get<2>(result)) + {std::get<0>(result), std::get<1>(result), std::get<2>(result)}, + static_cast(std::get<3>(result)) }); - metadata.message_count += std::get<2>(result); - min_time = std::get<3>(result) < min_time ? std::get<3>(result) : min_time; - max_time = std::get<4>(result) > max_time ? std::get<4>(result) : max_time; + + metadata.message_count += std::get<3>(result); + min_time = std::get<4>(result) < min_time ? std::get<4>(result) : min_time; + max_time = std::get<5>(result) > max_time ? std::get<5>(result) : max_time; } if (metadata.message_count == 0) { diff --git a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp index c7d3a900b..d375729f0 100644 --- a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp +++ b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp @@ -85,7 +85,7 @@ class StorageTestFixture : public TemporaryDirectoryFixture } void write_messages_to_sqlite( - std::vector> messages) + std::vector> messages) { std::unique_ptr writable_storage = std::make_unique(); @@ -95,7 +95,8 @@ class StorageTestFixture : public TemporaryDirectoryFixture for (auto msg : messages) { std::string topic_name = std::get<2>(msg); std::string type_name = std::get<3>(msg); - writable_storage->create_topic({topic_name, type_name}); + std::string rmw_format = std::get<4>(msg); + writable_storage->create_topic({topic_name, type_name, rmw_format}); auto bag_message = std::make_shared(); bag_message->serialized_data = make_serialized_message(std::get<0>(msg)); bag_message->time_stamp = std::get<1>(msg); diff --git a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp index 496c1f0d7..a28828ac6 100644 --- a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp @@ -56,10 +56,11 @@ bool operator!=(const TopicInformation & lhs, const TopicInformation & rhs) TEST_F(StorageTestFixture, string_messages_are_written_and_read_to_and_from_sqlite3_storage) { std::vector string_messages = {"first message", "second message", "third message"}; std::vector topics = {"topic1", "topic2", "topic3"}; - std::vector> messages = - {std::make_tuple(string_messages[0], 1, topics[0], "type1"), - std::make_tuple(string_messages[1], 2, topics[1], "type2"), - std::make_tuple(string_messages[2], 3, topics[2], "type3")}; + std::vector rmw_formats = {"rmw1", "rmw2", "rmw3"}; + std::vector> messages = + {std::make_tuple(string_messages[0], 1, topics[0], "type1", rmw_formats[0]), + std::make_tuple(string_messages[1], 2, topics[1], "type2", rmw_formats[1]), + std::make_tuple(string_messages[2], 3, topics[2], "type3", rmw_formats[2])}; write_messages_to_sqlite(messages); auto read_messages = read_all_messages_from_sqlite(); @@ -73,8 +74,10 @@ TEST_F(StorageTestFixture, string_messages_are_written_and_read_to_and_from_sqli } TEST_F(StorageTestFixture, has_next_return_false_if_there_are_no_more_messages) { - std::vector> string_messages = - {std::make_tuple("first message", 1, "", ""), std::make_tuple("second message", 2, "", "")}; + std::vector> + string_messages = + {std::make_tuple("first message", 1, "", "", ""), + std::make_tuple("second message", 2, "", "", "")}; write_messages_to_sqlite(string_messages); std::unique_ptr readable_storage = @@ -89,8 +92,10 @@ TEST_F(StorageTestFixture, has_next_return_false_if_there_are_no_more_messages) } TEST_F(StorageTestFixture, get_next_returns_messages_in_timestamp_order) { - std::vector> string_messages = - {std::make_tuple("first_message", 6, "", ""), std::make_tuple("second_message", 2, "", "")}; + std::vector> + string_messages = + {std::make_tuple("first message", 2, "", "", ""), + std::make_tuple("second message", 6, "", "", "")}; write_messages_to_sqlite(string_messages); std::unique_ptr readable_storage = @@ -110,8 +115,8 @@ TEST_F(StorageTestFixture, get_all_topics_and_types_returns_the_correct_vector) std::unique_ptr writable_storage = std::make_unique(); writable_storage->open(temporary_dir_path_); - writable_storage->create_topic({"topic1", "type1"}); - writable_storage->create_topic({"topic2", "type2"}); + writable_storage->create_topic({"topic1", "type1", "rmw1"}); + writable_storage->create_topic({"topic2", "type2", "rmw2"}); metadata_io_.write_metadata(temporary_dir_path_, writable_storage->get_metadata()); writable_storage.reset(); @@ -121,18 +126,21 @@ TEST_F(StorageTestFixture, get_all_topics_and_types_returns_the_correct_vector) auto topics_and_types = readable_storage->get_all_topics_and_types(); EXPECT_THAT(topics_and_types, ElementsAreArray({ - rosbag2_storage::TopicMetadata{"topic1", "type1"}, - rosbag2_storage::TopicMetadata{"topic2", "type2"} + rosbag2_storage::TopicMetadata{"topic1", "type1", "rmw1"}, + rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2"} })); } TEST_F(StorageTestFixture, get_metadata_returns_correct_struct) { std::vector string_messages = {"first message", "second message", "third message"}; std::vector topics = {"topic1", "topic2"}; - std::vector> messages = - {std::make_tuple(string_messages[0], static_cast(1e9), topics[0], "type1"), - std::make_tuple(string_messages[1], static_cast(2e9), topics[0], "type1"), - std::make_tuple(string_messages[2], static_cast(3e9), topics[1], "type2")}; + std::vector> messages = + {std::make_tuple( + string_messages[0], static_cast(1e9), topics[0], "type1", "rmw_format"), + std::make_tuple( + string_messages[1], static_cast(2e9), topics[0], "type1", "rmw_format"), + std::make_tuple( + string_messages[2], static_cast(3e9), topics[1], "type2", "rmw_format")}; write_messages_to_sqlite(messages); @@ -142,13 +150,14 @@ TEST_F(StorageTestFixture, get_metadata_returns_correct_struct) { auto metadata = readable_storage->get_metadata(); EXPECT_THAT(metadata.storage_identifier, Eq("sqlite3")); - EXPECT_THAT(metadata.serialization_format, Eq("cdr")); EXPECT_THAT(metadata.relative_file_paths, ElementsAreArray({ rosbag2_storage::FilesystemHelper::get_folder_name(temporary_dir_path_) + ".db3" })); EXPECT_THAT(metadata.topics_with_message_count, ElementsAreArray({ - rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{"topic1", "type1"}, 2u}, - rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{"topic2", "type2"}, 1u} + rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{ + "topic1", "type1", "rmw_format"}, 2u}, + rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{ + "topic2", "type2", "rmw_format"}, 1u} })); EXPECT_THAT(metadata.message_count, Eq(3u)); EXPECT_THAT(metadata.starting_time, Eq( @@ -166,7 +175,6 @@ TEST_F(StorageTestFixture, get_metadata_returns_correct_struct_if_no_messages) { auto metadata = readable_storage->get_metadata(); EXPECT_THAT(metadata.storage_identifier, Eq("sqlite3")); - EXPECT_THAT(metadata.serialization_format, Eq("cdr")); EXPECT_THAT(metadata.relative_file_paths, ElementsAreArray({ rosbag2_storage::FilesystemHelper::get_folder_name(temporary_dir_path_) + ".db3" })); diff --git a/rosbag2_tests/resources/test/metadata.yaml b/rosbag2_tests/resources/test/metadata.yaml index 32ec41a82..3b860c9a3 100644 --- a/rosbag2_tests/resources/test/metadata.yaml +++ b/rosbag2_tests/resources/test/metadata.yaml @@ -1,7 +1,6 @@ rosbag2_bagfile_information: version: 1 storage_identifier: sqlite3 - serialization_format: cdr relative_file_paths: - test.db3 duration: @@ -13,8 +12,10 @@ rosbag2_bagfile_information: - topic_and_type: name: /test_topic type: test_msgs/Primitives + serialization_format: cdr message_count: 3 - topic_and_type: name: /array_topic type: test_msgs/StaticArrayPrimitives + serialization_format: cdr message_count: 4 diff --git a/rosbag2_tests/resources/test/test.db3 b/rosbag2_tests/resources/test/test.db3 index 37d84beb3617dde95b9c5f9bfe1948aa4b48fed4..278b1e2cc7169b29fdbb8604ecf7282035e0b7d8 100644 GIT binary patch delta 220 zcmZojXh@hKEy&EkzyQK9z&ugM7$~U67{$v!g@K8$jDfF=@88BkHa@OKRz`MlNlC`W z)X8)ClVo%hic^a+6LT`F5=%1k^WxL;i*gf76hd4hLNq7;cz`zW|Fu*iX#~3K6C-9pWD8$6~kAZ&*-@na*0)2dw|M5-Z%wl90 zmy~2|)SukImn5hOQpCa^&cJ`3e*=FFe>hN~ApgV$GZtkQ7KX{u@``Lm%q$H0?vvNZ x>#52zu`oy)>nG>rCubMs7pE5`7G%Wd7N-~MJA;Ju8Ce)qy(eqRYcEm|001bxBqRU; diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp index 56623f5f5..64919c08d 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp @@ -41,16 +41,15 @@ TEST_F(InfoEndToEndTestFixture, info_end_to_end_test) { EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); // The bag size depends on the os and is not asserted, the time is asserted time zone independent EXPECT_THAT(output, ContainsRegex( - "\nFiles: test\\.db3" - "\nBag size: .*B" - "\nStorage id: sqlite3" - "\nSerialization format: cdr" - "\nDuration: 0\\.155s" - "\nStart: Sep .+ 2018 .+:.+:44\\.241 \\(1537282604\\.241\\)" - "\nEnd Sep .+ 2018 .+:.+:44\\.397 \\(1537282604\\.397\\)" - "\nMessages: 7" - "\nTopics with Type: /test_topic; test_msgs/Primitives; 3 msgs" - "\n /array_topic; test_msgs/StaticArrayPrimitives; 4 msgs")); + "\nFiles: test\\.db3" + "\nBag size: .*B" + "\nStorage id: sqlite3" + "\nDuration: 0\\.155s" + "\nStart: Sep .+ 2018 .+:.+:44\\.241 \\(1537282604\\.241\\)" + "\nEnd Sep .+ 2018 .+:.+:44\\.397 \\(1537282604\\.397\\)" + "\nMessages: 7" + "\nTopics with Type: /test_topic; test_msgs/Primitives; 3 msgs; cdr" + "\n /array_topic; test_msgs/StaticArrayPrimitives; 4 msgs; cdr")); } // TODO(Martin-Idel-SI): Revisit exit code non-zero here, gracefully should be exit code zero diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index 2616d35e2..ebb4154bf 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -25,6 +25,7 @@ struct RecordOptions public: bool all; std::vector topics; + std::string rmw_serialization_format; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/formatter.cpp b/rosbag2_transport/src/rosbag2_transport/formatter.cpp index 4fabf4e38..56b17032e 100644 --- a/rosbag2_transport/src/rosbag2_transport/formatter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/formatter.cpp @@ -113,7 +113,8 @@ void Formatter::format_topics_with_type( size_t number_of_topics = topics.size(); for (size_t j = 0; j < number_of_topics; ++j) { std::string topic_with_type = topics[j].topic_with_type.name + "; " + - topics[j].topic_with_type.type + "; " + std::to_string(topics[j].message_count) + " msgs\n"; + topics[j].topic_with_type.type + "; " + std::to_string(topics[j].message_count) + " msgs; " + + topics[j].topic_with_type.serialization_format + "\n"; if (j == 0) { info_stream << topic_with_type; } else { diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 86aa35cc6..717b28e7f 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -42,7 +42,7 @@ void Recorder::record(const RecordOptions & record_options) auto subscription = create_subscription(topic_name, topic_type); if (subscription) { subscriptions_.push_back(subscription); - writer_->create_topic({topic_name, topic_type}); + writer_->create_topic({topic_name, topic_type, record_options.rmw_serialization_format}); ROSBAG2_TRANSPORT_LOG_INFO_STREAM("Subscribed to topic '" << topic_name << "'"); } } diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp index 6ba44e4b8..7bd401b71 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp @@ -63,7 +63,8 @@ void Rosbag2Transport::record( const StorageOptions & storage_options, const RecordOptions & record_options) { try { - writer_->open(storage_options, rmw_get_serialization_format()); + writer_->open( + storage_options, rmw_get_serialization_format(), record_options.rmw_serialization_format); auto transport_node = setup_node(); @@ -119,9 +120,8 @@ void Rosbag2Transport::print_bag_info(const std::string & uri) formatter->format_file_paths(metadata.relative_file_paths, info_stream, indentation_spaces); info_stream << "Bag size: " << formatter->format_file_size( metadata.bag_size) << std::endl; - info_stream << "Storage id: " << metadata.storage_identifier << std::endl; - info_stream << "Serialization format: " << metadata.serialization_format << std::endl; - info_stream << "Duration: " << formatter->format_duration( + info_stream << "Storage id: " << metadata.storage_identifier << std::endl; + info_stream << "Duration: " << formatter->format_duration( metadata.duration)["time_in_sec"] << "s" << std::endl; info_stream << "Start: " << formatter->format_time_point(start_time) << std::endl; diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp index c1b0b1e55..964a9f9a6 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp @@ -45,7 +45,6 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * storage_options.uri = std::string(uri); storage_options.storage_id = std::string(storage_id); - storage_options.rmw_serialization_format = std::string(encoding).empty() ? "cdr" : encoding; record_options.all = all; if (topics) { @@ -60,6 +59,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * Py_DECREF(topic_iterator); } } + record_options.rmw_serialization_format = std::string(encoding).empty() ? "cdr" : encoding; rosbag2_transport::Rosbag2Transport transport; transport.init(); diff --git a/rosbag2_transport/test/rosbag2_transport/mock_writer.hpp b/rosbag2_transport/test/rosbag2_transport/mock_writer.hpp index 55fca3bb9..96ea2d664 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_writer.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_writer.hpp @@ -27,10 +27,12 @@ class MockWriter : public rosbag2::Writer public: void open( const rosbag2::StorageOptions & options, - const std::string & input_serialization_format) override + const std::string & input_serialization_format, + const std::string & output_serialization_format) override { (void) options; (void) input_serialization_format; + (void) output_serialization_format; } void create_topic(const rosbag2::TopicMetadata & topic_with_type) override diff --git a/rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp b/rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp index 5b2863d4e..ff1750c14 100644 --- a/rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp +++ b/rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp @@ -57,7 +57,7 @@ class Rosbag2TransportTestFixture : public Test { public: Rosbag2TransportTestFixture() - : storage_options_({"uri", "storage_id", "rmw_serialization_format"}), play_options_({1000}), + : storage_options_({"uri", "storage_id"}), play_options_({1000}), reader_(std::make_shared()), writer_(std::make_shared()), info_(std::make_shared()) {} diff --git a/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp b/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp index 9e2451b01..b0a67bef7 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp @@ -74,13 +74,13 @@ TEST_F(FormatterTestFixture, format_files_prints_newline_if_there_are_no_paths) TEST_F(FormatterTestFixture, format_topics_with_type_correctly_layouts_more_topics) { std::vector topics; - topics.push_back({{"topic1", "type1"}, 100}); - topics.push_back({{"topic2", "type2"}, 200}); + topics.push_back({{"topic1", "type1", "rmw1"}, 100}); + topics.push_back({{"topic2", "type2", "rmw2"}, 200}); std::stringstream formatted_output; formatter_->format_topics_with_type(topics, formatted_output, indentation_spaces_); - EXPECT_THAT(formatted_output.str(), Eq("topic1; type1; 100 msgs\n" - " topic2; type2; 200 msgs\n")); + EXPECT_THAT(formatted_output.str(), Eq("topic1; type1; 100 msgs; rmw1\n" + " topic2; type2; 200 msgs; rmw2\n")); } TEST_F(FormatterTestFixture, format_topics_with_type_prints_newline_if_there_are_no_topics) { diff --git a/rosbag2_transport/test/rosbag2_transport/test_info.cpp b/rosbag2_transport/test/rosbag2_transport/test_info.cpp index 2b99f35b1..ba2093515 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_info.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_info.cpp @@ -32,32 +32,30 @@ TEST_F(Rosbag2TransportTestFixture, info_pretty_prints_information_from_bagfile) rosbag2::BagMetadata bagfile; bagfile.storage_identifier = "sqlite3"; - bagfile.serialization_format = "cdr"; bagfile.relative_file_paths.emplace_back("some_relative_path"); bagfile.relative_file_paths.emplace_back("some_other_relative_path"); bagfile.starting_time = std::chrono::time_point( std::chrono::nanoseconds(1538051985348887471)); // corresponds to Sept 27 14:39:45.348 bagfile.duration = std::chrono::nanoseconds(50000000); bagfile.message_count = 50; - bagfile.topics_with_message_count.push_back({{"topic1", "type1"}, 100}); - bagfile.topics_with_message_count.push_back({{"topic2", "type2"}, 200}); + bagfile.topics_with_message_count.push_back({{"topic1", "type1", "rmw1"}, 100}); + bagfile.topics_with_message_count.push_back({{"topic2", "type2", "rmw2"}, 200}); EXPECT_CALL(*info_, read_metadata(_, _)).WillOnce(Return(bagfile)); // the expected output uses a regex to handle different time zones. rosbag2_transport::Rosbag2Transport transport(reader_, writer_, info_); transport.print_bag_info("test"); std::string expected_output( - "\nFiles: some_relative_path\n" - " some_other_relative_path\n" - "Bag size: 0 B\n" - "Storage id: sqlite3\n" - "Serialization format: cdr\n" - "Duration: 0\\.50s\n" - "Start: Sep .+ 2018 .+:.+:45\\.348 \\(1538051985\\.348\\)\n" - "End Sep .+ 2018 .+:.+:45\\.398 \\(1538051985\\.398\\)\n" - "Messages: 50\n" - "Topics with Type: topic1; type1; 100 msgs\n" - " topic2; type2; 200 msgs\n\n"); + "\nFiles: some_relative_path\n" + " some_other_relative_path\n" + "Bag size: 0 B\n" + "Storage id: sqlite3\n" + "Duration: 0\\.50s\n" + "Start: Sep .+ 2018 .+:.+:45\\.348 \\(1538051985\\.348\\)\n" + "End Sep .+ 2018 .+:.+:45\\.398 \\(1538051985\\.398\\)\n" + "Messages: 50\n" + "Topics with Type: topic1; type1; 100 msgs; rmw1\n" + " topic2; type2; 200 msgs; rmw2\n\n"); std::string output = internal::GetCapturedStdout(); EXPECT_THAT(output, ContainsRegex(expected_output)); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index d4bbde7f1..c9774e9ab 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -63,8 +63,8 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {"topic1", "test_msgs/Primitives"}, - {"topic2", "test_msgs/StaticArrayPrimitives"}, + {"topic1", "test_msgs/Primitives", ""}, + {"topic2", "test_msgs/StaticArrayPrimitives", ""}, }; std::vector> messages = diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp index 38c04668f..9d13c7b76 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp @@ -39,7 +39,8 @@ TEST_F(Rosbag2TransportTestFixture, playing_respects_relative_timing_of_stored_m primitive_message2->string_value = "Hello World 2"; auto message_time_difference = std::chrono::seconds(1); - auto topics_and_types = std::vector{{"topic1", "test_msgs/Primitives"}}; + auto topics_and_types = + std::vector{{"topic1", "test_msgs/Primitives", ""}}; std::vector> messages = {serialize_test_message("topic1", 0, primitive_message1), serialize_test_message("topic1", 0, primitive_message2)}; diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index e33f6e159..6b1c13a13 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -42,7 +42,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_man_.add_publisher( array_topic, array_message, 2); - start_recording({false, {string_topic, array_topic}}); + start_recording({false, {string_topic, array_topic}, ""}); run_publishers(); stop_recording(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index 3dca12027..743d4ef35 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -40,7 +40,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_man_.add_publisher(string_topic, string_message, 2); pub_man_.add_publisher(array_topic, array_message, 2); - start_recording({true, {}}); + start_recording({true, {}, ""}); run_publishers(); stop_recording(); From 89778820cad6655282a66dd0cd0a4eb3d3855fc8 Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Wed, 31 Oct 2018 09:53:09 +0100 Subject: [PATCH 04/22] GH-118 Fix tests after rebase --- .../src/rosbag2_transport/rosbag2_transport.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp index 7bd401b71..cf643132a 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp @@ -113,21 +113,21 @@ void Rosbag2Transport::print_bag_info(const std::string & uri) auto end_time = start_time + metadata.duration; auto formatter = std::make_unique(); std::stringstream info_stream; - int indentation_spaces = 22; // The longest info field (Serialization format:) plus one space. + int indentation_spaces = 18; // The longest info field (Topics with Type:) plus one space. info_stream << std::endl; - info_stream << "Files: "; + info_stream << "Files: "; formatter->format_file_paths(metadata.relative_file_paths, info_stream, indentation_spaces); - info_stream << "Bag size: " << formatter->format_file_size( + info_stream << "Bag size: " << formatter->format_file_size( metadata.bag_size) << std::endl; info_stream << "Storage id: " << metadata.storage_identifier << std::endl; info_stream << "Duration: " << formatter->format_duration( metadata.duration)["time_in_sec"] << "s" << std::endl; - info_stream << "Start: " << formatter->format_time_point(start_time) << + info_stream << "Start: " << formatter->format_time_point(start_time) << std::endl; - info_stream << "End " << formatter->format_time_point(end_time) << std::endl; - info_stream << "Messages: " << metadata.message_count << std::endl; - info_stream << "Topics with Type: "; + info_stream << "End " << formatter->format_time_point(end_time) << std::endl; + info_stream << "Messages: " << metadata.message_count << std::endl; + info_stream << "Topics with Type: "; formatter->format_topics_with_type( metadata.topics_with_message_count, info_stream, indentation_spaces); From cb6af774e666e4688d614c167f92dd7778404544 Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Wed, 31 Oct 2018 10:31:05 +0100 Subject: [PATCH 05/22] GH-118 Fix MockMetadataIO and use it in test_writer --- rosbag2/test/rosbag2/mock_metadata_io.hpp | 3 ++- rosbag2/test/rosbag2/test_writer.cpp | 8 +++++++- rosbag2_storage/include/rosbag2_storage/metadata_io.hpp | 8 +++++--- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/rosbag2/test/rosbag2/mock_metadata_io.hpp b/rosbag2/test/rosbag2/mock_metadata_io.hpp index 0c473cda4..fa7866377 100644 --- a/rosbag2/test/rosbag2/mock_metadata_io.hpp +++ b/rosbag2/test/rosbag2/mock_metadata_io.hpp @@ -27,8 +27,9 @@ class MockMetadataIo : public rosbag2_storage::MetadataIo { public: - MOCK_METHOD2(write_metadata, void(const std::string &, rosbag2_storage::BagMetadata)); + MOCK_METHOD2(write_metadata, void(const std::string &, const rosbag2_storage::BagMetadata &)); MOCK_METHOD1(read_metadata, rosbag2_storage::BagMetadata(const std::string &)); + MOCK_METHOD1(metadata_file_exists, bool(const std::string &)); }; #endif // ROSBAG2__MOCK_METADATA_IO_HPP_ diff --git a/rosbag2/test/rosbag2/test_writer.cpp b/rosbag2/test/rosbag2/test_writer.cpp index aace2fe88..3e2ad7377 100644 --- a/rosbag2/test/rosbag2/test_writer.cpp +++ b/rosbag2/test/rosbag2/test_writer.cpp @@ -25,6 +25,7 @@ #include "mock_converter.hpp" #include "mock_converter_factory.hpp" +#include "mock_metadata_io.hpp" #include "mock_storage.hpp" #include "mock_storage_factory.hpp" @@ -38,16 +39,19 @@ class WriterTest : public Test storage_factory_ = std::make_unique>(); storage_ = std::make_shared>(); converter_factory_ = std::make_shared>(); + metadata_io_ = std::make_unique>(); EXPECT_CALL(*storage_factory_, open_read_write(_, _)).WillOnce(Return(storage_)); + EXPECT_CALL(*metadata_io_, write_metadata(_, _)).Times(1); writer_ = std::make_unique( - std::move(storage_factory_), converter_factory_); + std::move(storage_factory_), converter_factory_, std::move(metadata_io_)); } std::unique_ptr> storage_factory_; std::shared_ptr> storage_; std::shared_ptr> converter_factory_; + std::unique_ptr metadata_io_; std::unique_ptr writer_; }; @@ -69,6 +73,7 @@ TEST_F(WriterTest, auto message = std::make_shared(); message->topic_name = "test_topic"; rosbag2::StorageOptions options; + options.uri = "uri"; writer_->open(options, input_format, storage_serialization_format); writer_->create_topic({"test_topic", "test_msgs/Primitives", ""}); writer_->write(message); @@ -82,6 +87,7 @@ TEST_F(WriterTest, write_does_not_use_converters_if_input_and_output_format_are_ auto message = std::make_shared(); message->topic_name = "test_topic"; rosbag2::StorageOptions options; + options.uri = "uri"; writer_->open(options, storage_serialization_format, storage_serialization_format); writer_->create_topic({"test_topic", "test_msgs/Primitives", ""}); writer_->write(message); diff --git a/rosbag2_storage/include/rosbag2_storage/metadata_io.hpp b/rosbag2_storage/include/rosbag2_storage/metadata_io.hpp index 99c43618a..d29db80a3 100644 --- a/rosbag2_storage/include/rosbag2_storage/metadata_io.hpp +++ b/rosbag2_storage/include/rosbag2_storage/metadata_io.hpp @@ -30,14 +30,16 @@ class MetadataIo public: static constexpr const char * const metadata_filename = "metadata.yaml"; + virtual ~MetadataIo() = default; + ROSBAG2_STORAGE_PUBLIC - void write_metadata(const std::string & uri, const BagMetadata & metadata); + virtual void write_metadata(const std::string & uri, const BagMetadata & metadata); ROSBAG2_STORAGE_PUBLIC - BagMetadata read_metadata(const std::string & uri); + virtual BagMetadata read_metadata(const std::string & uri); ROSBAG2_STORAGE_PUBLIC - bool metadata_file_exists(const std::string & uri); + virtual bool metadata_file_exists(const std::string & uri); private: std::string get_metadata_file_name(const std::string & uri); From afe2a728ac933b8138f57a5b653f2b9d2721354a Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 31 Oct 2018 11:13:38 +0100 Subject: [PATCH 06/22] GH-118 Fix Windows build and minor refactoring --- rosbag2/include/rosbag2/writer.hpp | 4 ++-- rosbag2/src/rosbag2/converter.cpp | 5 ++--- rosbag2/src/rosbag2/types/ros2_message.cpp | 1 - rosbag2/src/rosbag2/writer.cpp | 4 ++-- .../include/rosbag2_storage/metadata_io.hpp | 2 -- rosbag2_storage/src/rosbag2_storage/metadata_io.cpp | 1 + .../rosbag2_tests/test_rosbag2_record_end_to_end.cpp | 1 - .../include/rosbag2_transport/rosbag2_transport.hpp | 10 +++++++--- rosbag2_transport/src/rosbag2_transport/player.cpp | 3 ++- rosbag2_transport/src/rosbag2_transport/player.hpp | 6 +++++- rosbag2_transport/src/rosbag2_transport/recorder.cpp | 3 +++ rosbag2_transport/src/rosbag2_transport/recorder.hpp | 10 +++++++--- .../src/rosbag2_transport/rosbag2_transport.cpp | 8 +++++++- .../src/rosbag2_transport/rosbag2_transport_python.cpp | 2 +- 14 files changed, 39 insertions(+), 21 deletions(-) diff --git a/rosbag2/include/rosbag2/writer.hpp b/rosbag2/include/rosbag2/writer.hpp index 8444f5b00..91aa1572a 100644 --- a/rosbag2/include/rosbag2/writer.hpp +++ b/rosbag2/include/rosbag2/writer.hpp @@ -61,8 +61,8 @@ class ROSBAG2_PUBLIC Writer * This must be called before any other function is used. * * \param options Options to configure the storage - * \param input_serialization_format The serialization format corresponding to the currently - * used rmw + * \param input_serialization_format The serialization format of the messages to write + * \param output_serialization_format The serialization format in which the messages must be saved */ virtual void open( const StorageOptions & options, diff --git a/rosbag2/src/rosbag2/converter.cpp b/rosbag2/src/rosbag2/converter.cpp index bc6366b87..41b265a2c 100644 --- a/rosbag2/src/rosbag2/converter.cpp +++ b/rosbag2/src/rosbag2/converter.cpp @@ -60,9 +60,8 @@ Converter::~Converter() std::shared_ptr Converter::convert( std::shared_ptr message) { - auto ts = topics_and_types_[message->topic_name].type_support; - auto introspection_ts = topics_and_types_[message->topic_name].introspection_type_support; - + auto ts = topics_and_types_.at(message->topic_name).type_support; + auto introspection_ts = topics_and_types_.at(message->topic_name).introspection_type_support; auto allocator = rcutils_get_default_allocator(); std::shared_ptr allocated_ros_message = allocate_ros2_message(introspection_ts, &allocator); diff --git a/rosbag2/src/rosbag2/types/ros2_message.cpp b/rosbag2/src/rosbag2/types/ros2_message.cpp index eb31b8647..588734cd3 100644 --- a/rosbag2/src/rosbag2/types/ros2_message.cpp +++ b/rosbag2/src/rosbag2/types/ros2_message.cpp @@ -14,7 +14,6 @@ #include "rosbag2/types/ros2_message.hpp" -#include #include #include #include diff --git a/rosbag2/src/rosbag2/writer.cpp b/rosbag2/src/rosbag2/writer.cpp index 4153d19f1..82f5b29fc 100644 --- a/rosbag2/src/rosbag2/writer.cpp +++ b/rosbag2/src/rosbag2/writer.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include "rosbag2/info.hpp" #include "rosbag2/storage_options.hpp" @@ -41,7 +40,8 @@ Writer::~Writer() metadata_io_->write_metadata(uri_, storage_->get_metadata()); } - storage_.reset(); // Necessary to ensure that the writer is destroyed before the factory + storage_.reset(); // Necessary to ensure that the storage is destroyed before the factory + storage_factory_.reset(); } void Writer::open( diff --git a/rosbag2_storage/include/rosbag2_storage/metadata_io.hpp b/rosbag2_storage/include/rosbag2_storage/metadata_io.hpp index d29db80a3..9908a3412 100644 --- a/rosbag2_storage/include/rosbag2_storage/metadata_io.hpp +++ b/rosbag2_storage/include/rosbag2_storage/metadata_io.hpp @@ -18,8 +18,6 @@ #include #include "rosbag2_storage/bag_metadata.hpp" -#include "rosbag2_storage/filesystem_helper.hpp" -#include "rosbag2_storage/topic_with_type.hpp" #include "rosbag2_storage/visibility_control.hpp" namespace rosbag2_storage diff --git a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp index 6f440d3ad..d781b31a6 100644 --- a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp +++ b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp @@ -18,6 +18,7 @@ #include #include +#include "rosbag2_storage/topic_with_type.hpp" #include "rosbag2_storage/filesystem_helper.hpp" #ifdef _WIN32 diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp index 6c81a3f75..2c8fbe857 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp @@ -48,7 +48,6 @@ TEST_F(RecordFixture, record_end_to_end_test) { rosbag2_storage::BagMetadata metadata{}; metadata.version = 1; metadata.storage_identifier = "sqlite3"; - metadata.serialization_format = "cdr"; metadata.relative_file_paths.emplace_back("bag.db3"); metadata.duration = std::chrono::nanoseconds(0); metadata.starting_time = diff --git a/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp b/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp index 651e6a9d2..9e54ee8fd 100644 --- a/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp +++ b/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp @@ -21,14 +21,18 @@ #include #include -#include "rosbag2/info.hpp" -#include "rosbag2/sequential_reader.hpp" -#include "rosbag2/writer.hpp" #include "rosbag2_transport/play_options.hpp" #include "rosbag2_transport/record_options.hpp" #include "rosbag2_transport/storage_options.hpp" #include "rosbag2_transport/visibility_control.hpp" +namespace rosbag2 +{ +class Info; +class SequentialReader; +class Writer; +} // namespace rosbag2 + namespace rosbag2_transport { diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 7ea8bb87f..ad13b3339 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -26,10 +26,11 @@ #include "rcl/graph.h" #include "rcutils/time.h" +#include "rosbag2/sequential_reader.hpp" +#include "rosbag2/typesupport_helpers.hpp" #include "rosbag2_transport/logging.hpp" #include "rosbag2_node.hpp" #include "replayable_message.hpp" -#include "rosbag2/typesupport_helpers.hpp" namespace rosbag2_transport { diff --git a/rosbag2_transport/src/rosbag2_transport/player.hpp b/rosbag2_transport/src/rosbag2_transport/player.hpp index fd3e4031f..e6d1fc872 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player.hpp @@ -24,12 +24,16 @@ #include "moodycamel/readerwriterqueue.h" #include "replayable_message.hpp" -#include "rosbag2/sequential_reader.hpp" #include "rosbag2/types.hpp" #include "rosbag2_transport/play_options.hpp" using TimePoint = std::chrono::time_point; +namespace rosbag2 +{ +class SequentialReader; +} // namespace rosbag2 + namespace rosbag2_transport { diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 717b28e7f..d747f05b6 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -18,7 +18,10 @@ #include #include +#include "rosbag2/writer.hpp" #include "rosbag2_transport/logging.hpp" +#include "generic_subscription.hpp" +#include "rosbag2_node.hpp" namespace rosbag2_transport { diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.hpp b/rosbag2_transport/src/rosbag2_transport/recorder.hpp index 19f4c37d0..afba018b3 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.hpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.hpp @@ -19,15 +19,19 @@ #include #include -#include "rosbag2/writer.hpp" #include "rosbag2_transport/record_options.hpp" -#include "generic_subscription.hpp" -#include "rosbag2_node.hpp" +namespace rosbag2 +{ +class Writer; +} namespace rosbag2_transport { +class GenericSubscription; +class Rosbag2Node; + class Recorder { public: diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp index cf643132a..9b53f40d6 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp @@ -25,6 +25,7 @@ #include "rcutils/time.h" #include "rosbag2_transport/logging.hpp" +#include "rosbag2/info.hpp" #include "rosbag2/sequential_reader.hpp" #include "rosbag2/types.hpp" #include "rosbag2/typesupport_helpers.hpp" @@ -33,6 +34,7 @@ #include "formatter.hpp" #include "player.hpp" #include "recorder.hpp" +#include "rosbag2_node.hpp" namespace rosbag2_transport { @@ -64,7 +66,11 @@ void Rosbag2Transport::record( { try { writer_->open( - storage_options, rmw_get_serialization_format(), record_options.rmw_serialization_format); + storage_options, + rmw_get_serialization_format(), + record_options.rmw_serialization_format.empty() ? + rmw_get_serialization_format() : + record_options.rmw_serialization_format); auto transport_node = setup_node(); diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp index 964a9f9a6..89abcfaf5 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp @@ -59,7 +59,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * Py_DECREF(topic_iterator); } } - record_options.rmw_serialization_format = std::string(encoding).empty() ? "cdr" : encoding; + record_options.rmw_serialization_format = encoding; rosbag2_transport::Rosbag2Transport transport; transport.init(); From 02bf9bde219299fc43878f81d948b3aaccb75f8f Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Wed, 31 Oct 2018 11:14:11 +0100 Subject: [PATCH 07/22] GH-118 Add test for writer to check that error is thrown if converter plugin does not exist --- rosbag2/test/rosbag2/test_writer.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/rosbag2/test/rosbag2/test_writer.cpp b/rosbag2/test/rosbag2/test_writer.cpp index 3e2ad7377..f7f47ad23 100644 --- a/rosbag2/test/rosbag2/test_writer.cpp +++ b/rosbag2/test/rosbag2/test_writer.cpp @@ -40,6 +40,7 @@ class WriterTest : public Test storage_ = std::make_shared>(); converter_factory_ = std::make_shared>(); metadata_io_ = std::make_unique>(); + storage_options_.uri = "uri"; EXPECT_CALL(*storage_factory_, open_read_write(_, _)).WillOnce(Return(storage_)); EXPECT_CALL(*metadata_io_, write_metadata(_, _)).Times(1); @@ -53,6 +54,7 @@ class WriterTest : public Test std::shared_ptr> converter_factory_; std::unique_ptr metadata_io_; std::unique_ptr writer_; + rosbag2::StorageOptions storage_options_; }; TEST_F(WriterTest, @@ -72,9 +74,7 @@ TEST_F(WriterTest, auto message = std::make_shared(); message->topic_name = "test_topic"; - rosbag2::StorageOptions options; - options.uri = "uri"; - writer_->open(options, input_format, storage_serialization_format); + writer_->open(storage_options_, input_format, storage_serialization_format); writer_->create_topic({"test_topic", "test_msgs/Primitives", ""}); writer_->write(message); } @@ -86,9 +86,20 @@ TEST_F(WriterTest, write_does_not_use_converters_if_input_and_output_format_are_ auto message = std::make_shared(); message->topic_name = "test_topic"; - rosbag2::StorageOptions options; - options.uri = "uri"; - writer_->open(options, storage_serialization_format, storage_serialization_format); + writer_->open(storage_options_, storage_serialization_format, storage_serialization_format); writer_->create_topic({"test_topic", "test_msgs/Primitives", ""}); writer_->write(message); } + +TEST_F(WriterTest, open_throws_error_if_converter_plugin_does_not_exist) { + std::string input_format = "rmw1_format"; + std::string output_format = "rmw2_format"; + + auto format1_converter = std::make_unique>(); + EXPECT_CALL(*converter_factory_, load_converter(input_format)) + .WillOnce(Return(ByMove(std::move(format1_converter)))); + EXPECT_CALL(*converter_factory_, load_converter(output_format)) + .WillOnce(Return(ByMove(nullptr))); + + EXPECT_ANY_THROW(writer_->open(storage_options_, input_format, output_format)); +} From dc90aebe50860d8ea7e230851b2e197c61b9a0b5 Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Wed, 31 Oct 2018 11:34:37 +0100 Subject: [PATCH 08/22] GH-118 Add test to check that metadat_io_ writes metadata file in writer's destructor --- rosbag2/test/rosbag2/test_writer.cpp | 29 ++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/rosbag2/test/rosbag2/test_writer.cpp b/rosbag2/test/rosbag2/test_writer.cpp index f7f47ad23..1fd386a08 100644 --- a/rosbag2/test/rosbag2/test_writer.cpp +++ b/rosbag2/test/rosbag2/test_writer.cpp @@ -39,14 +39,11 @@ class WriterTest : public Test storage_factory_ = std::make_unique>(); storage_ = std::make_shared>(); converter_factory_ = std::make_shared>(); - metadata_io_ = std::make_unique>(); + metadata_io_ = std::make_unique>(); storage_options_.uri = "uri"; - EXPECT_CALL(*storage_factory_, open_read_write(_, _)).WillOnce(Return(storage_)); - EXPECT_CALL(*metadata_io_, write_metadata(_, _)).Times(1); - - writer_ = std::make_unique( - std::move(storage_factory_), converter_factory_, std::move(metadata_io_)); + EXPECT_CALL( + *storage_factory_, open_read_write(_, _)).Times(AtLeast(0)).WillRepeatedly(Return(storage_)); } std::unique_ptr> storage_factory_; @@ -59,6 +56,9 @@ class WriterTest : public Test TEST_F(WriterTest, write_uses_converters_to_convert_serialization_format_if_input_and_output_format_are_different) { + writer_ = std::make_unique( + std::move(storage_factory_), converter_factory_, std::move(metadata_io_)); + std::string storage_serialization_format = "rmw1_format"; std::string input_format = "rmw2_format"; @@ -80,6 +80,9 @@ TEST_F(WriterTest, } TEST_F(WriterTest, write_does_not_use_converters_if_input_and_output_format_are_equal) { + writer_ = std::make_unique( + std::move(storage_factory_), converter_factory_, std::move(metadata_io_)); + std::string storage_serialization_format = "rmw_format"; EXPECT_CALL(*converter_factory_, load_converter(storage_serialization_format)).Times(0); @@ -91,7 +94,21 @@ TEST_F(WriterTest, write_does_not_use_converters_if_input_and_output_format_are_ writer_->write(message); } +TEST_F(WriterTest, metadata_io_writes_metadata_file_in_destructor) { + EXPECT_CALL(*metadata_io_, write_metadata(_, _)).Times(1); + writer_ = std::make_unique( + std::move(storage_factory_), converter_factory_, std::move(metadata_io_)); + + std::string rmw_format = "rmw_format"; + + writer_->open(storage_options_, rmw_format, rmw_format); + writer_.reset(); +} + TEST_F(WriterTest, open_throws_error_if_converter_plugin_does_not_exist) { + writer_ = std::make_unique( + std::move(storage_factory_), converter_factory_, std::move(metadata_io_)); + std::string input_format = "rmw1_format"; std::string output_format = "rmw2_format"; From 8d2d311cc42835939f069d4b4904712d44735e88 Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Wed, 31 Oct 2018 11:35:50 +0100 Subject: [PATCH 09/22] GH-118 Build Converter before opening the database in Writer::open() - This assures that if one of the converter plugins does not exist, the database is not created --- rosbag2/src/rosbag2/writer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rosbag2/src/rosbag2/writer.cpp b/rosbag2/src/rosbag2/writer.cpp index 82f5b29fc..e41e0674c 100644 --- a/rosbag2/src/rosbag2/writer.cpp +++ b/rosbag2/src/rosbag2/writer.cpp @@ -49,18 +49,18 @@ void Writer::open( const std::string & input_serialization_format, const std::string & output_serialization_format) { - storage_ = storage_factory_->open_read_write(options.uri, options.storage_id); - if (!storage_) { - throw std::runtime_error("No storage could be initialized. Abort"); - } - uri_ = options.uri; - if (output_serialization_format != input_serialization_format) { converter_ = std::make_unique( input_serialization_format, output_serialization_format, converter_factory_); } + + storage_ = storage_factory_->open_read_write(options.uri, options.storage_id); + if (!storage_) { + throw std::runtime_error("No storage could be initialized. Abort"); + } + uri_ = options.uri; } void Writer::create_topic(const TopicMetadata & topic_with_type) From 84baffa215745c7d556a9a528be5e5d79935c58d Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Wed, 31 Oct 2018 11:55:12 +0100 Subject: [PATCH 10/22] GH-118 Add end-to-end tests to check graceful failure if converter plugins do not exists - Both a test for record and play has been added --- .../{test/test.db3 => cdr_test/cdr_test.db3} | Bin .../cdr_test.db3-shm} | Bin .../cdr_test.db3-wal} | 0 .../{test => cdr_test}/metadata.yaml | 2 +- rosbag2_tests/resources/cdr_test/test.db3 | Bin 0 -> 12288 bytes rosbag2_tests/resources/cdr_test/test.db3-shm | Bin 0 -> 32768 bytes rosbag2_tests/resources/cdr_test/test.db3-wal | 0 .../resources/wrong_rmw_test/metadata.yaml | 21 ++++++++++++++++++ .../wrong_rmw_test/wrong_rmw_test.db3 | Bin 0 -> 12288 bytes .../wrong_rmw_test/wrong_rmw_test.db3-shm | Bin 0 -> 32768 bytes .../wrong_rmw_test/wrong_rmw_test.db3-wal | 0 .../test_rosbag2_info_end_to_end.cpp | 4 ++-- .../test_rosbag2_play_end_to_end.cpp | 13 ++++++++++- .../test_rosbag2_record_end_to_end.cpp | 15 +++++++++++-- 14 files changed, 49 insertions(+), 6 deletions(-) rename rosbag2_tests/resources/{test/test.db3 => cdr_test/cdr_test.db3} (100%) rename rosbag2_tests/resources/{test/test.db3-shm => cdr_test/cdr_test.db3-shm} (100%) rename rosbag2_tests/resources/{test/test.db3-wal => cdr_test/cdr_test.db3-wal} (100%) rename rosbag2_tests/resources/{test => cdr_test}/metadata.yaml (96%) create mode 100644 rosbag2_tests/resources/cdr_test/test.db3 create mode 100644 rosbag2_tests/resources/cdr_test/test.db3-shm create mode 100644 rosbag2_tests/resources/cdr_test/test.db3-wal create mode 100644 rosbag2_tests/resources/wrong_rmw_test/metadata.yaml create mode 100644 rosbag2_tests/resources/wrong_rmw_test/wrong_rmw_test.db3 create mode 100644 rosbag2_tests/resources/wrong_rmw_test/wrong_rmw_test.db3-shm create mode 100644 rosbag2_tests/resources/wrong_rmw_test/wrong_rmw_test.db3-wal diff --git a/rosbag2_tests/resources/test/test.db3 b/rosbag2_tests/resources/cdr_test/cdr_test.db3 similarity index 100% rename from rosbag2_tests/resources/test/test.db3 rename to rosbag2_tests/resources/cdr_test/cdr_test.db3 diff --git a/rosbag2_tests/resources/test/test.db3-shm b/rosbag2_tests/resources/cdr_test/cdr_test.db3-shm similarity index 100% rename from rosbag2_tests/resources/test/test.db3-shm rename to rosbag2_tests/resources/cdr_test/cdr_test.db3-shm diff --git a/rosbag2_tests/resources/test/test.db3-wal b/rosbag2_tests/resources/cdr_test/cdr_test.db3-wal similarity index 100% rename from rosbag2_tests/resources/test/test.db3-wal rename to rosbag2_tests/resources/cdr_test/cdr_test.db3-wal diff --git a/rosbag2_tests/resources/test/metadata.yaml b/rosbag2_tests/resources/cdr_test/metadata.yaml similarity index 96% rename from rosbag2_tests/resources/test/metadata.yaml rename to rosbag2_tests/resources/cdr_test/metadata.yaml index 3b860c9a3..c593f27bd 100644 --- a/rosbag2_tests/resources/test/metadata.yaml +++ b/rosbag2_tests/resources/cdr_test/metadata.yaml @@ -2,7 +2,7 @@ rosbag2_bagfile_information: version: 1 storage_identifier: sqlite3 relative_file_paths: - - test.db3 + - cdr_test.db3 duration: nanoseconds: 155979811 starting_time: diff --git a/rosbag2_tests/resources/cdr_test/test.db3 b/rosbag2_tests/resources/cdr_test/test.db3 new file mode 100644 index 0000000000000000000000000000000000000000..278b1e2cc7169b29fdbb8604ecf7282035e0b7d8 GIT binary patch literal 12288 zcmeI2&rcIU6vy9gyRAmrHE9fTAdV2CB*cw0n2?Y_OEyTTfTcw7Kr?IyT-oh5yPHOg z5jOscHxDLWym~S5$jJ-;fc^*k2_Ep`%{sGf*@_ZO2$$x)X6L>4oj2PzpY}4FUAkK^ zeMZ(D*QP$n!bOl}m?8uKVy%n2&_o_@#llG)@o`}cZb%PO>N}9rbx7CKzm6+J9SI-- zB!C2v01`j~NB{{S0VIF~x{*MjNXg#5J}F>6U9*_))J(%`j`DO-&y{pi%H<0>X=>Na z3YncR=`(tfEEZ?)&!DMF2C*2;FuN` z9$6CAU5hI)IF>n~ir?*4OaW~sqt&R(pLqDpEwehS`krf8WwXljb!sqATWnA3VwSJF zDl5>J&1i<@7@Mx+ZMd}du-W*uka$UocbdHL2GlF*Z>jI84?SOc9`O_n2_OL^fCP{L z5mm5A76u=}&?h{ZdIO>DmZe0pNB gh}gvX?u)J?CU+1UpE^)aPfUu4jgNkP-EG8v1F_-B7ytkO literal 0 HcmV?d00001 diff --git a/rosbag2_tests/resources/cdr_test/test.db3-shm b/rosbag2_tests/resources/cdr_test/test.db3-shm new file mode 100644 index 0000000000000000000000000000000000000000..fe9ac2845eca6fe6da8a63cd096d9cf9e24ece10 GIT binary patch literal 32768 zcmeIuAr62r3oUTKHc`A*F=yk;v;7xi38C#77zpp&L{ z#at(|^Cf*oFOtRL?Co4}h0N(IBUNfMQqu31$ovAomJ5Xuf2;PdL_Fr2)G{Aa-*l?w z)>c=f4F`R4vVU+;3bw*JZ032i!o0SR{Z&g_J+7Lt)^bOwQBfw(^{HJuSjE8*S*JcF z`NBf}U=73kjaveacKUFJQ|c3ltG%G!RD0Dg>YHx-x8a(Q01`j~NB{{S0VIF~kN^@u z0!RP}942s8R$yQ(qZyWCY`TtDacS*-+4d@)HZ7!wu2ZeFP7x+01u}CPjk+$~Du)LE zKHup#XD|8uz`@9gp+(oUP2a3DuQQuJ`@+z8pBAQw3IU_^I2`WD%k0@_SQ zt5KJqh4{=ZvpTE#o@-cTv&!QQYA{b*Y!B;Vw6lg3FCuyFHK>=<-%>wPA9_CbJm4W3 z5<(F&ur{6z{?`tLgYXEFV4}O~kII|bU zqW6<)!lXUe-VyVmu?;)C72VJp4X%LDgc|807A7=qVvew#u;ootj^?Xen|AD)#U7Dc z%(9#t0Ee{EBeXGqg+v0RM9>o#3(UUQzw}=p5VtF7d4+qiClX<>szk)H!S1iFBNp!< oHo5u!)5(d&B4U#pyU)9hnA|~ZVrpMKIWZ|BHZl73RksoQ10Vy>?EnA( literal 0 HcmV?d00001 diff --git a/rosbag2_tests/resources/wrong_rmw_test/wrong_rmw_test.db3-shm b/rosbag2_tests/resources/wrong_rmw_test/wrong_rmw_test.db3-shm new file mode 100644 index 0000000000000000000000000000000000000000..fe9ac2845eca6fe6da8a63cd096d9cf9e24ece10 GIT binary patch literal 32768 zcmeIuAr62r3spin_subscriptions(); - auto exit_code = execute_and_wait_until_completion("ros2 bag play test", database_path_); + auto exit_code = execute_and_wait_until_completion("ros2 bag play cdr_test", database_path_); subscription_future.get(); @@ -96,3 +96,14 @@ TEST_F(PlayEndToEndTestFixture, play_fails_gracefully_if_bag_does_not_exist) { EXPECT_THAT(exit_code, Eq(EXIT_FAILURE)); EXPECT_THAT(error_output, HasSubstr("'does_not_exist' does not exist")); } + +TEST_F(PlayEndToEndTestFixture, play_fails_gracefully_if_needed_coverter_plugin_does_not_exist) { + internal::CaptureStderr(); + auto exit_code = + execute_and_wait_until_completion("ros2 bag play wrong_rmw_test", database_path_); + auto error_output = internal::GetCapturedStderr(); + + EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); + EXPECT_THAT( + error_output, HasSubstr("Requested converter id 'wrong_format_converter' does not exist")); +} diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp index 2c8fbe857..d5e38ce76 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp @@ -71,11 +71,11 @@ TEST_F(RecordFixture, record_fails_gracefully_if_bag_already_exists) { internal::CaptureStderr(); auto exit_code = - execute_and_wait_until_completion("ros2 bag record --output test -a", database_path); + execute_and_wait_until_completion("ros2 bag record --output cdr_test -a", database_path); auto error_output = internal::GetCapturedStderr(); EXPECT_THAT(exit_code, Eq(EXIT_FAILURE)); - EXPECT_THAT(error_output, HasSubstr("Output folder 'test' already exists")); + EXPECT_THAT(error_output, HasSubstr("Output folder 'cdr_test' already exists")); } TEST_F(RecordFixture, record_fails_if_both_all_and_topic_list_is_specified) { @@ -87,3 +87,14 @@ TEST_F(RecordFixture, record_fails_if_both_all_and_topic_list_is_specified) { EXPECT_THAT(exit_code, Eq(EXIT_FAILURE)); EXPECT_THAT(error_output, HasSubstr("Can not specify topics and -a at the same time.")); } + +TEST_F(RecordFixture, record_fails_gracefully_if_plugin_for_given_encoding_does_not_exist) { + internal::CaptureStderr(); + auto exit_code = + execute_and_wait_until_completion("ros2 bag record -a -e some_rmw", temporary_dir_path_); + auto error_output = internal::GetCapturedStderr(); + + EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); + EXPECT_THAT( + error_output, HasSubstr("Requested converter id 'some_rmw_converter' does not exist")); +} From b469fc7f4e399f9004acd0b428b908a1cf7ee2ee Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Wed, 31 Oct 2018 17:25:13 +0100 Subject: [PATCH 11/22] GH-118 Rename 'encoding' CLI option to 'serialization_format' --- ros2bag/ros2bag/verb/record.py | 11 ++++++++--- .../rosbag2_tests/test_rosbag2_record_end_to_end.cpp | 2 +- .../rosbag2_transport/rosbag2_transport_python.cpp | 9 +++++---- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 35bd6f9bf..4d9138bd3 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -43,7 +43,7 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '-s', '--storage', default='sqlite3', help='storage identifier to be used, defaults to "sqlite3"') parser.add_argument( - '-e', '--encoding', default='', + '-f', '--serialization-format', default='', help='rmw serialization format in which the messages are saved, defaults to the' ' rmw currently in use') @@ -66,10 +66,15 @@ def main(self, *, args): # noqa: D102 if args.all: rosbag2_transport_py.record( - uri=uri, storage_id=args.storage, encoding=args.encoding, all=True) + uri=uri, + storage_id=args.storage, + serialization_format=args.serialization_format, + all=True) elif args.topics and len(args.topics) > 0: rosbag2_transport_py.record( - uri=uri, storage_id=args.storage, encoding=args.encoding, topics=args.topics) + uri=uri, storage_id=args.storage, + serialization_format=args.serialization_format, + topics=args.topics) else: self._subparser.print_help() diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp index d5e38ce76..fa1f3e85a 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp @@ -91,7 +91,7 @@ TEST_F(RecordFixture, record_fails_if_both_all_and_topic_list_is_specified) { TEST_F(RecordFixture, record_fails_gracefully_if_plugin_for_given_encoding_does_not_exist) { internal::CaptureStderr(); auto exit_code = - execute_and_wait_until_completion("ros2 bag record -a -e some_rmw", temporary_dir_path_); + execute_and_wait_until_completion("ros2 bag record -a -f some_rmw", temporary_dir_path_); auto error_output = internal::GetCapturedStderr(); EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp index 89abcfaf5..cd4ee5cfd 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp @@ -26,17 +26,18 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * rosbag2_transport::StorageOptions storage_options{}; rosbag2_transport::RecordOptions record_options{}; - static const char * kwlist[] = {"uri", "storage_id", "encoding", "all", "topics", nullptr}; + static const char * kwlist[] = { + "uri", "storage_id", "serialization_format", "all", "topics", nullptr}; char * uri = nullptr; char * storage_id = nullptr; - char * encoding = nullptr; + char * serilization_format = nullptr; bool all = false; PyObject * topics = nullptr; if (!PyArg_ParseTupleAndKeywords(args, kwargs, "sss|bO", const_cast(kwlist), &uri, &storage_id, - &encoding, + &serilization_format, &all, &topics)) { @@ -59,7 +60,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * Py_DECREF(topic_iterator); } } - record_options.rmw_serialization_format = encoding; + record_options.rmw_serialization_format = serilization_format; rosbag2_transport::Rosbag2Transport transport; transport.init(); From 91e7eb8e329dd3f050b3eeaa02e8426beb8b7630 Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Tue, 13 Nov 2018 17:14:59 +0100 Subject: [PATCH 12/22] GH-127 Write serialization format in database also when it's not specified at CLI level - Tests to check that the serialization format is written in the database have also been added. --- rosbag2_tests/test/rosbag2_tests/record_fixture.hpp | 10 ++++++++++ .../rosbag2_tests/test_rosbag2_record_end_to_end.cpp | 1 + rosbag2_transport/CMakeLists.txt | 2 ++ rosbag2_transport/package.xml | 1 + .../src/rosbag2_transport/rosbag2_transport.cpp | 6 +----- .../src/rosbag2_transport/rosbag2_transport_python.cpp | 5 ++++- .../test/rosbag2_transport/test_record.cpp | 6 +++++- 7 files changed, 24 insertions(+), 7 deletions(-) diff --git a/rosbag2_tests/test/rosbag2_tests/record_fixture.hpp b/rosbag2_tests/test/rosbag2_tests/record_fixture.hpp index 589e4587d..fe0fb1cd0 100644 --- a/rosbag2_tests/test/rosbag2_tests/record_fixture.hpp +++ b/rosbag2_tests/test/rosbag2_tests/record_fixture.hpp @@ -128,6 +128,16 @@ class RecordFixture : public TemporaryDirectoryFixture return table_msgs; } + std::string get_rwm_format_for_topic( + const std::string & topic_name, rosbag2_storage_plugins::SqliteWrapper & db) + { + auto topic_format = db.prepare_statement( + "SELECT serialization_format " + "FROM topics " + "WHERE name = ?;")->bind(topic_name)->execute_query().get_single_line(); + return std::get<0>(topic_format); + } + std::string bag_path_; std::string database_path_; PublisherManager pub_man_; diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp index fa1f3e85a..3f1a40c48 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp @@ -61,6 +61,7 @@ TEST_F(RecordFixture, record_end_to_end_test) { EXPECT_THAT(test_topic_messages, SizeIs(Ge(expected_test_messages))); EXPECT_THAT(test_topic_messages, Each(Pointee(Field(&test_msgs::msg::Primitives::string_value, "test")))); + EXPECT_THAT(get_rwm_format_for_topic("/test_topic", db), Eq(rmw_get_serialization_format())); auto wrong_topic_messages = get_messages_for_topic("/wrong_topic"); EXPECT_THAT(wrong_topic_messages, IsEmpty()); diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index 53c5db302..e49c0da20 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) find_package(rosbag2 REQUIRED) +find_package(rmw REQUIRED) find_package(shared_queues_vendor REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -41,6 +42,7 @@ ament_target_dependencies(${PROJECT_NAME} rcutils rosbag2 rosidl_generator_cpp + rmw shared_queues_vendor ) diff --git a/rosbag2_transport/package.xml b/rosbag2_transport/package.xml index be16362d4..c6cd60bca 100644 --- a/rosbag2_transport/package.xml +++ b/rosbag2_transport/package.xml @@ -11,6 +11,7 @@ rclcpp rosbag2 + rmw shared_queues_vendor ament_cmake_gmock diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp index 9b53f40d6..3a3cd4699 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp @@ -66,11 +66,7 @@ void Rosbag2Transport::record( { try { writer_->open( - storage_options, - rmw_get_serialization_format(), - record_options.rmw_serialization_format.empty() ? - rmw_get_serialization_format() : - record_options.rmw_serialization_format); + storage_options, rmw_get_serialization_format(), record_options.rmw_serialization_format); auto transport_node = setup_node(); diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp index cd4ee5cfd..05c6b1ab2 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp @@ -19,6 +19,7 @@ #include "rosbag2_transport/rosbag2_transport.hpp" #include "rosbag2_transport/record_options.hpp" #include "rosbag2_transport/storage_options.hpp" +#include "rmw/rmw.h" static PyObject * rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * kwargs) @@ -60,7 +61,9 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * Py_DECREF(topic_iterator); } } - record_options.rmw_serialization_format = serilization_format; + record_options.rmw_serialization_format = std::string(serilization_format).empty() ? + rmw_get_serialization_format() : + serilization_format; rosbag2_transport::Rosbag2Transport transport; transport.init(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 6b1c13a13..73f10893c 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -42,12 +42,16 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_man_.add_publisher( array_topic, array_message, 2); - start_recording({false, {string_topic, array_topic}, ""}); + start_recording({false, {string_topic, array_topic}, "rmw_format"}); run_publishers(); stop_recording(); auto recorded_messages = writer_->get_messages(); + auto recorded_topics = writer_->get_topics(); + ASSERT_THAT(recorded_topics, SizeIs(2)); + EXPECT_THAT(recorded_topics.at(string_topic).serialization_format, Eq("rmw_format")); + EXPECT_THAT(recorded_topics.at(array_topic).serialization_format, Eq("rmw_format")); ASSERT_THAT(recorded_messages, SizeIs(4)); auto string_messages = filter_messages( recorded_messages, string_topic); From 65d7ea3c606a41ec424cb4850d14cd2769b342ec Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 21 Nov 2018 15:17:28 +0100 Subject: [PATCH 13/22] GH-137: Fix cdr converter plugin - update pluginlib descriptions file after several renames - fix export of missing includes folder --- rosbag2_converter_default_plugins/CMakeLists.txt | 4 ---- rosbag2_converter_default_plugins/package.xml | 1 - rosbag2_converter_default_plugins/plugin_description.xml | 4 ++-- 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/rosbag2_converter_default_plugins/CMakeLists.txt b/rosbag2_converter_default_plugins/CMakeLists.txt index a909d0e81..ab7e0b4a1 100644 --- a/rosbag2_converter_default_plugins/CMakeLists.txt +++ b/rosbag2_converter_default_plugins/CMakeLists.txt @@ -50,10 +50,6 @@ install( LIBRARY DESTINATION lib RUNTIME DESTINATION bin) -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(rosbag2) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_lint_auto REQUIRED) diff --git a/rosbag2_converter_default_plugins/package.xml b/rosbag2_converter_default_plugins/package.xml index 286e30576..cba42ae92 100644 --- a/rosbag2_converter_default_plugins/package.xml +++ b/rosbag2_converter_default_plugins/package.xml @@ -1,5 +1,4 @@ - rosbag2_converter_default_plugins 0.0.0 diff --git a/rosbag2_converter_default_plugins/plugin_description.xml b/rosbag2_converter_default_plugins/plugin_description.xml index 63235c7ec..d153ba7a8 100644 --- a/rosbag2_converter_default_plugins/plugin_description.xml +++ b/rosbag2_converter_default_plugins/plugin_description.xml @@ -1,8 +1,8 @@ This is a converter plugin for cdr format. From 2c28e1dc34df2bf19f2a0f004af6cf2df59a84a7 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 21 Nov 2018 15:31:44 +0100 Subject: [PATCH 14/22] GH-137 Add integration test for cdr converter --- rosbag2_tests/CMakeLists.txt | 9 ++ rosbag2_tests/package.xml | 2 + .../test/rosbag2_tests/test_converter.cpp | 106 ++++++++++++++++++ 3 files changed, 117 insertions(+) create mode 100644 rosbag2_tests/test/rosbag2_tests/test_converter.cpp diff --git a/rosbag2_tests/CMakeLists.txt b/rosbag2_tests/CMakeLists.txt index 7317d2ed8..a0c6a88a7 100644 --- a/rosbag2_tests/CMakeLists.txt +++ b/rosbag2_tests/CMakeLists.txt @@ -63,6 +63,15 @@ if(BUILD_TESTING) rosbag2_storage) endif() + ament_add_gmock(test_converter + test/rosbag2_tests/test_converter.cpp + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + if(TARGET test_converter) + ament_target_dependencies(test_converter + rosbag2 + rosbag2_test_common + test_msgs) + endif() endif() ament_package() diff --git a/rosbag2_tests/package.xml b/rosbag2_tests/package.xml index d93394c7a..339c82853 100644 --- a/rosbag2_tests/package.xml +++ b/rosbag2_tests/package.xml @@ -15,8 +15,10 @@ ament_lint_auto ament_lint_common rosbag2_storage_default_plugins + rosbag2_converter_default_plugins rosbag2_test_common rosbag2_storage + rosbag2 ros2bag rclcpp test_msgs diff --git a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp new file mode 100644 index 000000000..b366a5bbb --- /dev/null +++ b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp @@ -0,0 +1,106 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcutils/strdup.h" +#include "rosbag2/serialization_format_converter_factory.hpp" +#include "rosbag2/typesupport_helpers.hpp" +#include "rosbag2/types/ros2_message.hpp" +#include "rosbag2_test_common/memory_management.hpp" +#include "test_msgs/message_fixtures.hpp" + +using rosbag2::SerializationFormatConverterFactory; +using namespace ::testing; // NOLINT +using namespace rosbag2_test_common; // NOLINT + +class ConverterTestFixture : public Test +{ +public: + ConverterTestFixture() + { + factory_ = std::make_unique(); + memory_management_ = std::make_unique(); + topic_name_ = "test_topic"; + allocator_ = rcutils_get_default_allocator(); + cdr_converter_ = factory_->load_converter("cdr"); + } + std::shared_ptr + + allocate_empty_dynamic_array_message() + { + auto introspection_type_support = rosbag2::get_typesupport( + "test_msgs/DynamicArrayNested", "rosidl_typesupport_introspection_cpp"); + auto ros_message = rosbag2::allocate_ros2_message(introspection_type_support, &allocator_); + ros_message->timestamp = 1; + ros_message->topic_name = topic_name_.c_str(); + return ros_message; + } + + std::unique_ptr factory_; + std::unique_ptr cdr_converter_; + std::unique_ptr memory_management_; + std::string topic_name_; + rcutils_allocator_t allocator_; +}; + +TEST_F(ConverterTestFixture, serialize_converts_ros_message_into_cdr_for_dynamic_array_nested) { + auto ros_message = allocate_empty_dynamic_array_message(); + + auto correctly_typed_ros_message = reinterpret_cast( + ros_message->message); + auto primitive_msgs = get_messages_primitives(); + for (const auto & primitive_msg : primitive_msgs) { + correctly_typed_ros_message->primitive_values.push_back(*primitive_msg); + } + // The message should be equivalent to the following message: + auto message = get_messages_dynamic_array_nested()[0]; + EXPECT_THAT(*correctly_typed_ros_message, Eq(*message)); + + auto serialized_message = std::make_shared(); + serialized_message->serialized_data = memory_management_->make_initialized_message(); + auto type_support = rosbag2::get_typesupport( + "test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); + + cdr_converter_->serialize(ros_message, type_support, serialized_message); + + auto deserialized_msg = memory_management_-> + deserialize_message(serialized_message->serialized_data); + EXPECT_THAT(*deserialized_msg, Eq(*message)); + EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); + EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->timestamp)); +} + +TEST_F(ConverterTestFixture, deserialize_converts_ros_message_into_cdr_for_dynamic_array_nested) { + auto ros_message = allocate_empty_dynamic_array_message(); + + auto message = get_messages_dynamic_array_nested()[0]; + auto bag_message = std::make_shared(); + bag_message->topic_name = topic_name_; + bag_message->time_stamp = 1; + bag_message->serialized_data = memory_management_->serialize_message(message); + + auto type_support = rosbag2::get_typesupport( + "test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); + + cdr_converter_->deserialize(bag_message, type_support, ros_message); + + auto deserialized_message = + static_cast(ros_message->message); + EXPECT_THAT(*deserialized_message, Eq(*message)); + EXPECT_THAT(ros_message->topic_name, StrEq(topic_name_)); +} From 827bc40cfbb7755e954ce3c179c166373c6d8562 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Thu, 22 Nov 2018 10:26:12 +0100 Subject: [PATCH 15/22] GH-137 Fix superfluous printf --- .../src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp index b9ed68968..293130805 100644 --- a/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp +++ b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp @@ -101,7 +101,6 @@ void CdrConverter::deserialize( ros_message->topic_name = serialized_message->topic_name.c_str(); ros_message->time_stamp = serialized_message->time_stamp; - printf("this should be a call to FASTRTPS\n"); auto ret = deserialize_fcn_(serialized_message->serialized_data.get(), type_support, ros_message->message); if (ret != RMW_RET_OK) { From 605a15fe9ef494d5e193314768ed017bca10f45e Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 21 Nov 2018 14:13:30 +0100 Subject: [PATCH 16/22] GH-17 Add leak sanitizer to test - one of the main test goals can only be ssen by valgrind or sanitizers - enable leak sanitizer for gcc builds only (for now) --- rosbag2/CMakeLists.txt | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/rosbag2/CMakeLists.txt b/rosbag2/CMakeLists.txt index 62b61ddc2..745584a5e 100644 --- a/rosbag2/CMakeLists.txt +++ b/rosbag2/CMakeLists.txt @@ -119,11 +119,28 @@ if(BUILD_TESTING) target_link_libraries(test_sequential_reader rosbag2) endif() + # If compiling with gcc, run this test with sanitizers enabled ament_add_gmock(test_ros2_message - test/rosbag2/types/test_ros2_message.cpp) + test/rosbag2/types/test_ros2_message.cpp + src/rosbag2/typesupport_helpers.cpp + src/rosbag2/types/ros2_message.cpp) if(TARGET test_ros2_message) - target_link_libraries(test_ros2_message rosbag2) - ament_target_dependencies(test_ros2_message test_msgs) + target_compile_definitions(test_ros2_message PRIVATE "ROSBAG2_BUILDING_DLL") + target_compile_options(test_ros2_message PUBLIC $<$:-fsanitize=leak>) + target_link_libraries(test_ros2_message $<$:-fsanitize=leak>) + target_include_directories(test_ros2_message + PUBLIC + $ + $) + ament_target_dependencies(test_ros2_message + ament_index_cpp + Poco + rcutils + rosbag2_storage + rosidl_generator_cpp + rosidl_typesupport_introspection_cpp + rosidl_typesupport_cpp + test_msgs) endif() ament_add_gmock(test_writer From 11ba7010b19a6582ca402925794e6a4b64edd2eb Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Thu, 22 Nov 2018 15:32:38 +0100 Subject: [PATCH 17/22] GH-137 It suffices to have only one converter test --- .../test/rosbag2_tests/test_converter.cpp | 42 +++++-------------- 1 file changed, 11 insertions(+), 31 deletions(-) diff --git a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp index b366a5bbb..65b795030 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp @@ -39,9 +39,8 @@ class ConverterTestFixture : public Test allocator_ = rcutils_get_default_allocator(); cdr_converter_ = factory_->load_converter("cdr"); } - std::shared_ptr - allocate_empty_dynamic_array_message() + std::shared_ptr allocate_empty_dynamic_array_message() { auto introspection_type_support = rosbag2::get_typesupport( "test_msgs/DynamicArrayNested", "rosidl_typesupport_introspection_cpp"); @@ -59,48 +58,29 @@ class ConverterTestFixture : public Test }; TEST_F(ConverterTestFixture, serialize_converts_ros_message_into_cdr_for_dynamic_array_nested) { - auto ros_message = allocate_empty_dynamic_array_message(); + auto initial_ros_message = allocate_empty_dynamic_array_message(); auto correctly_typed_ros_message = reinterpret_cast( - ros_message->message); + initial_ros_message->message); auto primitive_msgs = get_messages_primitives(); for (const auto & primitive_msg : primitive_msgs) { correctly_typed_ros_message->primitive_values.push_back(*primitive_msg); } - // The message should be equivalent to the following message: - auto message = get_messages_dynamic_array_nested()[0]; - EXPECT_THAT(*correctly_typed_ros_message, Eq(*message)); auto serialized_message = std::make_shared(); serialized_message->serialized_data = memory_management_->make_initialized_message(); auto type_support = rosbag2::get_typesupport( "test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); - cdr_converter_->serialize(ros_message, type_support, serialized_message); + cdr_converter_->serialize(initial_ros_message, type_support, serialized_message); - auto deserialized_msg = memory_management_-> - deserialize_message(serialized_message->serialized_data); - EXPECT_THAT(*deserialized_msg, Eq(*message)); - EXPECT_THAT(serialized_message->topic_name, StrEq(topic_name_)); - EXPECT_THAT(serialized_message->time_stamp, Eq(ros_message->timestamp)); -} - -TEST_F(ConverterTestFixture, deserialize_converts_ros_message_into_cdr_for_dynamic_array_nested) { - auto ros_message = allocate_empty_dynamic_array_message(); - - auto message = get_messages_dynamic_array_nested()[0]; - auto bag_message = std::make_shared(); - bag_message->topic_name = topic_name_; - bag_message->time_stamp = 1; - bag_message->serialized_data = memory_management_->serialize_message(message); - - auto type_support = rosbag2::get_typesupport( - "test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); + auto final_roundtrip_ros_message = allocate_empty_dynamic_array_message(); - cdr_converter_->deserialize(bag_message, type_support, ros_message); + cdr_converter_->deserialize(serialized_message, type_support, final_roundtrip_ros_message); - auto deserialized_message = - static_cast(ros_message->message); - EXPECT_THAT(*deserialized_message, Eq(*message)); - EXPECT_THAT(ros_message->topic_name, StrEq(topic_name_)); + EXPECT_THAT( + *static_cast(final_roundtrip_ros_message->message), + Eq(*static_cast(initial_ros_message->message))); + EXPECT_THAT(final_roundtrip_ros_message->topic_name, StrEq(initial_ros_message->topic_name)); + EXPECT_THAT(final_roundtrip_ros_message->timestamp, Eq(initial_ros_message->timestamp)); } From 1a3cb60b13c4d0ad11d8d45420b035478deab610 Mon Sep 17 00:00:00 2001 From: Andreas Holzner Date: Fri, 23 Nov 2018 10:12:45 +0100 Subject: [PATCH 18/22] GH-137 Minor refactoring for better readability of test N.B. This exposes an pre-existing memory leak (not fixed here). --- .../test/rosbag2_tests/test_converter.cpp | 30 +++++++++++-------- 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp index 65b795030..261327ea6 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp @@ -38,6 +38,8 @@ class ConverterTestFixture : public Test topic_name_ = "test_topic"; allocator_ = rcutils_get_default_allocator(); cdr_converter_ = factory_->load_converter("cdr"); + type_support_ = + rosbag2::get_typesupport("test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); } std::shared_ptr allocate_empty_dynamic_array_message() @@ -50,33 +52,37 @@ class ConverterTestFixture : public Test return ros_message; } + void fill_dynamic_array_message(std::shared_ptr message) + { + auto correctly_typed_ros_message = + reinterpret_cast(message->message); + auto primitive_msgs = get_messages_primitives(); + for (const auto & primitive_msg : primitive_msgs) { + correctly_typed_ros_message->primitive_values.push_back(*primitive_msg); + } + } + std::unique_ptr factory_; std::unique_ptr cdr_converter_; std::unique_ptr memory_management_; std::string topic_name_; rcutils_allocator_t allocator_; + const rosidl_message_type_support_t * type_support_; }; -TEST_F(ConverterTestFixture, serialize_converts_ros_message_into_cdr_for_dynamic_array_nested) { +TEST_F(ConverterTestFixture, cdr_converter_plugin_can_serialize_and_deserialize_messages) { auto initial_ros_message = allocate_empty_dynamic_array_message(); - - auto correctly_typed_ros_message = reinterpret_cast( - initial_ros_message->message); - auto primitive_msgs = get_messages_primitives(); - for (const auto & primitive_msg : primitive_msgs) { - correctly_typed_ros_message->primitive_values.push_back(*primitive_msg); - } + fill_dynamic_array_message(initial_ros_message); auto serialized_message = std::make_shared(); serialized_message->serialized_data = memory_management_->make_initialized_message(); - auto type_support = rosbag2::get_typesupport( - "test_msgs/DynamicArrayNested", "rosidl_typesupport_cpp"); - cdr_converter_->serialize(initial_ros_message, type_support, serialized_message); + cdr_converter_->serialize(initial_ros_message, type_support_, serialized_message); auto final_roundtrip_ros_message = allocate_empty_dynamic_array_message(); - cdr_converter_->deserialize(serialized_message, type_support, final_roundtrip_ros_message); + cdr_converter_->deserialize(serialized_message, type_support_, final_roundtrip_ros_message); + serialized_message.reset(); EXPECT_THAT( *static_cast(final_roundtrip_ros_message->message), From 61d425bbb5119f30fcc6896767f1066dcd5592cc Mon Sep 17 00:00:00 2001 From: Andreas Holzner Date: Fri, 23 Nov 2018 12:08:37 +0100 Subject: [PATCH 19/22] GH-137 Fix memory leak of topic_name - topic_name member needs to be freed - provide a setter for convenience - Directly assigning a string literal in the test is not sufficient as this would be static memory that does not need to be freed. --- rosbag2/include/rosbag2/types/ros2_message.hpp | 5 ++++- rosbag2/src/rosbag2/types/ros2_message.cpp | 11 +++++++++++ rosbag2/test/rosbag2/types/test_ros2_message.cpp | 11 ++++++++++- .../cdr/cdr_converter.cpp | 2 +- .../cdr/test_cdr_converter.cpp | 2 +- rosbag2_tests/test/rosbag2_tests/test_converter.cpp | 4 ++-- 6 files changed, 29 insertions(+), 6 deletions(-) diff --git a/rosbag2/include/rosbag2/types/ros2_message.hpp b/rosbag2/include/rosbag2/types/ros2_message.hpp index a226fdf69..cbe92cf0e 100644 --- a/rosbag2/include/rosbag2/types/ros2_message.hpp +++ b/rosbag2/include/rosbag2/types/ros2_message.hpp @@ -26,7 +26,7 @@ typedef struct rosbag2_ros2_message_t { void * message; - const char * topic_name; + char * topic_name; rcutils_time_point_value_t time_stamp; rcutils_allocator_t allocator; } rosbag2_ros2_message_t; @@ -39,6 +39,9 @@ std::shared_ptr allocate_ros2_message( const rosidl_message_type_support_t * introspection_ts, const rcutils_allocator_t * allocator); +ROSBAG2_PUBLIC +void ros2_message_set_topic_name(rosbag2_ros2_message_t * msg, const char * topic_name); + ROSBAG2_PUBLIC void allocate_internal_types( void * msg, const rosidl_typesupport_introspection_cpp::MessageMembers * members); diff --git a/rosbag2/src/rosbag2/types/ros2_message.cpp b/rosbag2/src/rosbag2/types/ros2_message.cpp index 588734cd3..6121feaa3 100644 --- a/rosbag2/src/rosbag2/types/ros2_message.cpp +++ b/rosbag2/src/rosbag2/types/ros2_message.cpp @@ -18,6 +18,7 @@ #include #include +#include "rcutils/strdup.h" #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" @@ -33,6 +34,7 @@ allocate_ros2_message( introspection_ts->data); auto raw_ros2_message = new rosbag2_ros2_message_t(); raw_ros2_message->allocator = *allocator; + raw_ros2_message->topic_name = nullptr; raw_ros2_message->message = raw_ros2_message->allocator.zero_allocate( 1, intro_ts_members->size_of_, raw_ros2_message->allocator.state); // TODO(Martin-Idel-SI): Use custom allocator to make sure all memory is obtained that way @@ -44,12 +46,21 @@ allocate_ros2_message( return std::shared_ptr(raw_ros2_message, deleter); } +void ros2_message_set_topic_name(rosbag2_ros2_message_t * msg, const char * topic_name) +{ + if (msg->topic_name) { + msg->allocator.deallocate(msg->topic_name, msg->allocator.state); + } + msg->topic_name = rcutils_strdup(topic_name, msg->allocator); +} + void deallocate_ros2_message( rosbag2_ros2_message_t * msg, const rosidl_typesupport_introspection_cpp::MessageMembers * members) { deallocate_internal_types(msg->message, members); msg->allocator.deallocate(msg->message, msg->allocator.state); + msg->allocator.deallocate(msg->topic_name, msg->allocator.state); delete msg; } diff --git a/rosbag2/test/rosbag2/types/test_ros2_message.cpp b/rosbag2/test/rosbag2/types/test_ros2_message.cpp index 41221238c..d459769c8 100644 --- a/rosbag2/test/rosbag2/types/test_ros2_message.cpp +++ b/rosbag2/test/rosbag2/types/test_ros2_message.cpp @@ -208,7 +208,16 @@ TEST_F(Ros2MessageTest, allocate_ros2_message_allocates_nested_bounded_array_nes TEST_F(Ros2MessageTest, allocate_ros2_message_cleans_up_topic_name_on_shutdown) { auto message = get_allocated_message("test_msgs/BoundedArrayNested"); - message->topic_name = "Topic name"; + rosbag2::ros2_message_set_topic_name(message.get(), "Topic name"); + + EXPECT_THAT(message->topic_name, StrEq("Topic name")); +} + +TEST_F(Ros2MessageTest, duplicate_set_topic_does_not_leak) { + auto message = get_allocated_message("test_msgs/BoundedArrayNested"); + + rosbag2::ros2_message_set_topic_name(message.get(), "Topic name"); + rosbag2::ros2_message_set_topic_name(message.get(), "Topic name"); EXPECT_THAT(message->topic_name, StrEq("Topic name")); } diff --git a/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp index 293130805..616aba273 100644 --- a/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp +++ b/rosbag2_converter_default_plugins/src/rosbag2_converter_default_plugins/cdr/cdr_converter.cpp @@ -98,7 +98,7 @@ void CdrConverter::deserialize( const rosidl_message_type_support_t * type_support, std::shared_ptr ros_message) { - ros_message->topic_name = serialized_message->topic_name.c_str(); + rosbag2::ros2_message_set_topic_name(ros_message.get(), serialized_message->topic_name.c_str()); ros_message->time_stamp = serialized_message->time_stamp; auto ret = diff --git a/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp b/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp index fb3175f30..be3c5a431 100644 --- a/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp +++ b/rosbag2_converter_default_plugins/test/rosbag2_converter_default_plugins/cdr/test_cdr_converter.cpp @@ -49,7 +49,7 @@ class CdrConverterTestFixture : public Test ros_message->allocator = allocator_; if (!topic_name.empty()) { - ros_message->topic_name = topic_name.c_str(); + rosbag2::ros2_message_set_topic_name(ros_message.get(), topic_name.c_str()); } return ros_message; diff --git a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp index 261327ea6..fe8723111 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp @@ -47,8 +47,8 @@ class ConverterTestFixture : public Test auto introspection_type_support = rosbag2::get_typesupport( "test_msgs/DynamicArrayNested", "rosidl_typesupport_introspection_cpp"); auto ros_message = rosbag2::allocate_ros2_message(introspection_type_support, &allocator_); - ros_message->timestamp = 1; - ros_message->topic_name = topic_name_.c_str(); + ros_message->time_stamp = 1; + rosbag2::ros2_message_set_topic_name(ros_message.get(), topic_name_.c_str()); return ros_message; } From 5a93f1bf1d4bbbb906f2a2db08db6cfe6ba3f1f0 Mon Sep 17 00:00:00 2001 From: Andreas Holzner Date: Fri, 23 Nov 2018 12:32:08 +0100 Subject: [PATCH 20/22] GH-17 Allow disabling the usage of sanitizers This allows manual usage of valgrind. --- rosbag2/CMakeLists.txt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rosbag2/CMakeLists.txt b/rosbag2/CMakeLists.txt index 745584a5e..890c6d6d4 100644 --- a/rosbag2/CMakeLists.txt +++ b/rosbag2/CMakeLists.txt @@ -15,6 +15,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() +option(DISABLE_SANITIZERS "disables the use of gcc saniztizers") + find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(pluginlib REQUIRED) @@ -126,8 +128,10 @@ if(BUILD_TESTING) src/rosbag2/types/ros2_message.cpp) if(TARGET test_ros2_message) target_compile_definitions(test_ros2_message PRIVATE "ROSBAG2_BUILDING_DLL") - target_compile_options(test_ros2_message PUBLIC $<$:-fsanitize=leak>) - target_link_libraries(test_ros2_message $<$:-fsanitize=leak>) + if(NOT DISABLE_SANITIZERS) + target_compile_options(test_ros2_message PUBLIC $<$:-fsanitize=leak>) + target_link_libraries(test_ros2_message $<$:-fsanitize=leak>) + endif() target_include_directories(test_ros2_message PUBLIC $ From aa7355665a23e3ddf94b9db2b2a6b9e7b270be12 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Mon, 26 Nov 2018 17:59:55 +0100 Subject: [PATCH 21/22] GH-17 Fix renaming after rebase --- rosbag2_tests/test/rosbag2_tests/test_converter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp index fe8723111..307d1adf7 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_converter.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_converter.cpp @@ -88,5 +88,5 @@ TEST_F(ConverterTestFixture, cdr_converter_plugin_can_serialize_and_deserialize_ *static_cast(final_roundtrip_ros_message->message), Eq(*static_cast(initial_ros_message->message))); EXPECT_THAT(final_roundtrip_ros_message->topic_name, StrEq(initial_ros_message->topic_name)); - EXPECT_THAT(final_roundtrip_ros_message->timestamp, Eq(initial_ros_message->timestamp)); + EXPECT_THAT(final_roundtrip_ros_message->time_stamp, Eq(initial_ros_message->time_stamp)); } From 77608b5c02e797cdf2117b264d76ab3f0ddeb5fa Mon Sep 17 00:00:00 2001 From: Andreas Holzner Date: Wed, 28 Nov 2018 12:53:38 +0100 Subject: [PATCH 22/22] GH-17 Small cleanups (addressing review comments) --- ros2bag/ros2bag/verb/record.py | 3 ++- rosbag2/include/rosbag2/converter.hpp | 8 +++++++- rosbag2/include/rosbag2/writer.hpp | 1 + rosbag2/src/rosbag2/converter.cpp | 10 ++-------- rosbag2/src/rosbag2/types/ros2_message.cpp | 1 + 5 files changed, 13 insertions(+), 10 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 4d9138bd3..f83ace48d 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -72,7 +72,8 @@ def main(self, *, args): # noqa: D102 all=True) elif args.topics and len(args.topics) > 0: rosbag2_transport_py.record( - uri=uri, storage_id=args.storage, + uri=uri, + storage_id=args.storage, serialization_format=args.serialization_format, topics=args.topics) else: diff --git a/rosbag2/include/rosbag2/converter.hpp b/rosbag2/include/rosbag2/converter.hpp index ae177dc31..dc951a5bd 100644 --- a/rosbag2/include/rosbag2/converter.hpp +++ b/rosbag2/include/rosbag2/converter.hpp @@ -37,7 +37,13 @@ namespace rosbag2 { -struct ConverterTypeSupport; +// Convenience struct to keep both type supports (rmw and introspection) together. +// Only used internally. +struct ConverterTypeSupport +{ + const rosidl_message_type_support_t * cpp_type_support; + const rosidl_message_type_support_t * introspection_type_support; +}; class ROSBAG2_PUBLIC Converter { diff --git a/rosbag2/include/rosbag2/writer.hpp b/rosbag2/include/rosbag2/writer.hpp index 91aa1572a..39732de91 100644 --- a/rosbag2/include/rosbag2/writer.hpp +++ b/rosbag2/include/rosbag2/writer.hpp @@ -54,6 +54,7 @@ class ROSBAG2_PUBLIC Writer std::make_shared(), std::unique_ptr metadata_io = std::make_unique()); + virtual ~Writer(); /** diff --git a/rosbag2/src/rosbag2/converter.cpp b/rosbag2/src/rosbag2/converter.cpp index 41b265a2c..a648fe50a 100644 --- a/rosbag2/src/rosbag2/converter.cpp +++ b/rosbag2/src/rosbag2/converter.cpp @@ -28,12 +28,6 @@ namespace rosbag2 { -struct ConverterTypeSupport -{ - const rosidl_message_type_support_t * type_support; - const rosidl_message_type_support_t * introspection_type_support; -}; - Converter::Converter( const std::string & input_format, const std::string & output_format, @@ -60,7 +54,7 @@ Converter::~Converter() std::shared_ptr Converter::convert( std::shared_ptr message) { - auto ts = topics_and_types_.at(message->topic_name).type_support; + auto ts = topics_and_types_.at(message->topic_name).cpp_type_support; auto introspection_ts = topics_and_types_.at(message->topic_name).introspection_type_support; auto allocator = rcutils_get_default_allocator(); std::shared_ptr allocated_ros_message = @@ -76,7 +70,7 @@ std::shared_ptr Converter::convert( void Converter::add_topic(const std::string & topic, const std::string & type) { ConverterTypeSupport type_support; - type_support.type_support = get_typesupport(type, "rosidl_typesupport_cpp"); + type_support.cpp_type_support = get_typesupport(type, "rosidl_typesupport_cpp"); type_support.introspection_type_support = get_typesupport(type, "rosidl_typesupport_introspection_cpp"); diff --git a/rosbag2/src/rosbag2/types/ros2_message.cpp b/rosbag2/src/rosbag2/types/ros2_message.cpp index 6121feaa3..2efc7d0b8 100644 --- a/rosbag2/src/rosbag2/types/ros2_message.cpp +++ b/rosbag2/src/rosbag2/types/ros2_message.cpp @@ -50,6 +50,7 @@ void ros2_message_set_topic_name(rosbag2_ros2_message_t * msg, const char * topi { if (msg->topic_name) { msg->allocator.deallocate(msg->topic_name, msg->allocator.state); + msg->topic_name = nullptr; } msg->topic_name = rcutils_strdup(topic_name, msg->allocator); }