From 0e34ea2c6d3a08a31fcb0554a4ac01757fc55742 Mon Sep 17 00:00:00 2001
From: Karsten Knese <Karsten1987@users.noreply.github.com>
Date: Mon, 15 Mar 2021 19:34:02 -0700
Subject: [PATCH] rclcpp reader api

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>
---
 rosbag2_cpp/include/rosbag2_cpp/reader.hpp    | 25 +++++++++++++++++++
 rosbag2_cpp/include/rosbag2_cpp/writer.hpp    | 12 ++++++++-
 .../rosbag2_tests/test_rosbag2_cpp_api.cpp    | 16 +++++++++++-
 3 files changed, 51 insertions(+), 2 deletions(-)

diff --git a/rosbag2_cpp/include/rosbag2_cpp/reader.hpp b/rosbag2_cpp/include/rosbag2_cpp/reader.hpp
index 42db6ab660..ddac0fa72b 100644
--- a/rosbag2_cpp/include/rosbag2_cpp/reader.hpp
+++ b/rosbag2_cpp/include/rosbag2_cpp/reader.hpp
@@ -20,6 +20,9 @@
 #include <string>
 #include <vector>
 
+#include "rclcpp/serialization.hpp"
+#include "rclcpp/serialized_message.hpp"
+
 #include "rosbag2_cpp/converter_options.hpp"
 #include "rosbag2_cpp/readers/sequential_reader.hpp"
 #include "rosbag2_cpp/visibility_control.hpp"
@@ -110,6 +113,28 @@ class ROSBAG2_CPP_PUBLIC Reader final
    */
   std::shared_ptr<rosbag2_storage::SerializedBagMessage> read_next();
 
+  /**
+   * Read next message from storage. Will throw if no more messages are available.
+   * The message will be serialized in the format given to `open`.
+   *
+   * Expected usage:
+   * if (writer.has_next()) message = writer.read_next();
+   *
+   * \return next message in non-serialized form
+   * \throws runtime_error if the Reader is not open.
+   */
+  template<class MessageT>
+  MessageT read_next()
+  {
+    MessageT msg;
+    auto bag_message = read_next();
+    rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
+    rclcpp::Serialization<MessageT> serialization;
+    serialization.deserialize_message(&extracted_serialized_msg, &msg);
+
+    return msg;
+  }
+
   /**
     * Ask bagfile for its full metadata.
     *
diff --git a/rosbag2_cpp/include/rosbag2_cpp/writer.hpp b/rosbag2_cpp/include/rosbag2_cpp/writer.hpp
index b502de2854..7a368d7bbc 100644
--- a/rosbag2_cpp/include/rosbag2_cpp/writer.hpp
+++ b/rosbag2_cpp/include/rosbag2_cpp/writer.hpp
@@ -132,7 +132,7 @@ class ROSBAG2_CPP_PUBLIC Writer final
 
   /**
    * Write a serialized message to a bagfile.
-   * The topic needs to have been created before writing is possible.
+   * Opposing a call to \sa write without topic metadata, the topic will be created.
    *
    * \param message rclcpp::SerializedMessage The serialized message to be written to the bagfile
    * \param topic_name the string of the topic this messages belongs to
@@ -146,6 +146,16 @@ class ROSBAG2_CPP_PUBLIC Writer final
     const std::string & type_name,
     const rclcpp::Time & time);
 
+  /**
+   * Write a non-serialized message to a bagfile.
+   * Opposing a call to \sa write without topic metadata, the topic will be created.
+   *
+   * \param message MessageT The serialized message to be written to the bagfile
+   * \param topic_name the string of the topic this messages belongs to
+   * \param type_name the string of the type associated with this message
+   * \param time The time stamp of the message
+   * \throws runtime_error if the Writer is not open.
+   */
   template<class MessageT>
   void write(
     const MessageT & message,
diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_api.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_api.cpp
index 9a05f07ce3..22a8272f62 100644
--- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_api.cpp
+++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_api.cpp
@@ -76,7 +76,9 @@ TEST(TestRosbag2CPPAPI, minimal_writer_example)
     writer.write(bag_message, "/my/other/topic", "test_msgs/msg/BasicTypes");
 
     // yet another alternative, writing the rclcpp::SerializedMessage directly
-    writer.write(serialized_msg, "/yet/another/topic", "test_msgs/msg/BasicTypes", rclcpp::Clock().now());
+    writer.write(
+      serialized_msg, "/yet/another/topic", "test_msgs/msg/BasicTypes",
+      rclcpp::Clock().now());
 
     // writing a non-serialized message
     writer.write(test_msg, "/a/ros2/message", rclcpp::Clock().now());
@@ -108,6 +110,18 @@ TEST(TestRosbag2CPPAPI, minimal_writer_example)
     // close on scope exit
   }
 
+  // alternative reader
+  {
+    rosbag2_cpp::Reader reader;
+    reader.open(rosbag_directory.string());
+    while (reader.has_next()) {
+      TestMsgT extracted_test_msg = reader.read_next<TestMsgT>();
+      EXPECT_EQ(test_msg, extracted_test_msg);
+    }
+
+    // close on scope exit
+  }
+
   // remove the rosbag again after the test
   EXPECT_TRUE(rcpputils::fs::remove_all(rosbag_directory));
 }