templated stl structures exposed in Node

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -17,12 +17,18 @@
 
 #include <memory>
 
+namespace rclcpp
+{
+
+namespace allocator
+{
+
 template<typename T, typename Allocator>
 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<template<typename> class Alloc, typename T, typename D>
+D initialize_deleter(Alloc<T> * alloc)
+{
+  (void) alloc;
+  throw std::runtime_error("Reached unexpected template specialization");
+}
+
+template<typename T>
+std::default_delete<T> initialize_deleter(std::allocator<T> * alloc)
+{
+  (void) alloc;
+  return std::default_delete<T>();
+}
+
+template<template<typename> class Alloc, typename T>
+AllocatorDeleter<T, Alloc<T>> initialize_deleter(Alloc<T> * alloc)
+{
+  if (!alloc) {
+    throw std::invalid_argument("Allocator argument was NULL");
+  }
+  return AllocatorDeleter<T, Alloc<T>>(alloc);
+}
+*/
+template<template<typename> class Alloc, typename T, typename D>
+void set_allocator_for_deleter(D * deleter, Alloc<T> * alloc)
+{
+  (void) alloc;
+  throw std::runtime_error("Reached unexpected template specialization");
+}
+
+template<typename T>
+void set_allocator_for_deleter(std::default_delete<T> * deleter, std::allocator<T> * alloc)
+{
+  (void) deleter;
+  (void) alloc;
+}
+
+template<template<typename> class Alloc, typename T>
+void set_allocator_for_deleter(AllocatorDeleter<T, Alloc<T>> * deleter, Alloc<T> * alloc)
+{
+  if (!deleter || !alloc) {
+    throw std::invalid_argument("Argument was NULL to set_allocator_for_deleter");
+  }
+  //return AllocatorDeleter<T, Alloc<T>>(alloc);
+  deleter->set_allocator(alloc);
+}
+
+template<typename Alloc, typename T>
+using Deleter = typename std::conditional<
+    std::is_same<Alloc, std::allocator<T>>::value,
+    std::default_delete<T>,
+    AllocatorDeleter<T, Alloc>
+    >::type;
+
+}
+}
+
 #endif

rclcpp/include/rclcpp/allocator/allocator_factory.hpp

@@ -42,23 +42,6 @@ AllocatorDeleter<T, Alloc<T>> initialize_deleter(Alloc<T> * alloc)
   return AllocatorDeleter<T, Alloc<T>>(alloc);
 }
 
-/*
-class AllocatorFactoryBase
-{
-public:
-  // Hmm
-  template<typename T, typename AllocT>
-  virtual void AllocT<typename T> * request_allocator(...) = 0;
-
-  template<typename T, typename AllocT>
-  virtual void return_allocator(AllocT<typename T> * allocator);
-
-  template<typename T, typename AllocT, typename DeleterT>
-  virtual DeleterT<T> * get_deleter_for_allocator(AllocT<typename T> * allocator) = 0;
-
-};
-*/
-
 template<typename T, template<typename> class Alloc>
 using Deleter = typename std::conditional<
     std::is_same<Alloc<T>, std::allocator<T>>::value,

rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp

@@ -17,50 +17,39 @@
 
 #include <memory>
 #include <iostream>
-#include <rclcpp/allocator_deleter.hpp>
+#include <rclcpp/allocator/allocator_deleter.hpp>
 
-
+namespace rclcpp
-template<typename Alloc, typename T, typename D>
-void initialize_deleter(D * deleter, Alloc * alloc)
 {
-  (void) deleter;
-  (void) alloc;
-  throw std::runtime_error("Reached unexpected template specialization");
-}
 
-template<typename T>
+namespace allocator
-void initialize_deleter(std::default_delete<T> * deleter, std::allocator<T> * alloc)
 {
-  (void) alloc;
-  deleter = new std::default_delete<T>;
-  if (!deleter) {
-    throw std::runtime_error("initialize_deleter failed");
-  }
-}
 
-template<typename Alloc, typename T>
+// Type-erased interface to AllocatorWrapper
-void initialize_deleter(AllocatorDeleter<T, Alloc> * deleter, Alloc * alloc)
+class AllocatorWrapper
 {
-  if (!alloc) {
+  virtual void * allocate(size_t size) = 0;
-    throw std::invalid_argument("Allocator argument was NULL");
+  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
-  deleter = new AllocatorDeleter<T, Alloc>(alloc);
+
-  if (!deleter) {
+  virtual void destroy(T* pointer) = 0;
-    throw std::runtime_error("initialize_deleter failed");
-  }
 }
 
 template<typename T, typename Alloc>
-class AllocatorWrapper
+class TypedAllocatorWrapper : public AllocatorWrapper
 {
 public:
+  /*
   using Deleter = typename std::conditional<
       std::is_same<Alloc, std::allocator<T>>::value,
       std::default_delete<T>,
       AllocatorDeleter<T, Alloc>
       >::type;
+  */
 
-  AllocatorWrapper(Alloc * allocator)
+  using DeleterT = Deleter<Alloc, T>;
+
+  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<Alloc>::allocate(*allocator_, size);
   }
 
-  T * deallocate(void * pointer, size_t size)
+  void deallocate(void * pointer, size_t size)
+  {
+    deallocate(static_cast<T*>(pointer), size);
+  }
+
+  void deallocate(T * pointer, size_t size)
   {
     std::allocator_traits<Alloc>::deallocate(*allocator_, pointer, size);
   }
@@ -121,7 +115,18 @@ public:
     std::allocator_traits<Alloc>::construct(*allocator_, pointer, std::forward<Args>(args)...);
   }
 
-  Deleter * get_deleter() const
+  void destroy(void * pointer)
+  {
+    destroy(static_cast<T*>(pointer));
+  }
+
+  template<class T>
+  void destroy(T * pointer)
+  {
+    std::allocator_traits<Alloc>::destroy(*allocator_, pointer);
+  }
+
+  DeleterT * get_deleter() const
   {
     return deleter_;
   }
@@ -132,10 +137,13 @@ public:
 
 private:
   Alloc * allocator_;
-  Deleter * deleter_;
+  DeleterT * deleter_;
 };
 
 template<typename T>
-using DefaultAllocator = AllocatorWrapper<T, std::allocator<T>>;
+using DefaultAllocator = TypedAllocatorWrapper<T, std::allocator<T>>;
+
+}
+}
 
 #endif

rclcpp/include/rclcpp/node.hpp

@@ -202,25 +202,34 @@ public:
     FunctorT callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
-  std::vector<rcl_interfaces::msg::SetParametersResult> set_parameters(
+  template<typename Alloc>
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  using StringRebind = typename Alloc::template rebind<std::string>::other;
 
+  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
+  typename std::vector<rcl_interfaces::msg::SetParametersResult, Alloc> set_parameters(
+    const typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> & parameters);
+
+  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
   rcl_interfaces::msg::SetParametersResult set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+    const typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> & parameters);
 
-  std::vector<rclcpp::parameter::ParameterVariant> get_parameters(
+  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
-    const std::vector<std::string> & names) const;
+  typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> get_parameters(
+    const typename std::vector<std::string, StringRebind<Alloc>> & names) const;
 
-  std::vector<rcl_interfaces::msg::ParameterDescriptor> describe_parameters(
+  template<typename Alloc = std::allocator<rcl_interfaces::msg::ParameterDescriptor>>
-    const std::vector<std::string> & names) const;
+  typename std::vector<rcl_interfaces::msg::ParameterDescriptor, Alloc> describe_parameters(
+    const typename std::vector<std::string, StringRebind<Alloc>>> & names) const;
 
-  std::vector<uint8_t> get_parameter_types(
+  template<typename Alloc = std::allocator<uint8_t>>
-    const std::vector<std::string> & names) const;
+  typename std::vector<uint8_t, Alloc> get_parameter_types(
+    const typename std::vector<std::string, StringRebind<Alloc>>> & names) const;
 
   rcl_interfaces::msg::ListParametersResult list_parameters(
     const std::vector<std::string> & prefixes, uint64_t depth) const;
 
-  std::map<std::string, std::string> get_topic_names_and_types() const;
+  template<typename Alloc = std::allocator<std::pair<const std::string, std::string>>>
+  typename std::map<std::string, std::string, Alloc> get_topic_names_and_types() const;
 
   size_t count_publishers(const std::string & topic_name) const;
 

rclcpp/include/rclcpp/node_impl.hpp

@@ -443,6 +443,7 @@ Node::create_service(
   return serv;
 }
 
+template<typename Alloc>
 std::vector<rcl_interfaces::msg::SetParametersResult>
 Node::set_parameters(
   const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
@@ -455,6 +456,7 @@ Node::set_parameters(
   return results;
 }
 
+template<typename Alloc>
 rcl_interfaces::msg::SetParametersResult
 Node::set_parameters_atomically(
   const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
@@ -489,6 +491,7 @@ Node::set_parameters_atomically(
   return result;
 }
 
+template<typename Alloc>
 std::vector<rclcpp::parameter::ParameterVariant>
 Node::get_parameters(
   const std::vector<std::string> & names) const
@@ -508,6 +511,7 @@ Node::get_parameters(
   return results;
 }
 
+template<typename Alloc>
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
 Node::describe_parameters(
   const std::vector<std::string> & names) const
@@ -528,6 +532,7 @@ Node::describe_parameters(
   return results;
 }
 
+template<typename Alloc>
 std::vector<uint8_t>
 Node::get_parameter_types(
   const std::vector<std::string> & names) const

rclcpp/include/rclcpp/publisher.hpp

@@ -28,7 +28,7 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
-#include <rclcpp/allocator/allocator_factory.hpp>
+#include <rclcpp/allocator/allocator_deleter.hpp>
 
 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<Alloc<MessageT>>::allocate(message_allocator_, 1);
     std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, *msg.get());
-    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
+    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, 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<Alloc<MessageT>>::allocate(message_allocator_, 1);
     std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, *msg.get());
-    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
+    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, 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<Alloc<MessageT>>::allocate(message_allocator_, 1);
     std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, msg);
-    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
+    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, MessageT>> unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -333,7 +333,8 @@ protected:
   }
 
   Alloc<MessageT> message_allocator_;
-  Deleter<MessageT, Alloc> message_deleter_;
+
+  allocator::Deleter<Alloc<MessageT>, MessageT> message_deleter_;
 
 };