From 74a216a65b4e898c9c87196d5bd37604e217d58f Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Fri, 17 Jul 2015 18:56:52 -0700 Subject: [PATCH] Message memory pool strategy --- .../rclcpp/message_memory_strategy.hpp | 53 ++++++++++++ rclcpp/include/rclcpp/node.hpp | 7 +- rclcpp/include/rclcpp/node_impl.hpp | 6 +- .../message_pool_memory_strategy.hpp | 85 +++++++++++++++++++ rclcpp/include/rclcpp/subscription.hpp | 19 ++++- 5 files changed, 164 insertions(+), 6 deletions(-) create mode 100644 rclcpp/include/rclcpp/message_memory_strategy.hpp create mode 100644 rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp new file mode 100644 index 0000000..58c9807 --- /dev/null +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -0,0 +1,53 @@ +// 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_MSG_MEMORY_STRATEGY_HPP_ +#define RCLCPP_RCLCPP_MSG_MEMORY_STRATEGY_HPP_ + +#include + +#include + +namespace rclcpp +{ +namespace message_memory_strategy +{ + +template +class MessageMemoryStrategy +{ + +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(MessageMemoryStrategy); + + static SharedPtr create_default() + { + return SharedPtr(new MessageMemoryStrategy); + } + + virtual std::shared_ptr borrow_message() + { + return std::shared_ptr(new MessageT); + } + + virtual void return_message(std::shared_ptr & msg) + { + msg.reset(); + } +}; + +} /* message_memory_strategy */ +} /* rclcpp */ + +#endif /* RCLCPP_RCLCPP_MSG_MEMORY_STRATEGY_HPP_ */ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 0885d70..23663ab 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -120,6 +121,7 @@ public: create_publisher(const std::string & topic_name, size_t queue_size); /* Create and return a Subscription. */ + template typename rclcpp::subscription::Subscription::SharedPtr create_subscription( @@ -127,7 +129,10 @@ public: size_t queue_size, std::function &)> callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - bool ignore_local_publications = false); + bool ignore_local_publications = false, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + msg_mem_strat = + rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default()); /* Create a timer. */ rclcpp::timer::WallTimer::SharedPtr diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 4da69f8..ae6810b 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -121,7 +121,8 @@ Node::create_subscription( size_t queue_size, std::function &)> callback, rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications) + bool ignore_local_publications, + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr msg_mem_strat) { using rosidl_generator_cpp::get_message_type_support_handle; auto type_support_handle = get_message_type_support_handle(); @@ -143,7 +144,8 @@ Node::create_subscription( subscriber_handle, topic_name, ignore_local_publications, - callback); + callback, + msg_mem_strat); auto sub_base_ptr = std::dynamic_pointer_cast(sub); if (group) { if (!group_in_node(group)) { diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp new file mode 100644 index 0000000..17db254 --- /dev/null +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -0,0 +1,85 @@ +// 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_MSG_POOL_MEMORY_STRATEGY_HPP_ +#define RCLCPP_RCLCPP_MSG_POOL_MEMORY_STRATEGY_HPP_ + +#include +#include + +namespace rclcpp +{ +namespace strategies +{ +namespace message_pool_memory_strategy +{ + +template::value>::type * = +nullptr> +class MessagePoolMemoryStrategy + : public message_memory_strategy::MessageMemoryStrategy +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(MessagePoolMemoryStrategy); + MessagePoolMemoryStrategy() + : next_array_index_(0) + { + for (size_t i = 0; i < size; ++i) { + pool_[i].msg_ptr_ = std::make_shared(); + pool_[i].used = false; + } + } + + std::shared_ptr borrow_message() + { + size_t current_index = next_array_index_; + next_array_index_ = (next_array_index_ + 1) % size; + if (pool_[current_index].used) { + throw std::runtime_error("Tried to access message that was still in use! Abort."); + } + pool_[current_index].msg_ptr_->~MessageT(); + new (pool_[current_index].msg_ptr_.get())MessageT; + + pool_[current_index].used = true; + return pool_[current_index].msg_ptr_; + } + + void return_message(std::shared_ptr & msg) + { + for (size_t i = 0; i < size; ++i) { + if (pool_[i].msg_ptr_ == msg) { + pool_[i].used = false; + return; + } + } + throw std::runtime_error("Unrecognized message ptr in return_message."); + } + +protected: + struct PoolMember + { + std::shared_ptr msg_ptr_; + bool used; + }; + + std::array pool_; + size_t next_array_index_; + +}; + +} /* message_pool_memory_strategy */ +} /* strategies */ +} /* rclcpp */ +#endif /* RCLCPP_RCLCPP_MSG_POOL_MEMORY_STRATEGY_HPP_ */ diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 856c0cb..5b50821 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -25,6 +25,7 @@ #include #include +#include namespace rclcpp { @@ -102,26 +103,38 @@ public: rmw_subscription_t * subscription_handle, const std::string & topic_name, bool ignore_local_publications, - CallbackType callback) + CallbackType callback, + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr memory_strategy = + message_memory_strategy::MessageMemoryStrategy::create_default()) : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications), - callback_(callback) + callback_(callback), + message_memory_strategy_(memory_strategy) {} + void set_message_memory_strategy( + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) + { + message_memory_strategy_ = message_memory_strategy; + } + std::shared_ptr create_message() { - return std::shared_ptr(new MessageT()); + return message_memory_strategy_->borrow_message(); } void handle_message(std::shared_ptr & message) { auto typed_message = std::static_pointer_cast(message); callback_(typed_message); + message_memory_strategy_->return_message(typed_message); } private: RCLCPP_DISABLE_COPY(Subscription); CallbackType callback_; + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr + message_memory_strategy_; };