Use converters when playing back files

rosbag2/CMakeLists.txt

@@ -23,13 +23,17 @@ find_package(Poco COMPONENTS Foundation)
 find_package(rcutils REQUIRED)
 find_package(rosbag2_storage REQUIRED)
 find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_typesupport_cpp REQUIRED)
+find_package(rosidl_typesupport_introspection_cpp REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
+  src/rosbag2/converter.cpp
   src/rosbag2/info.cpp
   src/rosbag2/sequential_reader.cpp
   src/rosbag2/serialization_format_converter_factory.cpp
   src/rosbag2/typesupport_helpers.cpp
-  src/rosbag2/writer.cpp)
+  src/rosbag2/writer.cpp
+  src/rosbag2/types/ros2_message.cpp)
 
 ament_target_dependencies(${PROJECT_NAME}
   ament_index_cpp
@@ -38,6 +42,8 @@ ament_target_dependencies(${PROJECT_NAME}
   rcutils
   rosbag2_storage
   rosidl_generator_cpp
+  rosidl_typesupport_introspection_cpp
+  rosidl_typesupport_cpp
 )
 
 target_include_directories(${PROJECT_NAME}
@@ -65,11 +71,12 @@ install(
 
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
-ament_export_dependencies(rosbag2_storage)
+ament_export_dependencies(rosbag2_storage rosidl_typesupport_introspection_cpp)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ament_lint_auto REQUIRED)
+  find_package(test_msgs REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
   add_library(
@@ -100,12 +107,24 @@ if(BUILD_TESTING)
 
   ament_add_gmock(test_info
     test/rosbag2/test_info.cpp
-    test/rosbag2/mock_metadata_io.hpp
     WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
   if(TARGET test_info)
     target_link_libraries(test_info rosbag2)
     ament_target_dependencies(test_info rosbag2_test_common)
   endif()
+
+  ament_add_gmock(test_sequential_reader
+    test/rosbag2/test_sequential_reader.cpp)
+  if(TARGET test_sequential_reader)
+    target_link_libraries(test_sequential_reader rosbag2)
+  endif()
+
+  ament_add_gmock(test_ros2_message
+    test/rosbag2/types/test_ros2_message.cpp)
+  if(TARGET test_ros2_message)
+    target_link_libraries(test_ros2_message rosbag2)
+    ament_target_dependencies(test_ros2_message test_msgs)
+  endif()
 endif()
 
 ament_package()

rosbag2/include/rosbag2/converter.hpp

@@ -0,0 +1,72 @@
+// 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.
+
+#ifndef ROSBAG2__CONVERTER_HPP_
+#define ROSBAG2__CONVERTER_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rosbag2/serialization_format_converter_factory.hpp"
+#include "rosbag2/serialization_format_converter_factory_interface.hpp"
+#include "rosbag2/serialization_format_converter_interface.hpp"
+#include "rosbag2/types.hpp"
+#include "rosbag2/visibility_control.hpp"
+
+// This is necessary because of using stl types here. It is completely safe, because
+// a) the member is not accessible from the outside
+// b) there are no inline functions.
+#ifdef _WIN32
+# pragma warning(push)
+# pragma warning(disable:4251)
+#endif
+
+namespace rosbag2
+{
+class ROSBAG2_PUBLIC Converter
+{
+public:
+  explicit
+  Converter(
+    const std::string & input_format,
+    const std::string & output_format,
+    const std::vector<TopicWithType> & topics_and_types,
+    std::shared_ptr<SerializationFormatConverterFactoryInterface> converter_factory =
+    std::make_shared<SerializationFormatConverterFactory>());
+
+  ~Converter();
+
+  /**
+   * Converts the given SerializedBagMessage into the output format of the converter. The
+   * serialization format of the input message must be identical to the input format of the
+   * converter.
+   *
+   * \param message Message to convert
+   * \returns Converted message
+   */
+  std::shared_ptr<SerializedBagMessage>
+  convert(std::shared_ptr<const SerializedBagMessage> message);
+
+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_;
+};
+
+}  // namespace rosbag2
+
+#endif  // ROSBAG2__CONVERTER_HPP_

rosbag2/include/rosbag2/sequential_reader.hpp

@@ -20,7 +20,11 @@
 #include <vector>
 
 #include "rosbag2_storage/storage_factory.hpp"
+#include "rosbag2_storage/storage_factory_interface.hpp"
 #include "rosbag2_storage/storage_interfaces/read_only_interface.hpp"
+#include "rosbag2/converter.hpp"
+#include "rosbag2/serialization_format_converter_factory.hpp"
+#include "rosbag2/serialization_format_converter_factory_interface.hpp"
 #include "rosbag2/storage_options.hpp"
 #include "rosbag2/types.hpp"
 #include "rosbag2/visibility_control.hpp"
@@ -42,16 +46,27 @@ namespace rosbag2
 class ROSBAG2_PUBLIC SequentialReader
 {
 public:
+  explicit
+  SequentialReader(
+    std::unique_ptr<rosbag2_storage::StorageFactoryInterface> storage_factory =
+    std::make_unique<rosbag2_storage::StorageFactory>(),
+    std::shared_ptr<SerializationFormatConverterFactoryInterface> converter_factory =
+    std::make_shared<SerializationFormatConverterFactory>());
   virtual ~SequentialReader();
 
   /**
    * Open a rosbag for reading messages sequentially (time-ordered). Throws if file could not be
    * opened. This must be called before any other function is used. The rosbag is
    * automatically closed on destruction.
    *
+   * If the rmw_serialization_format is not the same as the format of the underlying storage, a
+   * converter will be used to automatically convert the functions. Throws if the converter
+   * plugin does not exist
+   *
    * \param options Options to configure the storage
+   * \param rmw_serialization_format Messages will be serialized in this format
    */
-  virtual void open(const StorageOptions & options);
+  virtual void open(const StorageOptions & options, const std::string & rmw_serialization_format);
 
   /**
    * Ask whether the underlying bagfile contains at least one more message.
@@ -63,6 +78,7 @@ class ROSBAG2_PUBLIC SequentialReader
 
   /**
    * Read next message from storage. Will throw if no more messages are available.
+   * The message will be serialized in the format given to `open`.
    *
    * Expected usage:
    * if (writer.has_next()) message = writer.read_next();
@@ -81,8 +97,10 @@ class ROSBAG2_PUBLIC SequentialReader
   virtual std::vector<TopicWithType> get_all_topics_and_types();
 
 private:
-  rosbag2_storage::StorageFactory factory_;
+  std::unique_ptr<rosbag2_storage::StorageFactoryInterface> storage_factory_;
+  std::shared_ptr<SerializationFormatConverterFactoryInterface> converter_factory_;
   std::shared_ptr<rosbag2_storage::storage_interfaces::ReadOnlyInterface> storage_;
+  std::unique_ptr<Converter> converter_;
 };
 
 }  // namespace rosbag2

rosbag2/include/rosbag2/serialization_format_converter_factory.hpp

@@ -50,7 +50,7 @@ class ROSBAG2_PUBLIC SerializationFormatConverterFactory
 
   ~SerializationFormatConverterFactory() override;
 
-  std::shared_ptr<SerializationFormatConverterInterface>
+  std::unique_ptr<SerializationFormatConverterInterface>
   load_converter(const std::string & format) override;
 
 private:

rosbag2/include/rosbag2/serialization_format_converter_factory_interface.hpp

@@ -29,7 +29,7 @@ class ROSBAG2_PUBLIC SerializationFormatConverterFactoryInterface
 public:
   virtual ~SerializationFormatConverterFactoryInterface() = default;
 
-  virtual std::shared_ptr<SerializationFormatConverterInterface>
+  virtual std::unique_ptr<SerializationFormatConverterInterface>
   load_converter(const std::string & format) = 0;
 };
 

rosbag2/include/rosbag2/types/ros2_message.hpp

@@ -15,8 +15,13 @@
 #ifndef ROSBAG2__TYPES__ROS2_MESSAGE_HPP_
 #define ROSBAG2__TYPES__ROS2_MESSAGE_HPP_
 
+#include <memory>
+
+#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
 #include "rcutils/time.h"
 #include "rcutils/allocator.h"
+#include "rosbag2/visibility_control.hpp"
 
 typedef struct rosbag2_ros2_message_t
 {
@@ -26,4 +31,51 @@ typedef struct rosbag2_ros2_message_t
   rcutils_allocator_t allocator;
 } rosbag2_ros2_message_t;
 
+namespace rosbag2
+{
+
+ROSBAG2_PUBLIC
+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 allocate_internal_types(
+  void * msg, const rosidl_typesupport_introspection_cpp::MessageMembers * members);
+
+ROSBAG2_PUBLIC
+void allocate_element(
+  void * data, const rosidl_typesupport_introspection_cpp::MessageMember & member);
+
+ROSBAG2_PUBLIC
+void allocate_array(
+  void * data, const rosidl_typesupport_introspection_cpp::MessageMember & member);
+
+ROSBAG2_PUBLIC
+void allocate_vector(
+  void * data, const rosidl_typesupport_introspection_cpp::MessageMember & member);
+
+ROSBAG2_PUBLIC
+void deallocate_ros2_message(
+  rosbag2_ros2_message_t * msg,
+  const rosidl_typesupport_introspection_cpp::MessageMembers * members);
+
+ROSBAG2_PUBLIC
+void deallocate_internal_types(
+  void * msg, const rosidl_typesupport_introspection_cpp::MessageMembers * members);
+
+ROSBAG2_PUBLIC
+void cleanup_element(
+  void * data, const rosidl_typesupport_introspection_cpp::MessageMember & member);
+
+ROSBAG2_PUBLIC
+void cleanup_array(
+  void * data, const rosidl_typesupport_introspection_cpp::MessageMember & member);
+
+ROSBAG2_PUBLIC
+void cleanup_vector(
+  void * data, const rosidl_typesupport_introspection_cpp::MessageMember & member);
+
+}  // namespace rosbag2
+
 #endif  // ROSBAG2__TYPES__ROS2_MESSAGE_HPP_

rosbag2/include/rosbag2/typesupport_helpers.hpp

@@ -25,7 +25,8 @@ namespace rosbag2
 {
 
 ROSBAG2_PUBLIC
-const rosidl_message_type_support_t * get_typesupport(const std::string & type);
+const rosidl_message_type_support_t *
+get_typesupport(const std::string & type, const std::string & typesupport_identifier);
 
 ROSBAG2_PUBLIC
 const std::pair<std::string, std::string> extract_type_and_package(const std::string & full_type);

rosbag2/include/rosbag2/writer.hpp

@@ -19,6 +19,7 @@
 #include <string>
 
 #include "rosbag2_storage/storage_factory.hpp"
+#include "rosbag2_storage/storage_factory_interface.hpp"
 #include "rosbag2_storage/storage_interfaces/read_write_interface.hpp"
 #include "rosbag2/storage_options.hpp"
 #include "rosbag2/types.hpp"
@@ -42,6 +43,10 @@ namespace rosbag2
 class ROSBAG2_PUBLIC Writer
 {
 public:
+  explicit
+  Writer(
+    std::unique_ptr<rosbag2_storage::StorageFactoryInterface> factory =
+    std::make_unique<rosbag2_storage::StorageFactory>());
   virtual ~Writer();
 
   /**
@@ -71,7 +76,7 @@ class ROSBAG2_PUBLIC Writer
 
 private:
   std::string uri_;
-  rosbag2_storage::StorageFactory factory_;
+  std::unique_ptr<rosbag2_storage::StorageFactoryInterface> factory_;
   std::shared_ptr<rosbag2_storage::storage_interfaces::ReadWriteInterface> storage_;
 };
 

rosbag2/package.xml

@@ -14,6 +14,8 @@
   <depend>rcutils</depend>
   <depend>rosbag2_storage</depend>
   <depend>rosidl_generator_cpp</depend>
+  <depend>rosidl_typesupport_cpp</depend>
+  <depend>rosidl_typesupport_introspection_cpp</depend>
   <depend>shared_queues_vendor</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>