From 41cc5324f4389e4470b270e2801e50f934899c6a Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 13 Jul 2015 14:48:19 -0700 Subject: [PATCH] get next executable instantiation, move any_executable to own file, remove dynamic_memory_strategy --- rclcpp/include/rclcpp/any_executable.hpp | 46 +++++++++++++ rclcpp/include/rclcpp/executor.hpp | 60 +++++++--------- .../executors/multi_threaded_executor.hpp | 2 +- .../executors/single_threaded_executor.hpp | 1 + rclcpp/include/rclcpp/memory_strategies.hpp | 9 --- rclcpp/include/rclcpp/memory_strategy.hpp | 41 +++++++++-- .../strategies/dynamic_memory_strategy.hpp | 69 ------------------- .../strategies/static_memory_strategy.hpp | 29 +++++++- 8 files changed, 137 insertions(+), 120 deletions(-) create mode 100644 rclcpp/include/rclcpp/any_executable.hpp delete mode 100644 rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.hpp diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp new file mode 100644 index 0000000..05c0b15 --- /dev/null +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -0,0 +1,46 @@ +// Copyright 2014 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_ANY_EXECUTABLE_HPP_ +#define RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_ + +#include + +#include + +namespace rclcpp +{ +namespace executor +{ + struct AnyExecutable + { + + AnyExecutable() + : subscription(0), timer(0), callback_group(0), node(0) + {} + // Either the subscription or the timer will be set, but not both + rclcpp::subscription::SubscriptionBase::SharedPtr subscription; + rclcpp::timer::TimerBase::SharedPtr timer; + rclcpp::service::ServiceBase::SharedPtr service; + rclcpp::client::ClientBase::SharedPtr client; + // These are used to keep the scope on the containing items + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; + rclcpp::node::Node::SharedPtr node; + }; + + typedef std::shared_ptr AnyExecutableSharedPtr; +} +} + +#endif diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 6e7aae4..b966b9c 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -24,10 +24,11 @@ #include #include +#include #include +#include #include #include -#include namespace rclcpp { @@ -36,17 +37,16 @@ namespace executor class Executor { + friend class memory_strategy::MemoryStrategy; + public: RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); - Executor() + Executor(memory_strategy::MemoryStrategySharedPtr ms = + memory_strategy::create_default_strategy()) : interrupt_guard_condition_(rmw_create_guard_condition()), - _memory_strategy(memory_strategies::create_default_strategy()) + _memory_strategy(ms) { - if (_memory_strategy == nullptr) - { - throw std::runtime_error("Received NULL memory strategy in executor constructor."); - } } virtual ~Executor() @@ -98,7 +98,7 @@ public: { this->add_node(node); // non-blocking = true - std::shared_ptr any_exec = get_next_executable(nonblocking); + AnyExecutableSharedPtr any_exec = get_next_executable(nonblocking); if (any_exec) { execute_any_executable(any_exec); } @@ -109,7 +109,7 @@ public: { this->add_node(node); // non-blocking = true - std::shared_ptr any_exec; + AnyExecutableSharedPtr any_exec; while ((any_exec = get_next_executable(true))) { execute_any_executable(any_exec); } @@ -118,34 +118,19 @@ public: // Support dynamic switching of memory strategy void - set_memory_strategy(memory_strategies::MemoryStrategySharedPtr memory_strategy) + set_memory_strategy(memory_strategy::MemoryStrategySharedPtr memory_strategy) { if (memory_strategy == nullptr) { throw std::runtime_error("Received NULL memory strategy in executor."); } - std::cout << "called set_memory_strategy" << std::endl; _memory_strategy = memory_strategy; } protected: - struct AnyExecutable - { - AnyExecutable() - : subscription(0), timer(0), callback_group(0), node(0) - {} - // Either the subscription or the timer will be set, but not both - rclcpp::subscription::SubscriptionBase::SharedPtr subscription; - rclcpp::timer::TimerBase::SharedPtr timer; - rclcpp::service::ServiceBase::SharedPtr service; - rclcpp::client::ClientBase::SharedPtr client; - // These are used to keep the scope on the containing items - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; - rclcpp::node::Node::SharedPtr node; - }; void - execute_any_executable(std::shared_ptr & any_exec) + execute_any_executable(AnyExecutableSharedPtr & any_exec) { if (!any_exec) { return; @@ -588,7 +573,7 @@ protected: } void - get_next_timer(std::shared_ptr & any_exec) + get_next_timer(AnyExecutableSharedPtr & any_exec) { for (auto handle : guard_condition_handles_) { auto timer = get_timer_by_handle(handle); @@ -640,7 +625,7 @@ protected: } void - get_next_subscription(std::shared_ptr & any_exec) + get_next_subscription(AnyExecutableSharedPtr & any_exec) { for (auto handle : subscriber_handles_) { auto subscription = get_subscription_by_handle(handle); @@ -692,7 +677,7 @@ protected: } void - get_next_service(std::shared_ptr & any_exec) + get_next_service(AnyExecutableSharedPtr & any_exec) { for (auto handle : service_handles_) { auto service = get_service_by_handle(handle); @@ -744,7 +729,7 @@ protected: } void - get_next_client(std::shared_ptr & any_exec) + get_next_client(AnyExecutableSharedPtr & any_exec) { for (auto handle : client_handles_) { auto client = get_client_by_handle(handle); @@ -774,10 +759,15 @@ protected: } } - std::shared_ptr + AnyExecutableSharedPtr get_next_ready_executable() { - std::shared_ptr any_exec(new AnyExecutable()); + return get_next_ready_executable(this->_memory_strategy->instantiate_next_executable()); + } + + AnyExecutableSharedPtr + get_next_ready_executable(AnyExecutableSharedPtr any_exec) + { // Check the timers to see if there are any that are ready, if so return get_next_timer(any_exec); if (any_exec->timer) { @@ -803,7 +793,7 @@ protected: return any_exec; } - std::shared_ptr + AnyExecutableSharedPtr get_next_executable(bool nonblocking = false) { // Check to see if there are any subscriptions or timers needing service @@ -835,11 +825,11 @@ protected: rmw_guard_condition_t * interrupt_guard_condition_; + memory_strategy::MemoryStrategySharedPtr _memory_strategy; + private: RCLCPP_DISABLE_COPY(Executor); - std::shared_ptr _memory_strategy; - std::vector> weak_nodes_; typedef std::list SubscriberHandles; SubscriberHandles subscriber_handles_; diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index d16135c..53f2679 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -79,7 +79,7 @@ private: { rclcpp::thread_id = this_thread_id; while (rclcpp::utilities::ok()) { - std::shared_ptr any_exec; + std::shared_ptr any_exec; { std::lock_guard wait_lock(wait_mutex_); if (!rclcpp::utilities::ok()) { diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index ebcd51d..8ddd759 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -27,6 +27,7 @@ #include #include #include +#include namespace rclcpp { diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp index 9580876..19da9b5 100644 --- a/rclcpp/include/rclcpp/memory_strategies.hpp +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -15,7 +15,6 @@ #ifndef RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_ #define RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_ -#include #include namespace rclcpp @@ -23,16 +22,8 @@ namespace rclcpp namespace memory_strategies { -typedef std::shared_ptr MemoryStrategySharedPtr; - -using rclcpp::memory_strategies::dynamic_memory_strategy::DynamicMemoryStrategy; using rclcpp::memory_strategies::static_memory_strategy::StaticMemoryStrategy; -MemoryStrategySharedPtr create_default_strategy() -{ - return std::make_shared(DynamicMemoryStrategy()); -} - } } diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 9984774..f5e7b77 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -14,27 +14,53 @@ #ifndef RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_ #define RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_ +#include namespace rclcpp { +// TODO move HandleType somewhere where it makes sense enum class HandleType{subscriber_handle, service_handle, client_handle, guard_condition_handle}; +namespace executor +{ + class Executor; +}; + namespace memory_strategy { class MemoryStrategy { + friend class executor::Executor; + public: - virtual void** borrow_handles(HandleType type, size_t number_of_handles) = 0; + virtual void** borrow_handles(HandleType type, size_t number_of_handles) + { + return static_cast(rcl_malloc(sizeof(void *) * number_of_handles)); + } - virtual void return_handles(HandleType type, void** handles) = 0; + virtual void return_handles(HandleType type, void** handles) + { + this->rcl_free(handles); + } - virtual void *rcl_malloc(size_t size) = 0; + virtual executor::AnyExecutableSharedPtr instantiate_next_executable() + { + return executor::AnyExecutableSharedPtr(new executor::AnyExecutable); + } - virtual void rcl_free(void *ptr) = 0; + virtual void *rcl_malloc(size_t size) + { + return std::malloc(size); + } + + virtual void rcl_free(void *ptr) + { + return std::free(ptr); + } protected: @@ -42,6 +68,13 @@ private: }; +typedef std::shared_ptr MemoryStrategySharedPtr; + +MemoryStrategySharedPtr create_default_strategy() +{ + return std::make_shared(MemoryStrategy()); +} + } /* memory_strategy */ } /* rclcpp */ diff --git a/rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.hpp deleted file mode 100644 index 3c88294..0000000 --- a/rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// 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_DYNAMIC_MEMORY_STRATEGY_HPP_ -#define RCLCPP_RCLCPP_DYNAMIC_MEMORY_STRATEGY_HPP_ - -#include - -namespace rclcpp -{ - -namespace memory_strategies -{ - -namespace dynamic_memory_strategy -{ - -class DynamicMemoryStrategy : public memory_strategy::MemoryStrategy -{ - -public: - - DynamicMemoryStrategy() {} - - void** borrow_handles(HandleType /*type*/, size_t number_of_handles) - { - return static_cast(rcl_malloc(sizeof(void *) * number_of_handles)); - } - - void return_handles(HandleType /*type*/, void** handles) - { - this->rcl_free(handles); - } - - void *rcl_malloc(size_t size) - { - return std::malloc(size); - } - - void rcl_free(void *ptr) - { - return std::free(ptr); - } - -protected: - - -private: - -}; - -} - -} /* memory_strategies */ - -} /* rclcpp */ - -#endif diff --git a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp index cf5b3e3..1178be4 100644 --- a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp @@ -40,11 +40,17 @@ public: memset(_client_pool, 0, _max_clients); memset(_guard_condition_pool, 0, _max_guard_conditions); _pool_seq = 0; + _exec_seq = 0; for (size_t i = 0; i < _pool_size; ++i) { _memory_map[_memory_pool[i]] = 0; } + + for (size_t i = 0; i < _max_executables; ++i) + { + _executable_pool[i] = std::make_shared(executor::AnyExecutable()); + } } void** borrow_handles(HandleType type, size_t number_of_handles) @@ -85,6 +91,20 @@ public: throw std::runtime_error("Unrecognized enum, could not return handle memory."); break; } + } + + executor::AnyExecutableSharedPtr instantiate_next_executable() + { + if (_exec_seq >= _max_executables) + { + // wrap around + _exec_seq = 0; + } + size_t prev_exec_seq = _exec_seq; + ++_exec_seq; + + return _executable_pool[prev_exec_seq]; + } void *rcl_malloc(size_t size) { @@ -102,9 +122,9 @@ public: throw std::runtime_error("Unexpected pointer in rcl_malloc."); } _memory_map[ptr] = size; - size_t prev__pool_seq = _pool_seq; + size_t prev_pool_seq = _pool_seq; _pool_seq += size; - return _memory_pool[prev__pool_seq]; + return _memory_pool[prev_pool_seq]; } void rcl_free(void *ptr) @@ -127,13 +147,18 @@ private: static const size_t _max_services = 5; static const size_t _max_clients = 10; static const size_t _max_guard_conditions = 50; + static const size_t _max_executables = 1; void *_memory_pool[_pool_size]; void *_subscriber_pool[_max_subscribers]; void *_service_pool[_max_services]; void *_client_pool[_max_clients]; void *_guard_condition_pool[_max_guard_conditions]; + executor::AnyExecutableSharedPtr _executable_pool[_max_executables]; + size_t _pool_seq; + size_t _exec_seq; + std::map _memory_map; };