From 0f768bbd5a20bb10dd3be49cce3cb0e2bdc544c2 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 26 Apr 2021 19:01:08 -0300 Subject: [PATCH 1/4] 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 --- rclcpp/include/rclcpp/publisher_options.hpp | 13 +++++++------ rclcpp/include/rclcpp/subscription_options.hpp | 13 +++++++------ 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 8388f85e47..2b48bee8eb 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -64,8 +64,12 @@ struct PublisherOptionsBase template struct PublisherOptionsWithAllocator : public PublisherOptionsBase { + static_assert( + std::is_void::value_type>::value, + "Publisher allocator value type must be void"); + /// Optional custom allocator. - std::shared_ptr allocator = nullptr; + mutable std::shared_ptr allocator = nullptr; PublisherOptionsWithAllocator() {} @@ -80,10 +84,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,7 +103,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase get_allocator() const { if (!this->allocator) { - return std::make_shared(); + this->allocator = std::make_shared(); } return this->allocator; } diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index d9d4085d85..fe5c14bac1 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -86,8 +86,12 @@ struct SubscriptionOptionsBase template struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase { + static_assert( + std::is_void::value_type>::value, + "Subscription allocator value type must be void"); + /// Optional custom allocator. - std::shared_ptr allocator = nullptr; + mutable std::shared_ptr allocator = nullptr; SubscriptionOptionsWithAllocator() {} @@ -107,10 +111,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 = @@ -129,7 +130,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase get_allocator() const { if (!this->allocator) { - return std::make_shared(); + this->allocator = std::make_shared(); } return this->allocator; } From f4dcd4e56fea89102cbfe609948c9520f1694de5 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 29 Apr 2021 17:09:57 -0300 Subject: [PATCH 2/4] Avoid sizeof(void) in MyAllocator implementation. Signed-off-by: Michel Hidalgo --- .../rclcpp/test_intra_process_manager_with_allocators.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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) From 6ff1246a3de528ee829628453adac269a2b25709 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Apr 2021 12:24:30 -0300 Subject: [PATCH 3/4] Address peer review comment Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/publisher_options.hpp | 12 +++++++++--- rclcpp/include/rclcpp/subscription_options.hpp | 12 +++++++++--- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 2b48bee8eb..841dcc6851 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" @@ -65,11 +66,11 @@ template struct PublisherOptionsWithAllocator : public PublisherOptionsBase { static_assert( - std::is_void::value_type>::value, + std::is_void_v::value_type>, "Publisher allocator value type must be void"); /// Optional custom allocator. - mutable std::shared_ptr allocator = nullptr; + std::shared_ptr allocator = nullptr; PublisherOptionsWithAllocator() {} @@ -103,7 +104,12 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase get_allocator() const { if (!this->allocator) { - this->allocator = std::make_shared(); + if constexpr (std::is_same_v>) { + auto g_instance = std::make_shared>(); + return g_instance; + } else { + throw std::runtime_error("allocator is nullptr"); + } } return this->allocator; } diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index fe5c14bac1..590757162d 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" @@ -87,11 +88,11 @@ template struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase { static_assert( - std::is_void::value_type>::value, + std::is_void_v::value_type>, "Subscription allocator value type must be void"); /// Optional custom allocator. - mutable std::shared_ptr allocator = nullptr; + std::shared_ptr allocator = nullptr; SubscriptionOptionsWithAllocator() {} @@ -130,7 +131,12 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase get_allocator() const { if (!this->allocator) { - this->allocator = std::make_shared(); + if constexpr (std::is_same_v>) { + auto g_instance = std::make_shared>(); + return g_instance; + } else { + throw std::runtime_error("allocator is nullptr"); + } } return this->allocator; } From fe13993311aa5f0ce43b60039503a019b84353f4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 4 May 2021 17:21:41 -0300 Subject: [PATCH 4/4] Use a lazely initialized private field when 'allocator' is not initialized Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/publisher_options.hpp | 13 ++++++++----- rclcpp/include/rclcpp/subscription_options.hpp | 14 ++++++++------ 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 841dcc6851..efaa4b48c8 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -104,15 +104,18 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase get_allocator() const { if (!this->allocator) { - if constexpr (std::is_same_v>) { - auto g_instance = std::make_shared>(); - return g_instance; - } else { - throw std::runtime_error("allocator is nullptr"); + 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 590757162d..373850e00b 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -126,20 +126,22 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase return result; } - /// Get the allocator, creating one if needed. std::shared_ptr get_allocator() const { if (!this->allocator) { - if constexpr (std::is_same_v>) { - auto g_instance = std::make_shared>(); - return g_instance; - } else { - throw std::runtime_error("allocator is nullptr"); + 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>;