From 2e68bd54383494360ba07bdc4e933640663ebb57 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 14 Oct 2015 14:14:16 -0700 Subject: [PATCH] publisher template on allocator --- .../rclcpp/allocator/allocator_deleter.hpp | 56 +++++++ .../rclcpp/allocator/allocator_factory.hpp | 69 +++++++++ .../rclcpp/allocator/allocator_wrapper.hpp | 141 ++++++++++++++++++ rclcpp/include/rclcpp/publisher.hpp | 26 +++- 4 files changed, 286 insertions(+), 6 deletions(-) create mode 100644 rclcpp/include/rclcpp/allocator/allocator_deleter.hpp create mode 100644 rclcpp/include/rclcpp/allocator/allocator_factory.hpp create mode 100644 rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp new file mode 100644 index 0000000..a757a8a --- /dev/null +++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp @@ -0,0 +1,56 @@ +// Copyright 2015 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_RCLCPP_ALLOCATOR_DELETER_HPP_ +#define RCLCPP_RCLCPP_ALLOCATOR_DELETER_HPP_ + +#include + +template +class AllocatorDeleter +{ +public: + AllocatorDeleter() + : allocator_(new Allocator) + { + } + + AllocatorDeleter(Allocator * a) + : allocator_(a) + { + } + + template + AllocatorDeleter(const AllocatorDeleter & a) + { + allocator_ = a.get_allocator(); + } + + void operator()(T * ptr) + { + std::allocator_traits::destroy(*allocator_, ptr); + std::allocator_traits::deallocate(*allocator_, ptr, sizeof(T)); + ptr = NULL; + } + + Allocator * get_allocator() const + { + return allocator_; + } + +private: + Allocator * allocator_; +}; + +#endif diff --git a/rclcpp/include/rclcpp/allocator/allocator_factory.hpp b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp new file mode 100644 index 0000000..4b58b74 --- /dev/null +++ b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp @@ -0,0 +1,69 @@ +// Copyright 2015 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_RCLCPP_ALLOCATOR_FACTORY_HPP_ +#define RCLCPP_RCLCPP_ALLOCATOR_FACTORY_HPP_ + +#include +#include +#include + +template class Alloc, typename T, typename D> +D initialize_deleter(Alloc * alloc) +{ + (void) alloc; + throw std::runtime_error("Reached unexpected template specialization"); +} + +template +std::default_delete initialize_deleter(std::allocator * alloc) +{ + (void) alloc; + return std::default_delete(); +} + +template class Alloc, typename T> +AllocatorDeleter> initialize_deleter(Alloc * alloc) +{ + if (!alloc) { + throw std::invalid_argument("Allocator argument was NULL"); + } + return AllocatorDeleter>(alloc); +} + +/* +class AllocatorFactoryBase +{ +public: + // Hmm + template + virtual void AllocT * request_allocator(...) = 0; + + template + virtual void return_allocator(AllocT * allocator); + + template + virtual DeleterT * get_deleter_for_allocator(AllocT * allocator) = 0; + +}; +*/ + +template class Alloc> +using Deleter = typename std::conditional< + std::is_same, std::allocator>::value, + std::default_delete, + AllocatorDeleter> + >::type; + +#endif diff --git a/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp new file mode 100644 index 0000000..30f6f7b --- /dev/null +++ b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp @@ -0,0 +1,141 @@ +// Copyright 2015 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_RCLCPP_ALLOCATOR_WRAPPER_HPP_ +#define RCLCPP_RCLCPP_ALLOCATOR_WRAPPER_HPP_ + +#include +#include +#include + + +template +void initialize_deleter(D * deleter, Alloc * alloc) +{ + (void) deleter; + (void) alloc; + throw std::runtime_error("Reached unexpected template specialization"); +} + +template +void initialize_deleter(std::default_delete * deleter, std::allocator * alloc) +{ + (void) alloc; + deleter = new std::default_delete; + if (!deleter) { + throw std::runtime_error("initialize_deleter failed"); + } +} + +template +void initialize_deleter(AllocatorDeleter * deleter, Alloc * alloc) +{ + if (!alloc) { + throw std::invalid_argument("Allocator argument was NULL"); + } + deleter = new AllocatorDeleter(alloc); + if (!deleter) { + throw std::runtime_error("initialize_deleter failed"); + } +} + +template +class AllocatorWrapper +{ +public: + using Deleter = typename std::conditional< + std::is_same>::value, + std::default_delete, + AllocatorDeleter + >::type; + + AllocatorWrapper(Alloc * allocator) + : allocator_(allocator) + { + if (!allocator_) { + throw std::invalid_argument("Allocator argument was NULL"); + } + initialize_deleter(deleter_, allocator_); + if (deleter_ == NULL) { + throw std::invalid_argument("Failed to initialize deleter"); + } + } + + AllocatorWrapper(Alloc * allocator, Deleter * deleter) + : allocator_(allocator), deleter_(deleter) + { + if (!allocator_) { + throw std::invalid_argument("Allocator argument was NULL"); + } + if (!deleter_) { + throw std::invalid_argument("Deleter argument was NULL"); + } + } + + AllocatorWrapper(Alloc & allocator) + { + allocator_ = &allocator; + if (!allocator_) { + throw std::invalid_argument("Allocator argument was NULL"); + } + initialize_deleter(deleter_, allocator_); + if (!deleter_) { + throw std::invalid_argument("Failed to initialize deleter"); + } + } + + AllocatorWrapper() + { + allocator_ = new Alloc(); + initialize_deleter(deleter_, allocator_); + if (!deleter_) { + //throw std::invalid_argument("Failed to initialize deleter"); + deleter_ = new Deleter; + } + } + + T * allocate(size_t size) + { + return std::allocator_traits::allocate(*allocator_, size); + } + + T * deallocate(void * pointer, size_t size) + { + std::allocator_traits::deallocate(*allocator_, pointer, size); + } + + template + void construct(T * pointer, Args && ... args) + { + std::allocator_traits::construct(*allocator_, pointer, std::forward(args)...); + } + + Deleter * get_deleter() const + { + return deleter_; + } + Alloc * get_underlying_allocator() const + { + return allocator_; + } + +private: + Alloc * allocator_; + Deleter * deleter_; +}; + +template +using DefaultAllocator = AllocatorWrapper>; + +#endif diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index d75ed05..f81e689 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -28,6 +28,8 @@ #include #include +#include + namespace rclcpp { @@ -203,13 +205,13 @@ protected: }; /// A publisher publishes messages of any type to a topic. -template +template class Alloc = std::allocator> class Publisher : public PublisherBase { friend rclcpp::node::Node; public: - RCLCPP_SMART_PTR_DEFINITIONS(Publisher); + RCLCPP_SMART_PTR_DEFINITIONS(Publisher); Publisher( std::shared_ptr node_handle, @@ -217,7 +219,10 @@ public: std::string topic, size_t queue_size) : PublisherBase(node_handle, publisher_handle, topic, queue_size) - {} + { + // TODO: avoid messy initialization + message_deleter_ = initialize_deleter(&message_allocator_); + } /// Send a message to the topic for this publisher. @@ -274,7 +279,9 @@ public: // The intra process manager should probably also be able to store // shared_ptr's and do the "smart" thing based on other intra process // subscriptions. For now call the other publish(). - std::unique_ptr unique_msg(new MessageT(*msg.get())); + auto ptr = std::allocator_traits>::allocate(message_allocator_, 1); + std::allocator_traits>::construct(message_allocator_, ptr, *msg.get()); + std::unique_ptr> unique_msg(ptr, message_deleter_); return this->publish(unique_msg); } @@ -291,7 +298,9 @@ public: // The intra process manager should probably also be able to store // shared_ptr's and do the "smart" thing based on other intra process // subscriptions. For now call the other publish(). - std::unique_ptr unique_msg(new MessageT(*msg.get())); + auto ptr = std::allocator_traits>::allocate(message_allocator_, 1); + std::allocator_traits>::construct(message_allocator_, ptr, *msg.get()); + std::unique_ptr> unique_msg(ptr, message_deleter_); return this->publish(unique_msg); } @@ -304,7 +313,9 @@ public: return this->do_inter_process_publish(&msg); } // Otherwise we have to allocate memory in a unique_ptr and pass it along. - std::unique_ptr unique_msg(new MessageT(msg)); + auto ptr = std::allocator_traits>::allocate(message_allocator_, 1); + std::allocator_traits>::construct(message_allocator_, ptr, msg); + std::unique_ptr> unique_msg(ptr, message_deleter_); return this->publish(unique_msg); } @@ -321,6 +332,9 @@ protected: } } + Alloc message_allocator_; + Deleter message_deleter_; + }; } /* namespace publisher */