From 0cd13608f77bc7662f53c900cf80a7dd890c31a0 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Thu, 22 Oct 2015 11:15:19 -0700 Subject: [PATCH] Add allocator memory strategy --- .../rclcpp/allocator/allocator_deleter.hpp | 3 +- .../rclcpp/message_memory_strategy.hpp | 5 + rclcpp/include/rclcpp/publisher.hpp | 3 +- .../strategies/allocator_memory_strategy.hpp | 187 ++++++++++++++++++ rclcpp/test/test_intra_process_manager.cpp | 5 +- 5 files changed, 200 insertions(+), 3 deletions(-) create mode 100644 rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp index d725f2b..2eb00ed 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp @@ -94,7 +94,8 @@ void set_allocator_for_deleter(AllocatorDeleter * deleter, Alloc * alloc) template using Deleter = typename std::conditional< - std::is_same>::value, + std::is_same::template rebind_alloc, + typename std::allocator::template rebind::other>::value, std::default_delete, AllocatorDeleter >::type; diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 25c8c2d..9076c93 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -37,6 +37,11 @@ public: using MessageAlloc = allocator::AllocRebind; using MessageDeleter = allocator::Deleter; + MessageMemoryStrategy() + { + message_allocator_ = new typename MessageAlloc::allocator_type(); + } + MessageMemoryStrategy(std::shared_ptr allocator) { message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get()); diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 0c706e1..92249d7 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -236,7 +236,8 @@ public: * \param[in] msg A shared pointer to the message to send. */ void - publish(MessageUniquePtr & msg) + publish(std::unique_ptr & msg) + //publish(MessageUniquePtr & msg) { this->do_inter_process_publish(msg.get()); if (store_intra_process_message_) { diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp new file mode 100644 index 0000000..daa2e0f --- /dev/null +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -0,0 +1,187 @@ +// 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_MEMORY_STRATEGY_HPP_ +#define RCLCPP_RCLCPP_ALLOCATOR_MEMORY_STRATEGY_HPP_ + +#include +#include + +namespace rclcpp +{ + + +namespace memory_strategies +{ + +namespace allocator_memory_strategy +{ + +/// Delegate for handling memory allocations while the Executor is executing. +/** + * By default, the memory strategy dynamically allocates memory for structures that come in from + * the rmw implementation after the executor waits for work, based on the number of entities that + * come through. + */ +template +class AllocatorMemoryStrategy : public MemoryStrategy +{ + +public: + RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy); + + using ExecAlloc = allocator::AllocRebind; + using ExecDeleter = + allocator::Deleter; + using VoidAlloc = allocator::AllocRebind; + + AllocatorMemoryStrategy(std::shared_ptr allocator) + { + executable_allocator_ = new ExecAlloc(*allocator.get()); + allocator_ = new VoidAlloc(*allocator.get()); + } + + /// Borrow memory for storing data for subscriptions, services, clients, or guard conditions. + /** + * The default implementation stores std::vectors for each handle type and resizes the vectors + * as necessary based on the requested number of handles. + * \param[in] The type of entity that this function is requesting for. + * \param[in] The number of handles to borrow. + * \return Pointer to the allocated handles. + */ + virtual void ** borrow_handles(HandleType type, size_t number_of_handles) + { + switch (type) { + case HandleType::subscription_handle: + if (subscription_handles.size() < number_of_handles) { + subscription_handles.resize(number_of_handles, 0); + } + return static_cast(subscription_handles.data()); + case HandleType::service_handle: + if (service_handles.size() < number_of_handles) { + service_handles.resize(number_of_handles, 0); + } + return static_cast(service_handles.data()); + case HandleType::client_handle: + if (client_handles.size() < number_of_handles) { + client_handles.resize(number_of_handles, 0); + } + return static_cast(client_handles.data()); + case HandleType::guard_condition_handle: + if (number_of_handles > 2) { + throw std::runtime_error("Too many guard condition handles requested!"); + } + return guard_cond_handles.data(); + default: + throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast(type)) + + ", could not borrow handle memory."); + } + } + + /// Return the memory borrowed in borrow_handles. + /** + * return_handles should always mirror the way memory was borrowed in borrow_handles. + * \param[in] The type of entity that this function is returning. + * \param[in] Pointer to the handles returned. + */ + virtual void return_handles(HandleType type, void ** handles) + { + switch (type) { + case HandleType::subscription_handle: + if (handles != subscription_handles.data()) { + throw std::runtime_error( + "tried to return memory that isn't handled by this AllocatorMemoryStrategy"); + } + memset(handles, 0, subscription_handles.size()); + break; + case HandleType::service_handle: + if (handles != service_handles.data()) { + throw std::runtime_error( + "tried to return memory that isn't handled by this AllocatorMemoryStrategy"); + } + memset(handles, 0, service_handles.size()); + break; + case HandleType::client_handle: + if (handles != client_handles.data()) { + throw std::runtime_error( + "tried to return memory that isn't handled by this AllocatorMemoryStrategy"); + } + memset(handles, 0, client_handles.size()); + break; + case HandleType::guard_condition_handle: + if (handles != guard_cond_handles.data()) { + throw std::runtime_error( + "tried to return memory that isn't handled by this AllocatorMemoryStrategy"); + } + guard_cond_handles.fill(0); + break; + default: + throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast(type)) + + ", could not borrow handle memory."); + } + } + + /// Provide a newly initialized AnyExecutable object. + // \return Shared pointer to the fresh executable. + virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() + { + //return std::make_shared(); + auto ptr = ExecAlloc::allocate(*executable_allocator_, 1); + ExecAlloc::construct(*executable_allocator_); + return std::shared_ptr(ptr, executable_deleter_); + } + + /// Implementation of a general-purpose allocation function. + /** + * \param[in] size Number of bytes to allocate. + * \return Pointer to the allocated chunk of memory. + */ + virtual void * alloc(size_t size) + { + if (size == 0) { + return NULL; + } + return VoidAlloc::allocate(*allocator_, size); + } + + /// Implementation of a general-purpose free. + /** + * \param[in] Pointer to deallocate. + */ + virtual void free(void * ptr) + { + VoidAlloc::deallocate(*allocator, ptr); + } + + std::vector subs; + std::vector services; + std::vector clients; + + std::vector subscription_handles; + std::vector service_handles; + std::vector client_handles; + std::array guard_cond_handles; + +private: + typename ExecAlloc::allocator_type * executable_allocator_; + ExecDeleter executable_deleter_; + typename VoidAlloc::allocator_type * allocator_; +}; + +} /* allocator_memory_strategy */ +} /* memory_strategies */ + +} /* rclcpp */ + +#endif diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index 3cae9bf..df460c9 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -15,7 +15,7 @@ #include #include - +#include #include #include @@ -59,6 +59,9 @@ template> class Publisher : public PublisherBase { public: + using MessageAlloc = allocator::AllocRebind; + using MessageDeleter = allocator::Deleter; + RCLCPP_SMART_PTR_DEFINITIONS(Publisher); };