introduce resize method for serialized message

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

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();
   }

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_;
 };

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

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) {

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();