Dnae adas/serialized message

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

rclcpp/include/rclcpp/serialization.hpp

@@ -0,0 +1,159 @@
+// 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 <string>
+
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
+
+#include "rcl/types.h"
+#include "rosidl_runtime_c/message_type_support_struct.h"
+
+namespace rclcpp
+{
+
+class SerializedMessage;
+
+/// Interface to (de)serialize a message
+class SerializationBase
+{
+protected:
+  explicit SerializationBase(const rosidl_message_type_support_t * type_support);
+
+  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;
+
+  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:
+  Serialization()
+  : SerializationBase(rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>())
+  {}
+
+  void serialize_message(
+    const MessageT & ros_message, SerializedMessage & serialized_message) const
+  {
+    SerializationBase::serialize_message(
+      reinterpret_cast<const void *>(&ros_message),
+      &serialized_message);
+  }
+
+  void deserialize_message(
+    const SerializedMessage & serialized_message, MessageT & ros_message) const
+  {
+    SerializationBase::deserialize_message(
+      &serialized_message,
+      reinterpret_cast<void *>(&ros_message));
+  }
+};
+
+template<>
+class Serialization<SerializedMessage>: public SerializationBase
+{
+public:
+  Serialization()
+  : SerializationBase(nullptr)
+  {}
+
+  void serialize_message(
+    const SerializedMessage & ros_message,
+    SerializedMessage & serialized_message) const
+  {
+    (void)ros_message;
+    (void)serialized_message;
+    throw std::runtime_error(
+            "Serialization of serialized message to serialized message is not possible.");
+  }
+
+  void deserialize_message(
+    const SerializedMessage & serialized_message,
+    SerializedMessage & ros_message) const
+  {
+    (void)ros_message;
+    (void)serialized_message;
+    throw std::runtime_error(
+            "Deserialization of serialized message to serialized message is not possible.");
+  }
+};
+
+template<>
+class Serialization<rcl_serialized_message_t>: public SerializationBase
+{
+public:
+  Serialization()
+  : SerializationBase(nullptr)
+  {}
+
+  void serialize_message(
+    rcl_serialized_message_t & ros_message,
+    const SerializedMessage & serialized_message) const
+  {
+    (void)ros_message;
+    (void)serialized_message;
+    throw std::runtime_error(
+            "Serialization of serialized message to serialized message is not possible.");
+  }
+
+  void deserialize_message(
+    const SerializedMessage & serialized_message,
+    rcl_serialized_message_t & ros_message) const
+  {
+    (void)ros_message;
+    (void)serialized_message;
+    throw std::runtime_error(
+            "Deserialization of serialized message to serialized message is not possible.");
+  }
+};
+
+// 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::false_type
+{};
+
+template<>
+struct is_serialized_message_class<SerializedMessage>
+  : std::true_type
+{};
+
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SERIALIZATION_HPP_

rclcpp/include/rclcpp/serialized_message.hpp

@@ -0,0 +1,76 @@
+// 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"
+
+namespace rclcpp
+{
+
+/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks
+class 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.
+   *
+   * \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 initial capacity of 0.
+   *
+   * \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);
+
+  /// Copy 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);
+
+  /// Move Constructor for a SerializedMessage from a rcl_serialized_message_t
+  explicit SerializedMessage(rcl_serialized_message_t && serialized_message);
+
+  SerializedMessage & operator=(const SerializedMessage & other);
+
+  SerializedMessage & operator=(const rcl_serialized_message_t & other);
+
+  SerializedMessage & operator=(SerializedMessage && other);
+
+  SerializedMessage & operator=(rcl_serialized_message_t && other);
+
+  /// Destructor for a SerializedMessage
+  ~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,68 @@
+// 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)
+{}
+
+void SerializationBase::serialize_message(
+  const void * ros_message, SerializedMessage * serialized_message) const
+{
+  rcpputils::check_true(type_support_ != nullptr, "Typesupport is nullpointer.");
+  rcpputils::check_true(ros_message != nullptr, "ROS message is nullpointer.");
+  rcpputils::check_true(serialized_message != nullptr, "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(type_support_ != nullptr, "Typesupport is nullpointer.");
+  rcpputils::check_true(serialized_message != nullptr, "Serialized message is nullpointer.");
+  rcpputils::check_true(
+    serialized_message->buffer_capacity != 0 &&
+    serialized_message->buffer_length != 0 &&
+    serialized_message->buffer != nullptr, "Serialized message is wrongly initialized.");
+  rcpputils::check_true(ros_message != nullptr, "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