Dnae adas/serialized message (#1075)

* Addes SerializedMessage and helper class for serialization to rcl_serialized_message

@Karsten1987 Thank you for your support

Signed-off-by: Joshua Hampp <j.hampp@denso-adas.de>

* Updateds subscription traits for SerializedMessage

@Karsten1987 Thank you for your support

Signed-off-by: Joshua Hampp <j.hampp@denso-adas.de>

* Addes tests SerializedMessage and subscription traits

@Karsten1987 Thank you for your support

Signed-off-by: Joshua Hampp <j.hampp@denso-adas.de>

* Update rclcpp/include/rclcpp/serialization.hpp

Co-Authored-By: Karsten Knese <Karsten1987@users.noreply.github.com>

* Update rclcpp/test/test_serialized_message.cpp

Co-Authored-By: Karsten Knese <Karsten1987@users.noreply.github.com>

* fix windows compilation

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* cosmetic touchups

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

Co-authored-by: Joshua Hampp <j.hampp@denso-adas.de>
Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>
Co-authored-by: Karsten Knese <karsten@openrobotics.org>

rclcpp/CMakeLists.txt

@@ -79,6 +79,8 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/publisher_base.cpp
   src/rclcpp/qos.cpp
   src/rclcpp/qos_event.cpp
+  src/rclcpp/serialization.cpp
+  src/rclcpp/serialized_message.cpp
   src/rclcpp/service.cpp
   src/rclcpp/signal_handler.cpp
   src/rclcpp/subscription_base.cpp
@@ -399,6 +401,15 @@ if(BUILD_TESTING)
       ${PROJECT_NAME}
     )
   endif()
+  ament_add_gtest(test_serialized_message test/test_serialized_message.cpp)
+  if(TARGET test_serialized_message)
+    ament_target_dependencies(test_serialized_message
+      test_msgs
+    )
+    target_link_libraries(test_serialized_message
+      ${PROJECT_NAME}
+    )
+  endif()
   ament_add_gtest(test_service test/test_service.cpp)
   if(TARGET test_service)
     ament_target_dependencies(test_service
@@ -437,6 +448,7 @@ if(BUILD_TESTING)
       "rcl"
       "test_msgs"
     )
+    target_link_libraries(test_subscription_traits ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
   if(TARGET test_find_weak_nodes)

rclcpp/include/rclcpp/serialization.hpp

@@ -0,0 +1,102 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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 RCLCPP__SERIALIZATION_HPP_
+#define RCLCPP__SERIALIZATION_HPP_
+
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/types.h"
+
+#include "rosidl_runtime_c/message_type_support_struct.h"
+
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
+
+namespace rclcpp
+{
+
+class SerializedMessage;
+
+namespace serialization_traits
+{
+// trait to check if type is the object oriented serialized message
+template<typename T>
+struct is_serialized_message_class : std::false_type
+{};
+
+template<>
+struct is_serialized_message_class<rcl_serialized_message_t>: std::true_type
+{};
+
+template<>
+struct is_serialized_message_class<SerializedMessage>: std::true_type
+{};
+}  // namespace serialization_traits
+
+/// Interface to (de)serialize a message
+class RCLCPP_PUBLIC_TYPE SerializationBase
+{
+public:
+  /// Constructor of SerializationBase
+  /**
+   * \param[in] type_support handle for the message type support
+   * to be used for serialization and deserialization.
+   */
+  explicit SerializationBase(const rosidl_message_type_support_t * type_support);
+
+  /// Destructor of SerializationBase
+  virtual ~SerializationBase() = default;
+
+  /// Serialize a ROS2 message to a serialized stream
+  /**
+   * \param[in] message The ROS2 message which is read and serialized by rmw.
+   * \param[out] serialized_message The serialized message.
+   */
+  void serialize_message(
+    const void * ros_message, SerializedMessage * serialized_message) const;
+
+  /// Deserialize a serialized stream to a ROS message
+  /**
+   * \param[in] serialized_message The serialized message to be converted to ROS2 by rmw.
+   * \param[out] message The deserialized ROS2 message.
+   */
+  void deserialize_message(
+    const SerializedMessage * serialized_message, void * ros_message) const;
+
+private:
+  const rosidl_message_type_support_t * type_support_;
+};
+
+/// Default implementation to (de)serialize a message by using rmw_(de)serialize
+template<typename MessageT>
+class Serialization : public SerializationBase
+{
+public:
+  /// Constructor of Serialization
+  Serialization()
+  : SerializationBase(rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>())
+  {
+    static_assert(
+      !serialization_traits::is_serialized_message_class<MessageT>::value,
+      "Serialization of serialized message to serialized message is not possible.");
+  }
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SERIALIZATION_HPP_

rclcpp/include/rclcpp/serialized_message.hpp

@@ -0,0 +1,84 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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 RCLCPP__SERIALIZED_MESSAGE_HPP_
+#define RCLCPP__SERIALIZED_MESSAGE_HPP_
+
+#include "rcl/allocator.h"
+#include "rcl/types.h"
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks
+class RCLCPP_PUBLIC_TYPE SerializedMessage : public rcl_serialized_message_t
+{
+public:
+  /// Default constructor for a SerializedMessage
+  /**
+   * Default constructs a serialized message and initalizes it
+   * with initial capacity of 0.
+   * The allocator defaults to `rcl_get_default_allocator()`.
+   *
+   * \param[in] allocator The allocator to be used for the initialization.
+   */
+  explicit SerializedMessage(
+    const rcl_allocator_t & allocator = rcl_get_default_allocator());
+
+  /// Default constructor for a SerializedMessage
+  /**
+   * Default constructs a serialized message and initalizes it
+   * with the provided capacity.
+   * The allocator defaults to `rcl_get_default_allocator()`.
+   *
+   * \param[in] initial_capacity The amount of memory to be allocated.
+   * \param[in] allocator The allocator to be used for the initialization.
+   */
+  SerializedMessage(
+    size_t initial_capacity,
+    const rcl_allocator_t & allocator = rcl_get_default_allocator());
+
+  /// Copy Constructor for a SerializedMessage
+  SerializedMessage(const SerializedMessage & serialized_message);
+
+  /// Constructor for a SerializedMessage from a rcl_serialized_message_t
+  explicit SerializedMessage(const rcl_serialized_message_t & serialized_message);
+
+  /// Move Constructor for a SerializedMessage
+  SerializedMessage(SerializedMessage && serialized_message);
+
+  /// Constructor for a SerializedMessage from a moved rcl_serialized_message_t
+  explicit SerializedMessage(rcl_serialized_message_t && serialized_message);
+
+  /// Copy assignment operator
+  SerializedMessage & operator=(const SerializedMessage & other);
+
+  /// Copy assignment operator from a rcl_serialized_message_t
+  SerializedMessage & operator=(const rcl_serialized_message_t & other);
+
+  /// Move assignment operator
+  SerializedMessage & operator=(SerializedMessage && other);
+
+  /// Move assignment operator from a rcl_serialized_message_t
+  SerializedMessage & operator=(rcl_serialized_message_t && other);
+
+  /// Destructor for a SerializedMessage
+  virtual ~SerializedMessage();
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SERIALIZED_MESSAGE_HPP_

rclcpp/include/rclcpp/subscription_traits.hpp

@@ -18,6 +18,8 @@
 #include <memory>
 
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_options.hpp"
 #include "rcl/types.h"
 
 namespace rclcpp
@@ -49,6 +51,15 @@ struct is_serialized_subscription_argument<std::shared_ptr<rcl_serialized_messag
   : std::true_type
 {};
 
+template<>
+struct is_serialized_subscription_argument<SerializedMessage>: std::true_type
+{};
+
+template<>
+struct is_serialized_subscription_argument<std::shared_ptr<SerializedMessage>>
+  : std::true_type
+{};
+
 template<typename T>
 struct is_serialized_subscription : is_serialized_subscription_argument<T>
 {};
@@ -75,6 +86,7 @@ struct extract_message_type<std::unique_ptr<MessageT, Deleter>>: extract_message
 
 template<
   typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
   // Do not attempt if CallbackT is an integer (mistaken for depth)
   typename = std::enable_if_t<!std::is_integral<
     std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
@@ -85,6 +97,10 @@ template<
   // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile)
   typename = std::enable_if_t<!std::is_same<
     rmw_qos_profile_t,
+    std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
+  // Do not attempt if CallbackT is a rclcpp::SubscriptionOptionsWithAllocator
+  typename = std::enable_if_t<!std::is_same<
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>,
     std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>
 >
 struct has_message_type : extract_message_type<

rclcpp/src/rclcpp/serialization.cpp

@@ -0,0 +1,69 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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 "rclcpp/serialization.hpp"
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/serialized_message.hpp"
+
+#include "rcpputils/asserts.hpp"
+
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+SerializationBase::SerializationBase(const rosidl_message_type_support_t * type_support)
+: type_support_(type_support)
+{
+  rcpputils::check_true(nullptr != type_support, "Typesupport is nullpointer.");
+}
+
+void SerializationBase::serialize_message(
+  const void * ros_message, SerializedMessage * serialized_message) const
+{
+  rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer.");
+  rcpputils::check_true(nullptr != ros_message, "ROS message is nullpointer.");
+  rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer.");
+
+  const auto ret = rmw_serialize(
+    ros_message,
+    type_support_,
+    serialized_message);
+  if (ret != RMW_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to serialize ROS message.");
+  }
+}
+
+void SerializationBase::deserialize_message(
+  const SerializedMessage * serialized_message, void * ros_message) const
+{
+  rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer.");
+  rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer.");
+  rcpputils::check_true(
+    0 != serialized_message->buffer_capacity &&
+    0 != serialized_message->buffer_length &&
+    nullptr != serialized_message->buffer, "Serialized message is wrongly initialized.");
+  rcpputils::check_true(nullptr != ros_message, "ROS message is a nullpointer.");
+
+  const auto ret = rmw_deserialize(serialized_message, type_support_, ros_message);
+  if (ret != RMW_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to deserialize ROS message.");
+  }
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/serialized_message.cpp

@@ -0,0 +1,134 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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 "rclcpp/serialized_message.hpp"
+
+#include <cstring>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+
+#include "rmw/types.h"
+
+namespace rclcpp
+{
+
+/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks
+SerializedMessage::SerializedMessage(const rcl_allocator_t & allocator)
+: SerializedMessage(0u, allocator)
+{}
+
+SerializedMessage::SerializedMessage(
+  size_t initial_capacity, const rcl_allocator_t & allocator)
+: rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message())
+{
+  const auto ret = rmw_serialized_message_init(
+    this, initial_capacity, &allocator);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+SerializedMessage::SerializedMessage(const SerializedMessage & serialized_message)
+: SerializedMessage(static_cast<const rcl_serialized_message_t &>(serialized_message))
+{}
+
+SerializedMessage::SerializedMessage(const rcl_serialized_message_t & serialized_message)
+: rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message())
+{
+  const auto ret = rmw_serialized_message_init(
+    this, serialized_message.buffer_capacity, &serialized_message.allocator);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  // do not call memcpy if the pointer is "static"
+  if (buffer != serialized_message.buffer) {
+    std::memcpy(buffer, serialized_message.buffer, serialized_message.buffer_length);
+  }
+  buffer_length = serialized_message.buffer_length;
+}
+
+SerializedMessage::SerializedMessage(SerializedMessage && serialized_message)
+: SerializedMessage(
+    std::forward<rcl_serialized_message_t>(
+      static_cast<rcl_serialized_message_t &&>(serialized_message)))
+{}
+
+SerializedMessage::SerializedMessage(rcl_serialized_message_t && serialized_message)
+: rcl_serialized_message_t(serialized_message)
+{
+  // reset buffer to prevent double free
+  serialized_message = rmw_get_zero_initialized_serialized_message();
+}
+
+SerializedMessage & SerializedMessage::operator=(const SerializedMessage & other)
+{
+  *this = static_cast<const rcl_serialized_message_t &>(other);
+
+  return *this;
+}
+
+SerializedMessage & SerializedMessage::operator=(const rcl_serialized_message_t & other)
+{
+  *this = static_cast<SerializedMessage>(rmw_get_zero_initialized_serialized_message());
+
+  const auto ret = rmw_serialized_message_init(
+    this, other.buffer_capacity, &other.allocator);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  // do not call memcpy if the pointer is "static"
+  if (buffer != other.buffer) {
+    std::memcpy(buffer, other.buffer, other.buffer_length);
+  }
+  buffer_length = other.buffer_length;
+
+  return *this;
+}
+
+SerializedMessage & SerializedMessage::operator=(SerializedMessage && other)
+{
+  *this = static_cast<rcl_serialized_message_t &&>(other);
+
+  return *this;
+}
+
+SerializedMessage & SerializedMessage::operator=(rcl_serialized_message_t && other)
+{
+  this->buffer = other.buffer;
+  this->buffer_capacity = other.buffer_length;
+  this->buffer_length = other.buffer_length;
+  this->allocator = other.allocator;
+
+  // reset original to prevent double free
+  other = rmw_get_zero_initialized_serialized_message();
+
+  return *this;
+}
+
+SerializedMessage::~SerializedMessage()
+{
+  if (nullptr != buffer) {
+    const auto fini_ret = rmw_serialized_message_fini(this);
+    if (RCL_RET_OK != fini_ret) {
+      RCLCPP_ERROR(
+        get_logger("rclcpp"),
+        "Failed to destroy serialized message: %s", rcl_get_error_string().str);
+    }
+  }
+}
+
+}  // namespace rclcpp

rclcpp/test/test_serialized_message.cpp

@@ -0,0 +1,149 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// 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 <gtest/gtest.h>
+
+#include <cstring>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/serialization.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcpputils/asserts.hpp"
+
+#include "test_msgs/message_fixtures.hpp"
+#include "test_msgs/msg/basic_types.hpp"
+
+TEST(TestSerializedMessage, empty_initialize) {
+  rclcpp::SerializedMessage serialized_message;
+  EXPECT_TRUE(serialized_message.buffer == nullptr);
+  EXPECT_EQ(0u, serialized_message.buffer_length);
+  EXPECT_EQ(0u, serialized_message.buffer_capacity);
+}
+
+TEST(TestSerializedMessage, initialize_with_capacity) {
+  rclcpp::SerializedMessage serialized_message(13);
+  EXPECT_TRUE(serialized_message.buffer != nullptr);
+  EXPECT_EQ(0u, serialized_message.buffer_length);
+  EXPECT_EQ(13u, serialized_message.buffer_capacity);
+}
+
+TEST(TestSerializedMessage, various_constructors) {
+  const std::string content = "Hello World";
+  const auto content_size = content.size() + 1;  // accounting for null terminator
+
+  rclcpp::SerializedMessage serialized_message(content_size);
+  // manually copy some content
+  std::memcpy(serialized_message.buffer, content.c_str(), content.size());
+  serialized_message.buffer[content.size()] = '\0';
+  serialized_message.buffer_length = content_size;
+  EXPECT_STREQ(content.c_str(), reinterpret_cast<char *>(serialized_message.buffer));
+  EXPECT_EQ(content_size, serialized_message.buffer_capacity);
+
+  // Copy Constructor
+  rclcpp::SerializedMessage other_serialized_message(serialized_message);
+  EXPECT_EQ(content_size, other_serialized_message.buffer_capacity);
+  EXPECT_EQ(content_size, other_serialized_message.buffer_length);
+  EXPECT_STREQ(
+    reinterpret_cast<char *>(serialized_message.buffer),
+    reinterpret_cast<char *>(other_serialized_message.buffer));
+
+  // Move Constructor
+  rclcpp::SerializedMessage yet_another_serialized_message(std::move(other_serialized_message));
+  EXPECT_TRUE(other_serialized_message.buffer == nullptr);
+  EXPECT_EQ(0u, other_serialized_message.buffer_capacity);
+  EXPECT_EQ(0u, other_serialized_message.buffer_length);
+
+  auto default_allocator = rcl_get_default_allocator();
+  auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message();
+  auto yet_another_rcl_serialized_msg = rmw_get_zero_initialized_serialized_message();
+  auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator);
+  ASSERT_EQ(RCL_RET_OK, ret);
+  ret = rmw_serialized_message_init(&yet_another_rcl_serialized_msg, 13, &default_allocator);
+  ASSERT_EQ(RCL_RET_OK, ret);
+
+  // manually copy some content
+  std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content.size());
+  rcl_serialized_msg.buffer[content.size()] = '\0';
+  rcl_serialized_msg.buffer_length = content_size;
+  EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity);
+  std::memcpy(yet_another_rcl_serialized_msg.buffer, content.c_str(), content.size());
+  yet_another_rcl_serialized_msg.buffer[content.size()] = '\0';
+  yet_another_rcl_serialized_msg.buffer_length = content_size;
+  EXPECT_EQ(13u, yet_another_rcl_serialized_msg.buffer_capacity);
+
+  // Copy Constructor from rcl_serialized_message_t
+  rclcpp::SerializedMessage from_rcl_msg(rcl_serialized_msg);
+  EXPECT_EQ(13u, from_rcl_msg.buffer_capacity);
+  EXPECT_EQ(content_size, from_rcl_msg.buffer_length);
+
+  // Move Constructor from rcl_serialized_message_t
+  rclcpp::SerializedMessage yet_another_moved_serialized_message(std::move(
+      yet_another_rcl_serialized_msg));
+  EXPECT_TRUE(yet_another_rcl_serialized_msg.buffer == nullptr);
+  EXPECT_EQ(0u, yet_another_rcl_serialized_msg.buffer_capacity);
+  EXPECT_EQ(0u, yet_another_rcl_serialized_msg.buffer_length);
+
+  // Verify that despite being fini'd, the copy is real
+  ret = rmw_serialized_message_fini(&rcl_serialized_msg);
+  ASSERT_EQ(RCL_RET_OK, ret);
+  EXPECT_EQ(nullptr, rcl_serialized_msg.buffer);
+  EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity);
+  EXPECT_EQ(0u, rcl_serialized_msg.buffer_length);
+  EXPECT_TRUE(nullptr != from_rcl_msg.buffer);
+  EXPECT_EQ(13u, from_rcl_msg.buffer_capacity);
+  EXPECT_EQ(content_size, from_rcl_msg.buffer_length);
+}
+
+TEST(TestSerializedMessage, serialization) {
+  using MessageT = test_msgs::msg::BasicTypes;
+
+  rclcpp::Serialization<MessageT> serializer;
+
+  auto basic_type_ros_msgs = get_messages_basic_types();
+  for (const auto & ros_msg : basic_type_ros_msgs) {
+    // convert ros msg to serialized msg
+    rclcpp::SerializedMessage serialized_msg;
+    serializer.serialize_message(ros_msg.get(), &serialized_msg);
+
+    // convert serialized msg back to ros msg
+    MessageT deserialized_ros_msg;
+    serializer.deserialize_message(&serialized_msg, &deserialized_ros_msg);
+
+    EXPECT_EQ(*ros_msg, deserialized_ros_msg);
+  }
+}
+
+template<typename MessageT>
+void test_empty_msg_serialize()
+{
+  rclcpp::Serialization<MessageT> serializer;
+  MessageT ros_msg;
+  rclcpp::SerializedMessage serialized_msg;
+
+  serializer.serialize_message(&ros_msg, &serialized_msg);
+}
+
+template<typename MessageT>
+void test_empty_msg_deserialize()
+{
+  rclcpp::Serialization<MessageT> serializer;
+  MessageT ros_msg;
+  rclcpp::SerializedMessage serialized_msg;
+
+  serializer.deserialize_message(&serialized_msg, &ros_msg);
+}

rclcpp/test/test_subscription_traits.cpp

@@ -21,6 +21,7 @@
 #include "rcl/types.h"
 
 #include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/serialized_message.hpp"
 #include "rclcpp/subscription_traits.hpp"
 
 #include "test_msgs/msg/empty.hpp"
@@ -52,6 +53,16 @@ void not_serialized_unique_ptr_callback(
   (void) unused;
 }
 
+void rclcpp_serialized_callback_copy(rclcpp::SerializedMessage unused)
+{
+  (void) unused;
+}
+
+void rclcpp_serialized_callback_shared_ptr(std::shared_ptr<rclcpp::SerializedMessage> unused)
+{
+  (void) unused;
+}
+
 TEST(TestSubscriptionTraits, is_serialized_callback) {
   // Test regular functions
   auto cb1 = &serialized_callback_copy;
@@ -97,6 +108,16 @@ TEST(TestSubscriptionTraits, is_serialized_callback) {
   static_assert(
     rclcpp::subscription_traits::is_serialized_callback<decltype(cb7)>::value == false,
     "passing a fancy unique_ptr of test_msgs::msg::Empty is not a serialized callback");
+
+  auto cb8 = &rclcpp_serialized_callback_copy;
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb8)>::value == true,
+    "rclcpp::SerializedMessage in a first argument callback makes it a serialized callback");
+
+  auto cb9 = &rclcpp_serialized_callback_shared_ptr;
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb9)>::value == true,
+    "std::shared_ptr<rclcpp::SerializedMessage> in a callback makes it a serialized callback");
 }
 
 TEST(TestSubscriptionTraits, callback_messages) {