Use converters when recording a bag file

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(
+            '-f', '--serialization-format', 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,17 @@ 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,
+                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, topics=args.topics)
+            rosbag2_transport_py.record(
+                uri=uri,
+                storage_id=args.storage,
+                serialization_format=args.serialization_format,
+                topics=args.topics)
         else:
             self._subparser.print_help()
 

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)
@@ -119,11 +121,36 @@ 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")
+    if(NOT DISABLE_SANITIZERS)
+      target_compile_options(test_ros2_message PUBLIC $<$<CXX_COMPILER_ID:GNU>:-fsanitize=leak>)
+      target_link_libraries(test_ros2_message $<$<CXX_COMPILER_ID:GNU>:-fsanitize=leak>)
+    endif()
+    target_include_directories(test_ros2_message
+      PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>)
+    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
+    test/rosbag2/test_writer.cpp)
+  if(TARGET test_writer)
+    target_link_libraries(test_writer rosbag2)
   endif()
 endif()
 

rosbag2/include/rosbag2/converter.hpp

@@ -36,14 +36,22 @@
 
 namespace rosbag2
 {
+
+// 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
 {
 public:
   explicit
   Converter(
     const std::string & input_format,
     const std::string & output_format,
-    const std::vector<TopicMetadata> & topics_and_types,
     std::shared_ptr<SerializationFormatConverterFactoryInterface> converter_factory =
     std::make_shared<SerializationFormatConverterFactory>());
 
@@ -60,11 +68,13 @@ class ROSBAG2_PUBLIC Converter
   std::shared_ptr<SerializedBagMessage>
   convert(std::shared_ptr<const SerializedBagMessage> message);
 
+  void add_topic(const std::string & topic, const std::string & type);
+
 private:
   std::shared_ptr<SerializationFormatConverterFactoryInterface> converter_factory_;
   std::unique_ptr<SerializationFormatConverterInterface> input_converter_;
   std::unique_ptr<SerializationFormatConverterInterface> output_converter_;
-  std::map<std::string, std::string> topics_and_types_;
+  std::map<std::string, ConverterTypeSupport> topics_and_types_;
 };
 
 }  // namespace rosbag2

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<rosbag2_ros2_message_t>
 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);

rosbag2/include/rosbag2/writer.hpp

@@ -18,9 +18,12 @@
 #include <memory>
 #include <string>
 
+#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,17 +48,27 @@ class ROSBAG2_PUBLIC Writer
 public:
   explicit
   Writer(
-    std::unique_ptr<rosbag2_storage::StorageFactoryInterface> factory =
-    std::make_unique<rosbag2_storage::StorageFactory>());
+    std::unique_ptr<rosbag2_storage::StorageFactoryInterface> storage_factory =
+    std::make_unique<rosbag2_storage::StorageFactory>(),
+    std::shared_ptr<SerializationFormatConverterFactoryInterface> converter_factory =
+    std::make_shared<SerializationFormatConverterFactory>(),
+    std::unique_ptr<rosbag2_storage::MetadataIo> metadata_io =
+    std::make_unique<rosbag2_storage::MetadataIo>());
+
   virtual ~Writer();
 
   /**
    * Opens a new bagfile and prepare it for writing messages. The bagfile must not exist.
    * This must be called before any other function is used.
    *
    * \param options Options to configure the storage
+   * \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);
+  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
@@ -76,8 +89,11 @@ class ROSBAG2_PUBLIC Writer
 
 private:
   std::string uri_;
-  std::unique_ptr<rosbag2_storage::StorageFactoryInterface> factory_;
+  std::unique_ptr<rosbag2_storage::StorageFactoryInterface> storage_factory_;
+  std::shared_ptr<SerializationFormatConverterFactoryInterface> converter_factory_;
   std::shared_ptr<rosbag2_storage::storage_interfaces::ReadWriteInterface> storage_;
+  std::unique_ptr<rosbag2_storage::MetadataIo> metadata_io_;
+  std::unique_ptr<Converter> converter_;
 };
 
 }  // namespace rosbag2

rosbag2/src/rosbag2/converter.cpp

@@ -31,13 +31,9 @@ namespace rosbag2
 Converter::Converter(
   const std::string & input_format,
   const std::string & output_format,
-  const std::vector<TopicMetadata> & topics_and_types,
   std::shared_ptr<rosbag2::SerializationFormatConverterFactoryInterface> 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 +54,8 @@ Converter::~Converter()
 std::shared_ptr<SerializedBagMessage> Converter::convert(
   std::shared_ptr<const rosbag2::SerializedBagMessage> 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_.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<rosbag2_ros2_message_t> allocated_ros_message =
     allocate_ros2_message(introspection_ts, &allocator);
@@ -72,4 +67,14 @@ std::shared_ptr<SerializedBagMessage> Converter::convert(
   return output_message;
 }
 
+void Converter::add_topic(const std::string & topic, const std::string & type)
+{
+  ConverterTypeSupport type_support;
+  type_support.cpp_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

rosbag2/src/rosbag2/sequential_reader.cpp

@@ -19,6 +19,8 @@
 #include <utility>
 #include <vector>
 
+#include "rosbag2/info.hpp"
+
 namespace rosbag2
 {
 
@@ -41,13 +43,29 @@ 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<Converter>(
       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);
+    }
   }
 }
 

rosbag2/src/rosbag2/types/ros2_message.cpp

@@ -14,11 +14,11 @@
 
 #include "rosbag2/types/ros2_message.hpp"
 
-#include <iostream>
 #include <memory>
 #include <string>
 #include <vector>
 
+#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"
@@ -34,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
@@ -45,12 +46,22 @@ allocate_ros2_message(
   return std::shared_ptr<rosbag2_ros2_message_t>(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 = nullptr;
+  }
+  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;
 }
 

rosbag2/src/rosbag2/writer.cpp

@@ -18,30 +18,45 @@
 #include <string>
 #include <utility>
 
-#include "rosbag2_storage/metadata_io.hpp"
 #include "rosbag2/info.hpp"
 #include "rosbag2/storage_options.hpp"
 
 namespace rosbag2
 {
 
-Writer::Writer(std::unique_ptr<rosbag2_storage::StorageFactoryInterface> factory)
-: factory_(std::move(factory))
+Writer::Writer(
+  std::unique_ptr<rosbag2_storage::StorageFactoryInterface> storage_factory,
+  std::shared_ptr<SerializationFormatConverterFactoryInterface> converter_factory,
+  std::unique_ptr<rosbag2_storage::MetadataIo> 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
+  storage_.reset();  // Necessary to ensure that the storage is destroyed before the factory
+  storage_factory_.reset();
 }
 
-void Writer::open(const StorageOptions & options)
+void Writer::open(
+  const StorageOptions & options,
+  const std::string & input_serialization_format,
+  const std::string & output_serialization_format)
 {
-  storage_ = factory_->open_read_write(options.uri, options.storage_id);
+  if (output_serialization_format != input_serialization_format) {
+    converter_ = std::make_unique<Converter>(
+      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");
   }
@@ -54,6 +69,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 +82,7 @@ void Writer::write(std::shared_ptr<SerializedBagMessage> 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