publisher template on allocator

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 <memory>
+
+template<typename T, typename Allocator>
+class AllocatorDeleter
+{
+public:
+  AllocatorDeleter()
+  : allocator_(new Allocator)
+  {
+  }
+
+  AllocatorDeleter(Allocator * a)
+  : allocator_(a)
+  {
+  }
+
+  template<typename U, typename B>
+  AllocatorDeleter(const AllocatorDeleter<U, B> & a)
+  {
+    allocator_ = a.get_allocator();
+  }
+
+  void operator()(T * ptr)
+  {
+    std::allocator_traits<Allocator>::destroy(*allocator_, ptr);
+    std::allocator_traits<Allocator>::deallocate(*allocator_, ptr, sizeof(T));
+    ptr = NULL;
+  }
+
+  Allocator * get_allocator() const
+  {
+    return allocator_;
+  }
+
+private:
+  Allocator * allocator_;
+};
+
+#endif

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 <memory>
+#include <iostream>
+#include <rclcpp/allocator/allocator_deleter.hpp>
+
+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);
+}
+
+/*
+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,
+    std::default_delete<T>,
+    AllocatorDeleter<T, Alloc<T>>
+    >::type;
+
+#endif

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 <memory>
+#include <iostream>
+#include <rclcpp/allocator_deleter.hpp>
+
+
+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>
+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>
+void initialize_deleter(AllocatorDeleter<T, Alloc> * deleter, Alloc * alloc)
+{
+  if (!alloc) {
+    throw std::invalid_argument("Allocator argument was NULL");
+  }
+  deleter = new AllocatorDeleter<T, Alloc>(alloc);
+  if (!deleter) {
+    throw std::runtime_error("initialize_deleter failed");
+  }
+}
+
+template<typename T, typename Alloc>
+class 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)
+  : 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<Alloc>::allocate(*allocator_, size);
+  }
+
+  T * deallocate(void * pointer, size_t size)
+  {
+    std::allocator_traits<Alloc>::deallocate(*allocator_, pointer, size);
+  }
+
+  template<class ... Args>
+  void construct(T * pointer, Args && ... args)
+  {
+    std::allocator_traits<Alloc>::construct(*allocator_, pointer, std::forward<Args>(args)...);
+  }
+
+  Deleter * get_deleter() const
+  {
+    return deleter_;
+  }
+  Alloc * get_underlying_allocator() const
+  {
+    return allocator_;
+  }
+
+private:
+  Alloc * allocator_;
+  Deleter * deleter_;
+};
+
+template<typename T>
+using DefaultAllocator = AllocatorWrapper<T, std::allocator<T>>;
+
+#endif

rclcpp/include/rclcpp/publisher.hpp

@@ -28,6 +28,8 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <rclcpp/allocator/allocator_factory.hpp>
+
 namespace rclcpp
 {
 
@@ -203,13 +205,13 @@ protected:
 };
 
 /// A publisher publishes messages of any type to a topic.
-template<typename MessageT>
+template<typename MessageT, template<typename> class Alloc = std::allocator>
 class Publisher : public PublisherBase
 {
   friend rclcpp::node::Node;
 
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT>);
+  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
 
   Publisher(
     std::shared_ptr<rmw_node_t> 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<MessageT> unique_msg(new MessageT(*msg.get()));
+    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_);
     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<MessageT> unique_msg(new MessageT(*msg.get()));
+    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_);
     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<MessageT> unique_msg(new MessageT(msg));
+    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_);
     return this->publish(unique_msg);
   }
 
@@ -321,6 +332,9 @@ protected:
     }
   }
 
+  Alloc<MessageT> message_allocator_;
+  Deleter<MessageT, Alloc> message_deleter_;
+
 };
 
 } /* namespace publisher */