From 010679c42f188faed66ae53ed3c2cb3ad34797d4 Mon Sep 17 00:00:00 2001
From: Karsten Knese <karsten@openrobotics.org>
Date: Wed, 22 Apr 2020 00:32:17 -0700
Subject: [PATCH] introduce resize method for serialized message

Signed-off-by: Karsten Knese <karsten@openrobotics.org>
---
 .../include/rclcpp/message_memory_strategy.hpp |  3 ++-
 rclcpp/include/rclcpp/serialized_message.hpp   |  7 +++++++
 rclcpp/src/rclcpp/serialized_message.cpp       |  9 +++++++++
 rclcpp/src/rclcpp/subscription_base.cpp        |  2 +-
 .../test/test_serialized_message_allocator.cpp | 18 +++++++++---------
 5 files changed, 28 insertions(+), 11 deletions(-)

diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp
index 6845e4b72f..6e4c0be065 100644
--- a/rclcpp/include/rclcpp/message_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp
@@ -108,7 +108,8 @@ class MessageMemoryStrategy
     msg.reset();
   }
 
-  virtual void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & serialized_msg)
+  virtual void return_serialized_message(
+    std::shared_ptr<rclcpp::SerializedMessage> & serialized_msg)
   {
     serialized_msg.reset();
   }
diff --git a/rclcpp/include/rclcpp/serialized_message.hpp b/rclcpp/include/rclcpp/serialized_message.hpp
index 13c2d1910b..9d0780e0ce 100644
--- a/rclcpp/include/rclcpp/serialized_message.hpp
+++ b/rclcpp/include/rclcpp/serialized_message.hpp
@@ -98,6 +98,13 @@ class RCLCPP_PUBLIC_TYPE SerializedMessage
    */
   size_t capacity() const;
 
+  /// Allocate memory in the data buffer
+  /**
+   * The data buffer of the underlying rcl_serialized_message_t will be resized.
+   * This might change the data layout and invalidates all pointers to the data.
+   */
+  void reserve(size_t capacity);
+
 private:
   rcl_serialized_message_t serialized_message_;
 };
diff --git a/rclcpp/src/rclcpp/serialized_message.cpp b/rclcpp/src/rclcpp/serialized_message.cpp
index a667977cea..65a0f49c50 100644
--- a/rclcpp/src/rclcpp/serialized_message.cpp
+++ b/rclcpp/src/rclcpp/serialized_message.cpp
@@ -144,4 +144,13 @@ size_t SerializedMessage::capacity() const
 {
   return serialized_message_.buffer_capacity;
 }
+
+void SerializedMessage::reserve(size_t capacity)
+{
+  auto ret = rmw_serialized_message_resize(&serialized_message_, capacity);
+  if (ret != RCL_RET_OK) {
+    throw std::runtime_error("failed to resize serialized message");
+  }
+}
+
 }  // namespace rclcpp
diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp
index b41de76a31..de1acfadb2 100644
--- a/rclcpp/src/rclcpp/subscription_base.cpp
+++ b/rclcpp/src/rclcpp/subscription_base.cpp
@@ -164,7 +164,7 @@ SubscriptionBase::take_serialized(
 {
   rcl_ret_t ret = rcl_take_serialized_message(
     this->get_subscription_handle().get(),
-    &message_out,
+    &message_out.get_rcl_serialized_message(),
     &message_info_out.get_rmw_message_info(),
     nullptr);
   if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
diff --git a/rclcpp/test/test_serialized_message_allocator.cpp b/rclcpp/test/test_serialized_message_allocator.cpp
index bc177722e4..968ad0ac2d 100644
--- a/rclcpp/test/test_serialized_message_allocator.cpp
+++ b/rclcpp/test/test_serialized_message_allocator.cpp
@@ -29,25 +29,25 @@ TEST(TestSerializedMessageAllocator, default_allocator) {
     rclcpp::message_memory_strategy::MessageMemoryStrategy<DummyMessageT>::create_default();
 
   auto msg0 = mem_strategy->borrow_serialized_message();
-  ASSERT_EQ(msg0->buffer_capacity, 0u);
+  ASSERT_EQ(msg0->capacity(), 0u);
   mem_strategy->return_serialized_message(msg0);
 
   auto msg100 = mem_strategy->borrow_serialized_message(100);
-  ASSERT_EQ(msg100->buffer_capacity, 100u);
+  ASSERT_EQ(msg100->capacity(), 100u);
   mem_strategy->return_serialized_message(msg100);
 
   auto msg200 = mem_strategy->borrow_serialized_message();
-  auto ret = rmw_serialized_message_resize(msg200.get(), 200);
+  auto ret = rmw_serialized_message_resize(&msg200->get_rcl_serialized_message(), 200);
   ASSERT_EQ(RCL_RET_OK, ret);
-  EXPECT_EQ(0u, msg200->buffer_length);
-  EXPECT_EQ(200u, msg200->buffer_capacity);
+  EXPECT_EQ(0u, msg200->size());
+  EXPECT_EQ(200u, msg200->capacity());
   mem_strategy->return_serialized_message(msg200);
 
   auto msg1000 = mem_strategy->borrow_serialized_message(1000);
-  ASSERT_EQ(msg1000->buffer_capacity, 1000u);
-  ret = rmw_serialized_message_resize(msg1000.get(), 2000);
+  ASSERT_EQ(msg1000->capacity(), 1000u);
+  ret = rmw_serialized_message_resize(&msg1000->get_rcl_serialized_message(), 2000);
   ASSERT_EQ(RCL_RET_OK, ret);
-  EXPECT_EQ(2000u, msg1000->buffer_capacity);
+  EXPECT_EQ(2000u, msg1000->capacity());
   mem_strategy->return_serialized_message(msg1000);
 }
 
@@ -61,7 +61,7 @@ TEST(TestSerializedMessageAllocator, borrow_from_subscription) {
     [](std::shared_ptr<test_msgs::msg::Empty> test_msg) {(void) test_msg;});
 
   auto msg0 = sub->create_serialized_message();
-  EXPECT_EQ(0u, msg0->buffer_capacity);
+  EXPECT_EQ(0u, msg0->capacity());
   sub->return_serialized_message(msg0);
 
   rclcpp::shutdown();