diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp index a757a8a..160c4e5 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp @@ -17,12 +17,18 @@ #include +namespace rclcpp +{ + +namespace allocator +{ + template class AllocatorDeleter { public: AllocatorDeleter() - : allocator_(new Allocator) + : allocator_(NULL) { } @@ -49,8 +55,71 @@ public: return allocator_; } + void set_allocator(Allocator * alloc) + { + allocator_ = alloc; + } + private: Allocator * allocator_; }; +/* +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); +} +*/ +template class Alloc, typename T, typename D> +void set_allocator_for_deleter(D * deleter, Alloc * alloc) +{ + (void) alloc; + throw std::runtime_error("Reached unexpected template specialization"); +} + +template +void set_allocator_for_deleter(std::default_delete * deleter, std::allocator * alloc) +{ + (void) deleter; + (void) alloc; +} + +template class Alloc, typename T> +void set_allocator_for_deleter(AllocatorDeleter> * deleter, Alloc * alloc) +{ + if (!deleter || !alloc) { + throw std::invalid_argument("Argument was NULL to set_allocator_for_deleter"); + } + //return AllocatorDeleter>(alloc); + deleter->set_allocator(alloc); +} + +template +using Deleter = typename std::conditional< + std::is_same>::value, + std::default_delete, + AllocatorDeleter + >::type; + +} +} + #endif diff --git a/rclcpp/include/rclcpp/allocator/allocator_factory.hpp b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp index 4b58b74..2a98b1f 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_factory.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp @@ -42,23 +42,6 @@ AllocatorDeleter> initialize_deleter(Alloc * alloc) 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, diff --git a/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp index 30f6f7b..99fc421 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp @@ -17,50 +17,39 @@ #include #include -#include +#include - -template -void initialize_deleter(D * deleter, Alloc * alloc) +namespace rclcpp { - (void) deleter; - (void) alloc; - throw std::runtime_error("Reached unexpected template specialization"); -} -template -void initialize_deleter(std::default_delete * deleter, std::allocator * alloc) +namespace allocator { - (void) alloc; - deleter = new std::default_delete; - if (!deleter) { - throw std::runtime_error("initialize_deleter failed"); - } -} -template -void initialize_deleter(AllocatorDeleter * deleter, Alloc * alloc) +// Type-erased interface to AllocatorWrapper +class AllocatorWrapper { - if (!alloc) { - throw std::invalid_argument("Allocator argument was NULL"); - } - deleter = new AllocatorDeleter(alloc); - if (!deleter) { - throw std::runtime_error("initialize_deleter failed"); - } + virtual void * allocate(size_t size) = 0; + virtual void deallocate(void * pointer, size_t size) = 0; + // Construct will have to be through placement new, since pure virtual function can't be templated + + virtual void destroy(T* pointer) = 0; } template -class AllocatorWrapper +class TypedAllocatorWrapper : public AllocatorWrapper { public: + /* using Deleter = typename std::conditional< std::is_same>::value, std::default_delete, AllocatorDeleter >::type; + */ - AllocatorWrapper(Alloc * allocator) + using DeleterT = Deleter; + + TypedAllocatorWrapper(Alloc * allocator) : allocator_(allocator) { if (!allocator_) { @@ -72,7 +61,7 @@ public: } } - AllocatorWrapper(Alloc * allocator, Deleter * deleter) + TypedAllocatorWrapper(Alloc * allocator, DeleterT * deleter) : allocator_(allocator), deleter_(deleter) { if (!allocator_) { @@ -83,7 +72,7 @@ public: } } - AllocatorWrapper(Alloc & allocator) + TypedAllocatorWrapper(Alloc & allocator) { allocator_ = &allocator; if (!allocator_) { @@ -95,22 +84,27 @@ public: } } - AllocatorWrapper() + TypedAllocatorWrapper() { allocator_ = new Alloc(); initialize_deleter(deleter_, allocator_); if (!deleter_) { //throw std::invalid_argument("Failed to initialize deleter"); - deleter_ = new Deleter; + deleter_ = new DeleterT; } } - T * allocate(size_t size) + void * allocate(size_t size) { return std::allocator_traits::allocate(*allocator_, size); } - T * deallocate(void * pointer, size_t size) + void deallocate(void * pointer, size_t size) + { + deallocate(static_cast(pointer), size); + } + + void deallocate(T * pointer, size_t size) { std::allocator_traits::deallocate(*allocator_, pointer, size); } @@ -121,7 +115,18 @@ public: std::allocator_traits::construct(*allocator_, pointer, std::forward(args)...); } - Deleter * get_deleter() const + void destroy(void * pointer) + { + destroy(static_cast(pointer)); + } + + template + void destroy(T * pointer) + { + std::allocator_traits::destroy(*allocator_, pointer); + } + + DeleterT * get_deleter() const { return deleter_; } @@ -132,10 +137,13 @@ public: private: Alloc * allocator_; - Deleter * deleter_; + DeleterT * deleter_; }; template -using DefaultAllocator = AllocatorWrapper>; +using DefaultAllocator = TypedAllocatorWrapper>; + +} +} #endif diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 6b64bec..e320651 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -202,25 +202,34 @@ public: FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - std::vector set_parameters( - const std::vector & parameters); + template + using StringRebind = typename Alloc::template rebind::other; + template> + typename std::vector set_parameters( + const typename std::vector & parameters); + + template> rcl_interfaces::msg::SetParametersResult set_parameters_atomically( - const std::vector & parameters); + const typename std::vector & parameters); - std::vector get_parameters( - const std::vector & names) const; + template> + typename std::vector get_parameters( + const typename std::vector> & names) const; - std::vector describe_parameters( - const std::vector & names) const; + template> + typename std::vector describe_parameters( + const typename std::vector>> & names) const; - std::vector get_parameter_types( - const std::vector & names) const; + template> + typename std::vector get_parameter_types( + const typename std::vector>> & names) const; rcl_interfaces::msg::ListParametersResult list_parameters( const std::vector & prefixes, uint64_t depth) const; - std::map get_topic_names_and_types() const; + template>> + typename std::map get_topic_names_and_types() const; size_t count_publishers(const std::string & topic_name) const; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 8a9c730..f568536 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -443,6 +443,7 @@ Node::create_service( return serv; } +template std::vector Node::set_parameters( const std::vector & parameters) @@ -455,6 +456,7 @@ Node::set_parameters( return results; } +template rcl_interfaces::msg::SetParametersResult Node::set_parameters_atomically( const std::vector & parameters) @@ -489,6 +491,7 @@ Node::set_parameters_atomically( return result; } +template std::vector Node::get_parameters( const std::vector & names) const @@ -508,6 +511,7 @@ Node::get_parameters( return results; } +template std::vector Node::describe_parameters( const std::vector & names) const @@ -528,6 +532,7 @@ Node::describe_parameters( return results; } +template std::vector Node::get_parameter_types( const std::vector & names) const diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f81e689..5670059 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include namespace rclcpp { @@ -221,7 +221,7 @@ public: : PublisherBase(node_handle, publisher_handle, topic, queue_size) { // TODO: avoid messy initialization - message_deleter_ = initialize_deleter(&message_allocator_); + allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_); } @@ -281,7 +281,7 @@ public: // subscriptions. For now call the other publish(). 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_); + std::unique_ptr, MessageT>> unique_msg(ptr, message_deleter_); return this->publish(unique_msg); } @@ -300,7 +300,7 @@ public: // subscriptions. For now call the other publish(). 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_); + std::unique_ptr, MessageT>> unique_msg(ptr, message_deleter_); return this->publish(unique_msg); } @@ -315,7 +315,7 @@ public: // Otherwise we have to allocate memory in a unique_ptr and pass it along. 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_); + std::unique_ptr, MessageT>> unique_msg(ptr, message_deleter_); return this->publish(unique_msg); } @@ -333,7 +333,8 @@ protected: } Alloc message_allocator_; - Deleter message_deleter_; + + allocator::Deleter, MessageT> message_deleter_; };