From 1fc2d58799a6d4530522be5c236c70be53455e60 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 4 May 2021 19:45:15 -0300 Subject: [PATCH] Keep custom allocator in publisher and subscription options alive. (#1647) * Keep custom allocator in publisher and subscription options alive. Also, enforce an allocator bound to void is used to avoid surprises. Signed-off-by: Michel Hidalgo * Avoid sizeof(void) in MyAllocator implementation. Signed-off-by: Michel Hidalgo * Address peer review comment Signed-off-by: Ivan Santiago Paunovic * Use a lazely initialized private field when 'allocator' is not initialized Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/publisher_options.hpp | 20 +++++++++++++----- .../include/rclcpp/subscription_options.hpp | 21 +++++++++++++------ ..._intra_process_manager_with_allocators.cpp | 6 +++++- 3 files changed, 35 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 8388f85e47..efaa4b48c8 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "rcl/publisher.h" @@ -64,6 +65,10 @@ struct PublisherOptionsBase template struct PublisherOptionsWithAllocator : public PublisherOptionsBase { + static_assert( + std::is_void_v::value_type>, + "Publisher allocator value type must be void"); + /// Optional custom allocator. std::shared_ptr allocator = nullptr; @@ -80,10 +85,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase to_rcl_publisher_options(const rclcpp::QoS & qos) const { rcl_publisher_options_t result = rcl_publisher_get_default_options(); - using AllocatorTraits = std::allocator_traits; - using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; - auto message_alloc = std::make_shared(*this->get_allocator().get()); - result.allocator = rclcpp::allocator::get_rcl_allocator(*message_alloc); + result.allocator = rclcpp::allocator::get_rcl_allocator(*this->get_allocator()); result.qos = qos.get_rmw_qos_profile(); result.rmw_publisher_options.require_unique_network_flow_endpoints = this->require_unique_network_flow_endpoints; @@ -102,10 +104,18 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase get_allocator() const { if (!this->allocator) { - return std::make_shared(); + if (!allocator_storage_) { + allocator_storage_ = std::make_shared(); + } + return allocator_storage_; } return this->allocator; } + +private: + // This is a temporal workaround, to make sure that get_allocator() + // always returns a copy of the same allocator. + mutable std::shared_ptr allocator_storage_; }; using PublisherOptions = PublisherOptionsWithAllocator>; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index d9d4085d85..373850e00b 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "rclcpp/callback_group.hpp" @@ -86,6 +87,10 @@ struct SubscriptionOptionsBase template struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase { + static_assert( + std::is_void_v::value_type>, + "Subscription allocator value type must be void"); + /// Optional custom allocator. std::shared_ptr allocator = nullptr; @@ -107,10 +112,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase to_rcl_subscription_options(const rclcpp::QoS & qos) const { rcl_subscription_options_t result = rcl_subscription_get_default_options(); - using AllocatorTraits = std::allocator_traits; - using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; - auto message_alloc = std::make_shared(*this->get_allocator().get()); - result.allocator = allocator::get_rcl_allocator(*message_alloc); + result.allocator = allocator::get_rcl_allocator(*this->get_allocator()); result.qos = qos.get_rmw_qos_profile(); result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications; result.rmw_subscription_options.require_unique_network_flow_endpoints = @@ -124,15 +126,22 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase return result; } - /// Get the allocator, creating one if needed. std::shared_ptr get_allocator() const { if (!this->allocator) { - return std::make_shared(); + if (!allocator_storage_) { + allocator_storage_ = std::make_shared(); + } + return allocator_storage_; } return this->allocator; } + +private: + // This is a temporal workaround, to make sure that get_allocator() + // always returns a copy of the same allocator. + mutable std::shared_ptr allocator_storage_; }; using SubscriptionOptions = SubscriptionOptionsWithAllocator>; diff --git a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp index f8cb96a421..8443706126 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "test_msgs/msg/empty.hpp" @@ -57,7 +58,10 @@ struct MyAllocator return nullptr; } num_allocs++; - return static_cast(std::malloc(size * sizeof(T))); + // Use sizeof(char) in place for sizeof(void) + constexpr size_t value_size = sizeof( + typename std::conditional::value, T, char>::type); + return static_cast(std::malloc(size * value_size)); } void deallocate(T * ptr, size_t size)