From ba91c8d34267a6d06b137f98d1fce9aa4ff88e00 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 13 Jul 2015 12:01:36 -0700 Subject: [PATCH 1/6] First pass at dependency injection of memory strategy objects --- rclcpp/include/rclcpp/executor.hpp | 74 ++++++--- rclcpp/include/rclcpp/memory_strategies.hpp | 39 +++++ rclcpp/include/rclcpp/memory_strategy.hpp | 49 ++++++ .../strategies/dynamic_memory_strategy.hpp | 69 +++++++++ .../strategies/static_memory_strategy.hpp | 146 ++++++++++++++++++ 5 files changed, 352 insertions(+), 25 deletions(-) create mode 100644 rclcpp/include/rclcpp/memory_strategies.hpp create mode 100644 rclcpp/include/rclcpp/memory_strategy.hpp create mode 100644 rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.hpp create mode 100644 rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index d3e9051..6e7aae4 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -27,6 +27,7 @@ #include #include #include +#include namespace rclcpp { @@ -39,8 +40,14 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); Executor() - : interrupt_guard_condition_(rmw_create_guard_condition()) - {} + : interrupt_guard_condition_(rmw_create_guard_condition()), + _memory_strategy(memory_strategies::create_default_strategy()) + { + if (_memory_strategy == nullptr) + { + throw std::runtime_error("Received NULL memory strategy in executor constructor."); + } + } virtual ~Executor() { @@ -109,6 +116,18 @@ public: this->remove_node(node); } + // Support dynamic switching of memory strategy + void + set_memory_strategy(memory_strategies::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 { @@ -265,8 +284,8 @@ protected: rmw_subscriptions_t subscriber_handles; subscriber_handles.subscriber_count = number_of_subscriptions; // TODO(wjwwood): Avoid redundant malloc's - subscriber_handles.subscribers = static_cast( - std::malloc(sizeof(void *) * number_of_subscriptions)); + subscriber_handles.subscribers = + _memory_strategy->borrow_handles(HandleType::subscriber_handle, number_of_subscriptions); if (subscriber_handles.subscribers == NULL) { // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for subscriber pointers."); @@ -283,9 +302,8 @@ protected: unsigned long number_of_services = services.size(); rmw_services_t service_handles; service_handles.service_count = number_of_services; - // TODO(esteve): Avoid redundant malloc's - service_handles.services = static_cast( - std::malloc(sizeof(void *) * number_of_services)); + service_handles.services = + _memory_strategy->borrow_handles(HandleType::service_handle, number_of_services); if (service_handles.services == NULL) { // TODO(esteve): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for service pointers."); @@ -302,9 +320,8 @@ protected: unsigned long number_of_clients = clients.size(); rmw_clients_t client_handles; client_handles.client_count = number_of_clients; - // TODO: Avoid redundant malloc's - client_handles.clients = static_cast( - std::malloc(sizeof(void *) * number_of_clients)); + client_handles.clients = + _memory_strategy->borrow_handles(HandleType::client_handle, number_of_clients); if (client_handles.clients == NULL) { // TODO: Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for client pointers."); @@ -324,9 +341,8 @@ protected: timers.size() + start_of_timer_guard_conds; rmw_guard_conditions_t guard_condition_handles; guard_condition_handles.guard_condition_count = number_of_guard_conds; - // TODO(wjwwood): Avoid redundant malloc's - guard_condition_handles.guard_conditions = static_cast( - std::malloc(sizeof(void *) * number_of_guard_conds)); + guard_condition_handles.guard_conditions = + _memory_strategy->borrow_handles(HandleType::guard_condition_handle, number_of_guard_conds); if (guard_condition_handles.guard_conditions == NULL) { // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for guard condition pointers."); @@ -355,12 +371,15 @@ protected: nonblocking); // If ctrl-c guard condition, return directly if (guard_condition_handles.guard_conditions[0] != 0) { - // Make sure to free memory - // TODO(wjwwood): Remove theses when the "Avoid redundant malloc's" - // todo has been addressed. - std::free(subscriber_handles.subscribers); - std::free(service_handles.services); - std::free(guard_condition_handles.guard_conditions); + // Make sure to free or clean memory + _memory_strategy->return_handles(HandleType::subscriber_handle, + subscriber_handles.subscribers); + _memory_strategy->return_handles(HandleType::service_handle, + service_handles.services); + _memory_strategy->return_handles(HandleType::client_handle, + client_handles.clients); + _memory_strategy->return_handles(HandleType::guard_condition_handle, + guard_condition_handles.guard_conditions); return; } // Add the new work to the class's list of things waiting to be executed @@ -393,12 +412,15 @@ protected: } } - // Make sure to free memory - // TODO(wjwwood): Remove theses when the "Avoid redundant malloc's" - // todo has been addressed. - std::free(subscriber_handles.subscribers); - std::free(service_handles.services); - std::free(guard_condition_handles.guard_conditions); + _memory_strategy->return_handles(HandleType::subscriber_handle, + subscriber_handles.subscribers); + _memory_strategy->return_handles(HandleType::service_handle, + service_handles.services); + _memory_strategy->return_handles(HandleType::client_handle, + client_handles.clients); + _memory_strategy->return_handles(HandleType::guard_condition_handle, + guard_condition_handles.guard_conditions); + } /******************************/ @@ -816,6 +838,8 @@ protected: 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/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp new file mode 100644 index 0000000..9580876 --- /dev/null +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -0,0 +1,39 @@ +// 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_MEMORY_STRATEGIES_HPP_ +#define RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_ + +#include +#include + +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()); +} + +} +} + +#endif diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp new file mode 100644 index 0000000..9984774 --- /dev/null +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -0,0 +1,49 @@ +// 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_MEMORY_STRATEGY_HPP_ +#define RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_ + +namespace rclcpp +{ + +enum class HandleType{subscriber_handle, service_handle, client_handle, guard_condition_handle}; + +namespace memory_strategy +{ + +class MemoryStrategy +{ + +public: + + virtual void** borrow_handles(HandleType type, size_t number_of_handles) = 0; + + virtual void return_handles(HandleType type, void** handles) = 0; + + virtual void *rcl_malloc(size_t size) = 0; + + virtual void rcl_free(void *ptr) = 0; + +protected: + +private: + +}; + +} /* memory_strategy */ + +} /* rclcpp */ + +#endif diff --git a/rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.hpp new file mode 100644 index 0000000..3c88294 --- /dev/null +++ b/rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.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_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 new file mode 100644 index 0000000..cf5b3e3 --- /dev/null +++ b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp @@ -0,0 +1,146 @@ + +// 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_STATIC_MEMORY_STRATEGY_HPP_ +#define RCLCPP_RCLCPP_STATIC_MEMORY_STRATEGY_HPP_ + +#include + +namespace rclcpp +{ + +namespace memory_strategies +{ + +namespace static_memory_strategy +{ + +class StaticMemoryStrategy : public memory_strategy::MemoryStrategy +{ + +public: + + StaticMemoryStrategy() + { + memset(_memory_pool, 0, _pool_size); + memset(_subscriber_pool, 0, _max_subscribers); + memset(_service_pool, 0, _max_services); + memset(_client_pool, 0, _max_clients); + memset(_guard_condition_pool, 0, _max_guard_conditions); + _pool_seq = 0; + + for (size_t i = 0; i < _pool_size; ++i) + { + _memory_map[_memory_pool[i]] = 0; + } + } + + void** borrow_handles(HandleType type, size_t number_of_handles) + { + switch(type) + { + case HandleType::subscriber_handle: + return _subscriber_pool; + case HandleType::service_handle: + return _service_pool; + case HandleType::client_handle: + return _client_pool; + case HandleType::guard_condition_handle: + return _guard_condition_pool; + default: + break; + } + throw std::runtime_error("Unrecognized enum, could not borrow handle memory."); + } + + void return_handles(HandleType type, void** handles) + { + switch(type) + { + case HandleType::subscriber_handle: + memset(_subscriber_pool, 0, _max_subscribers); + break; + case HandleType::service_handle: + memset(_service_pool, 0, _max_services); + break; + case HandleType::client_handle: + memset(_client_pool, 0, _max_clients); + break; + case HandleType::guard_condition_handle: + memset(_guard_condition_pool, 0, _max_guard_conditions); + break; + default: + throw std::runtime_error("Unrecognized enum, could not return handle memory."); + break; + } + + void *rcl_malloc(size_t size) + { + // Extremely naive static allocation strategy + // Keep track of block size at a given pointer + if (_pool_seq + size > _pool_size) + { + // Start at 0 + _pool_seq = 0; + } + void *ptr = _memory_pool[_pool_seq]; + if (_memory_map.count(ptr) == 0) + { + // We expect to have the state for all blocks pre-mapped into _memory_map + throw std::runtime_error("Unexpected pointer in rcl_malloc."); + } + _memory_map[ptr] = size; + size_t prev__pool_seq = _pool_seq; + _pool_seq += size; + return _memory_pool[prev__pool_seq]; + } + + void rcl_free(void *ptr) + { + if (_memory_map.count(ptr) == 0) + { + // We expect to have the state for all blocks pre-mapped into _memory_map + throw std::runtime_error("Unexpected pointer in rcl_free."); + } + + memset(ptr, 0, _memory_map[ptr]); + } + +protected: + + +private: + static const size_t _pool_size = 1024; + static const size_t _max_subscribers = 10; + static const size_t _max_services = 5; + static const size_t _max_clients = 10; + static const size_t _max_guard_conditions = 50; + + 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]; + size_t _pool_seq; + std::map _memory_map; +}; + +} + +} /* memory_strategies */ + +} /* rclcpp */ + +#endif From 41cc5324f4389e4470b270e2801e50f934899c6a Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 13 Jul 2015 14:48:19 -0700 Subject: [PATCH 2/6] 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; }; From 0557b115bce74b7be468adf7b34c4dcf26529562 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 13 Jul 2015 14:49:08 -0700 Subject: [PATCH 3/6] Uncrustified --- rclcpp/include/rclcpp/any_executable.hpp | 30 +++++------ rclcpp/include/rclcpp/executor.hpp | 6 +-- rclcpp/include/rclcpp/memory_strategy.hpp | 19 +++---- .../strategies/static_memory_strategy.hpp | 50 +++++++------------ 4 files changed, 44 insertions(+), 61 deletions(-) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 05c0b15..b791da3 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -23,23 +23,23 @@ namespace rclcpp { namespace executor { - struct AnyExecutable - { +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; - }; + 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; +typedef std::shared_ptr AnyExecutableSharedPtr; } } diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index b966b9c..1467d26 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -43,7 +43,7 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); Executor(memory_strategy::MemoryStrategySharedPtr ms = - memory_strategy::create_default_strategy()) + memory_strategy::create_default_strategy()) : interrupt_guard_condition_(rmw_create_guard_condition()), _memory_strategy(ms) { @@ -120,15 +120,13 @@ public: void set_memory_strategy(memory_strategy::MemoryStrategySharedPtr memory_strategy) { - if (memory_strategy == nullptr) - { + if (memory_strategy == nullptr) { throw std::runtime_error("Received NULL memory strategy in executor."); } _memory_strategy = memory_strategy; } protected: - void execute_any_executable(AnyExecutableSharedPtr & any_exec) { diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index f5e7b77..14296ee 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -20,12 +20,12 @@ namespace rclcpp { // TODO move HandleType somewhere where it makes sense -enum class HandleType{subscriber_handle, service_handle, client_handle, guard_condition_handle}; +enum class HandleType {subscriber_handle, service_handle, client_handle, guard_condition_handle}; namespace executor { - class Executor; -}; +class Executor; +} namespace memory_strategy { @@ -36,13 +36,12 @@ class MemoryStrategy friend class executor::Executor; public: - - virtual void** borrow_handles(HandleType type, size_t number_of_handles) + virtual void ** borrow_handles(HandleType type, size_t number_of_handles) { - return static_cast(rcl_malloc(sizeof(void *) * number_of_handles)); + return static_cast(rcl_malloc(sizeof(void *) * number_of_handles)); } - virtual void return_handles(HandleType type, void** handles) + virtual void return_handles(HandleType type, void ** handles) { this->rcl_free(handles); } @@ -52,20 +51,18 @@ public: return executor::AnyExecutableSharedPtr(new executor::AnyExecutable); } - virtual void *rcl_malloc(size_t size) + virtual void * rcl_malloc(size_t size) { return std::malloc(size); } - virtual void rcl_free(void *ptr) + virtual void rcl_free(void * ptr) { return std::free(ptr); } protected: - private: - }; typedef std::shared_ptr MemoryStrategySharedPtr; diff --git a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp index 1178be4..a6cb72b 100644 --- a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp @@ -1,4 +1,3 @@ - // Copyright 2015 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -31,7 +30,6 @@ class StaticMemoryStrategy : public memory_strategy::MemoryStrategy { public: - StaticMemoryStrategy() { memset(_memory_pool, 0, _pool_size); @@ -42,21 +40,18 @@ public: _pool_seq = 0; _exec_seq = 0; - for (size_t i = 0; i < _pool_size; ++i) - { + for (size_t i = 0; i < _pool_size; ++i) { _memory_map[_memory_pool[i]] = 0; } - for (size_t i = 0; i < _max_executables; ++i) - { + 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) + void ** borrow_handles(HandleType type, size_t number_of_handles) { - switch(type) - { + switch (type) { case HandleType::subscriber_handle: return _subscriber_pool; case HandleType::service_handle: @@ -71,10 +66,9 @@ public: throw std::runtime_error("Unrecognized enum, could not borrow handle memory."); } - void return_handles(HandleType type, void** handles) + void return_handles(HandleType type, void ** handles) { - switch(type) - { + switch (type) { case HandleType::subscriber_handle: memset(_subscriber_pool, 0, _max_subscribers); break; @@ -95,8 +89,7 @@ public: executor::AnyExecutableSharedPtr instantiate_next_executable() { - if (_exec_seq >= _max_executables) - { + if (_exec_seq >= _max_executables) { // wrap around _exec_seq = 0; } @@ -106,18 +99,16 @@ public: return _executable_pool[prev_exec_seq]; } - void *rcl_malloc(size_t size) + void * rcl_malloc(size_t size) { // Extremely naive static allocation strategy // Keep track of block size at a given pointer - if (_pool_seq + size > _pool_size) - { + if (_pool_seq + size > _pool_size) { // Start at 0 _pool_seq = 0; } - void *ptr = _memory_pool[_pool_seq]; - if (_memory_map.count(ptr) == 0) - { + void * ptr = _memory_pool[_pool_seq]; + if (_memory_map.count(ptr) == 0) { // We expect to have the state for all blocks pre-mapped into _memory_map throw std::runtime_error("Unexpected pointer in rcl_malloc."); } @@ -127,10 +118,9 @@ public: return _memory_pool[prev_pool_seq]; } - void rcl_free(void *ptr) + void rcl_free(void * ptr) { - if (_memory_map.count(ptr) == 0) - { + if (_memory_map.count(ptr) == 0) { // We expect to have the state for all blocks pre-mapped into _memory_map throw std::runtime_error("Unexpected pointer in rcl_free."); } @@ -139,8 +129,6 @@ public: } protected: - - private: static const size_t _pool_size = 1024; static const size_t _max_subscribers = 10; @@ -149,17 +137,17 @@ private: 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]; + 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; + std::map _memory_map; }; } From 541385a5dff2fcd36b151303176197566e3a9d81 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 13 Jul 2015 15:39:41 -0700 Subject: [PATCH 4/6] William's suggetions --- rclcpp/include/rclcpp/any_executable.hpp | 9 +++--- rclcpp/include/rclcpp/executor.hpp | 28 +++++++++---------- .../executors/multi_threaded_executor.hpp | 2 +- rclcpp/include/rclcpp/memory_strategy.hpp | 16 +++++------ .../strategies/static_memory_strategy.hpp | 14 ++++++---- 5 files changed, 37 insertions(+), 32 deletions(-) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index b791da3..001441e 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -17,15 +17,17 @@ #include +#include #include namespace rclcpp { namespace executor { + struct AnyExecutable { - + RCLCPP_MAKE_SHARED_DEFINITIONS(AnyExecutable); AnyExecutable() : subscription(0), timer(0), callback_group(0), node(0) {} @@ -39,8 +41,7 @@ struct AnyExecutable rclcpp::node::Node::SharedPtr node; }; -typedef std::shared_ptr AnyExecutableSharedPtr; -} -} +} /* executor */ +} /* rclcpp */ #endif diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 1467d26..a0546bb 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -42,7 +42,7 @@ class Executor public: RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); - Executor(memory_strategy::MemoryStrategySharedPtr ms = + Executor(memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategy::create_default_strategy()) : interrupt_guard_condition_(rmw_create_guard_condition()), _memory_strategy(ms) @@ -98,7 +98,7 @@ public: { this->add_node(node); // non-blocking = true - AnyExecutableSharedPtr any_exec = get_next_executable(nonblocking); + AnyExecutable::SharedPtr 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 - AnyExecutableSharedPtr any_exec; + AnyExecutable::SharedPtr any_exec; while ((any_exec = get_next_executable(true))) { execute_any_executable(any_exec); } @@ -118,7 +118,7 @@ public: // Support dynamic switching of memory strategy void - set_memory_strategy(memory_strategy::MemoryStrategySharedPtr memory_strategy) + set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy) { if (memory_strategy == nullptr) { throw std::runtime_error("Received NULL memory strategy in executor."); @@ -128,7 +128,7 @@ public: protected: void - execute_any_executable(AnyExecutableSharedPtr & any_exec) + execute_any_executable(AnyExecutable::SharedPtr & any_exec) { if (!any_exec) { return; @@ -571,7 +571,7 @@ protected: } void - get_next_timer(AnyExecutableSharedPtr & any_exec) + get_next_timer(AnyExecutable::SharedPtr & any_exec) { for (auto handle : guard_condition_handles_) { auto timer = get_timer_by_handle(handle); @@ -623,7 +623,7 @@ protected: } void - get_next_subscription(AnyExecutableSharedPtr & any_exec) + get_next_subscription(AnyExecutable::SharedPtr & any_exec) { for (auto handle : subscriber_handles_) { auto subscription = get_subscription_by_handle(handle); @@ -675,7 +675,7 @@ protected: } void - get_next_service(AnyExecutableSharedPtr & any_exec) + get_next_service(AnyExecutable::SharedPtr & any_exec) { for (auto handle : service_handles_) { auto service = get_service_by_handle(handle); @@ -727,7 +727,7 @@ protected: } void - get_next_client(AnyExecutableSharedPtr & any_exec) + get_next_client(AnyExecutable::SharedPtr & any_exec) { for (auto handle : client_handles_) { auto client = get_client_by_handle(handle); @@ -757,14 +757,14 @@ protected: } } - AnyExecutableSharedPtr + AnyExecutable::SharedPtr get_next_ready_executable() { return get_next_ready_executable(this->_memory_strategy->instantiate_next_executable()); } - AnyExecutableSharedPtr - get_next_ready_executable(AnyExecutableSharedPtr any_exec) + AnyExecutable::SharedPtr + get_next_ready_executable(AnyExecutable::SharedPtr any_exec) { // Check the timers to see if there are any that are ready, if so return get_next_timer(any_exec); @@ -791,7 +791,7 @@ protected: return any_exec; } - AnyExecutableSharedPtr + AnyExecutable::SharedPtr get_next_executable(bool nonblocking = false) { // Check to see if there are any subscriptions or timers needing service @@ -823,7 +823,7 @@ protected: rmw_guard_condition_t * interrupt_guard_condition_; - memory_strategy::MemoryStrategySharedPtr _memory_strategy; + memory_strategy::MemoryStrategy::SharedPtr _memory_strategy; private: RCLCPP_DISABLE_COPY(Executor); diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 53f2679..11a49f4 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; + executor::AnyExecutable::SharedPtr any_exec; { std::lock_guard wait_lock(wait_mutex_); if (!rclcpp::utilities::ok()) { diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 14296ee..6e08090 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -36,27 +36,28 @@ class MemoryStrategy friend class executor::Executor; public: + RCLCPP_MAKE_SHARED_DEFINITIONS(MemoryStrategy); virtual void ** borrow_handles(HandleType type, size_t number_of_handles) { - return static_cast(rcl_malloc(sizeof(void *) * number_of_handles)); + return static_cast(alloc(sizeof(void *) * number_of_handles)); } virtual void return_handles(HandleType type, void ** handles) { - this->rcl_free(handles); + this->free(handles); } - virtual executor::AnyExecutableSharedPtr instantiate_next_executable() + virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() { - return executor::AnyExecutableSharedPtr(new executor::AnyExecutable); + return executor::AnyExecutable::SharedPtr(new executor::AnyExecutable); } - virtual void * rcl_malloc(size_t size) + virtual void * alloc(size_t size) { return std::malloc(size); } - virtual void rcl_free(void * ptr) + virtual void free(void * ptr) { return std::free(ptr); } @@ -65,9 +66,8 @@ protected: private: }; -typedef std::shared_ptr MemoryStrategySharedPtr; -MemoryStrategySharedPtr create_default_strategy() +MemoryStrategy::SharedPtr create_default_strategy() { return std::make_shared(MemoryStrategy()); } diff --git a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp index a6cb72b..7e715eb 100644 --- a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP_RCLCPP_STATIC_MEMORY_STRATEGY_HPP_ #define RCLCPP_RCLCPP_STATIC_MEMORY_STRATEGY_HPP_ +#include + #include namespace rclcpp @@ -40,6 +42,8 @@ public: _pool_seq = 0; _exec_seq = 0; + // Reserve _pool_size buckets in the memory map. + _memory_map.reserve(_pool_size); for (size_t i = 0; i < _pool_size; ++i) { _memory_map[_memory_pool[i]] = 0; } @@ -87,7 +91,7 @@ public: } } - executor::AnyExecutableSharedPtr instantiate_next_executable() + executor::AnyExecutable::SharedPtr instantiate_next_executable() { if (_exec_seq >= _max_executables) { // wrap around @@ -99,7 +103,7 @@ public: return _executable_pool[prev_exec_seq]; } - void * rcl_malloc(size_t size) + void * alloc(size_t size) { // Extremely naive static allocation strategy // Keep track of block size at a given pointer @@ -118,7 +122,7 @@ public: return _memory_pool[prev_pool_seq]; } - void rcl_free(void * ptr) + void free(void * ptr) { if (_memory_map.count(ptr) == 0) { // We expect to have the state for all blocks pre-mapped into _memory_map @@ -142,12 +146,12 @@ private: void * _service_pool[_max_services]; void * _client_pool[_max_clients]; void * _guard_condition_pool[_max_guard_conditions]; - executor::AnyExecutableSharedPtr _executable_pool[_max_executables]; + executor::AnyExecutable::SharedPtr _executable_pool[_max_executables]; size_t _pool_seq; size_t _exec_seq; - std::map _memory_map; + std::unordered_map _memory_map; }; } From d3f5614bc7874744add7165873aa41ac27814712 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 13 Jul 2015 15:42:36 -0700 Subject: [PATCH 5/6] namespace tags --- rclcpp/include/rclcpp/memory_strategies.hpp | 4 ++-- rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp index 19da9b5..309842a 100644 --- a/rclcpp/include/rclcpp/memory_strategies.hpp +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -24,7 +24,7 @@ namespace memory_strategies using rclcpp::memory_strategies::static_memory_strategy::StaticMemoryStrategy; -} -} +} /* 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 7e715eb..c0ac7d3 100644 --- a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp @@ -154,7 +154,7 @@ private: std::unordered_map _memory_map; }; -} +} /* static_memory_strategy */ } /* memory_strategies */ From e7515303c8b5ddb45ef121eb761fb48945eb8f60 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Mon, 13 Jul 2015 16:09:54 -0700 Subject: [PATCH 6/6] Dirk's feedback: use auto, use trailing underscore, reorder includes --- rclcpp/include/rclcpp/executor.hpp | 37 +++--- .../executors/single_threaded_executor.hpp | 2 +- rclcpp/include/rclcpp/memory_strategies.hpp | 2 +- rclcpp/include/rclcpp/memory_strategy.hpp | 3 - .../strategies/static_memory_strategy.hpp | 124 ++++++++++-------- 5 files changed, 89 insertions(+), 79 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index a0546bb..ad6eb61 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -45,7 +45,7 @@ public: Executor(memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategy::create_default_strategy()) : interrupt_guard_condition_(rmw_create_guard_condition()), - _memory_strategy(ms) + memory_strategy_(ms) { } @@ -98,7 +98,7 @@ public: { this->add_node(node); // non-blocking = true - AnyExecutable::SharedPtr any_exec = get_next_executable(nonblocking); + auto any_exec = get_next_executable(nonblocking); if (any_exec) { execute_any_executable(any_exec); } @@ -109,8 +109,7 @@ public: { this->add_node(node); // non-blocking = true - AnyExecutable::SharedPtr any_exec; - while ((any_exec = get_next_executable(true))) { + while (AnyExecutable::SharedPtr any_exec = get_next_executable(true)) { execute_any_executable(any_exec); } this->remove_node(node); @@ -123,7 +122,7 @@ public: if (memory_strategy == nullptr) { throw std::runtime_error("Received NULL memory strategy in executor."); } - _memory_strategy = memory_strategy; + memory_strategy_ = memory_strategy; } protected: @@ -268,7 +267,7 @@ protected: subscriber_handles.subscriber_count = number_of_subscriptions; // TODO(wjwwood): Avoid redundant malloc's subscriber_handles.subscribers = - _memory_strategy->borrow_handles(HandleType::subscriber_handle, number_of_subscriptions); + memory_strategy_->borrow_handles(HandleType::subscriber_handle, number_of_subscriptions); if (subscriber_handles.subscribers == NULL) { // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for subscriber pointers."); @@ -286,7 +285,7 @@ protected: rmw_services_t service_handles; service_handles.service_count = number_of_services; service_handles.services = - _memory_strategy->borrow_handles(HandleType::service_handle, number_of_services); + memory_strategy_->borrow_handles(HandleType::service_handle, number_of_services); if (service_handles.services == NULL) { // TODO(esteve): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for service pointers."); @@ -304,7 +303,7 @@ protected: rmw_clients_t client_handles; client_handles.client_count = number_of_clients; client_handles.clients = - _memory_strategy->borrow_handles(HandleType::client_handle, number_of_clients); + memory_strategy_->borrow_handles(HandleType::client_handle, number_of_clients); if (client_handles.clients == NULL) { // TODO: Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for client pointers."); @@ -325,7 +324,7 @@ protected: rmw_guard_conditions_t guard_condition_handles; guard_condition_handles.guard_condition_count = number_of_guard_conds; guard_condition_handles.guard_conditions = - _memory_strategy->borrow_handles(HandleType::guard_condition_handle, number_of_guard_conds); + memory_strategy_->borrow_handles(HandleType::guard_condition_handle, number_of_guard_conds); if (guard_condition_handles.guard_conditions == NULL) { // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for guard condition pointers."); @@ -355,13 +354,13 @@ protected: // If ctrl-c guard condition, return directly if (guard_condition_handles.guard_conditions[0] != 0) { // Make sure to free or clean memory - _memory_strategy->return_handles(HandleType::subscriber_handle, + memory_strategy_->return_handles(HandleType::subscriber_handle, subscriber_handles.subscribers); - _memory_strategy->return_handles(HandleType::service_handle, + memory_strategy_->return_handles(HandleType::service_handle, service_handles.services); - _memory_strategy->return_handles(HandleType::client_handle, + memory_strategy_->return_handles(HandleType::client_handle, client_handles.clients); - _memory_strategy->return_handles(HandleType::guard_condition_handle, + memory_strategy_->return_handles(HandleType::guard_condition_handle, guard_condition_handles.guard_conditions); return; } @@ -395,13 +394,13 @@ protected: } } - _memory_strategy->return_handles(HandleType::subscriber_handle, + memory_strategy_->return_handles(HandleType::subscriber_handle, subscriber_handles.subscribers); - _memory_strategy->return_handles(HandleType::service_handle, + memory_strategy_->return_handles(HandleType::service_handle, service_handles.services); - _memory_strategy->return_handles(HandleType::client_handle, + memory_strategy_->return_handles(HandleType::client_handle, client_handles.clients); - _memory_strategy->return_handles(HandleType::guard_condition_handle, + memory_strategy_->return_handles(HandleType::guard_condition_handle, guard_condition_handles.guard_conditions); } @@ -760,7 +759,7 @@ protected: AnyExecutable::SharedPtr get_next_ready_executable() { - return get_next_ready_executable(this->_memory_strategy->instantiate_next_executable()); + return get_next_ready_executable(this->memory_strategy_->instantiate_next_executable()); } AnyExecutable::SharedPtr @@ -823,7 +822,7 @@ protected: rmw_guard_condition_t * interrupt_guard_condition_; - memory_strategy::MemoryStrategy::SharedPtr _memory_strategy; + memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; private: RCLCPP_DISABLE_COPY(Executor); diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index 8ddd759..e75007f 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -24,10 +24,10 @@ #include #include +#include #include #include #include -#include namespace rclcpp { diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp index 309842a..f7c9bd6 100644 --- a/rclcpp/include/rclcpp/memory_strategies.hpp +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// 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. diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 6e08090..6d81bce 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -61,9 +61,6 @@ public: { return std::free(ptr); } - -protected: -private: }; diff --git a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp index c0ac7d3..627ab1c 100644 --- a/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp @@ -34,22 +34,22 @@ class StaticMemoryStrategy : public memory_strategy::MemoryStrategy public: StaticMemoryStrategy() { - memset(_memory_pool, 0, _pool_size); - memset(_subscriber_pool, 0, _max_subscribers); - memset(_service_pool, 0, _max_services); - memset(_client_pool, 0, _max_clients); - memset(_guard_condition_pool, 0, _max_guard_conditions); - _pool_seq = 0; - _exec_seq = 0; + memset(memory_pool_, 0, pool_size_); + memset(subscriber_pool_, 0, max_subscribers_); + memset(service_pool_, 0, max_services_); + memset(client_pool_, 0, max_clients_); + memset(guard_condition_pool_, 0, max_guard_conditions_); + pool_seq_ = 0; + exec_seq_ = 0; - // Reserve _pool_size buckets in the memory map. - _memory_map.reserve(_pool_size); - for (size_t i = 0; i < _pool_size; ++i) { - _memory_map[_memory_pool[i]] = 0; + // Reserve pool_size_ buckets in the memory map. + memory_map_.reserve(pool_size_); + 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()); + for (size_t i = 0; i < max_executables_; ++i) { + executable_pool_[i] = std::make_shared(executor::AnyExecutable()); } } @@ -57,13 +57,29 @@ public: { switch (type) { case HandleType::subscriber_handle: - return _subscriber_pool; + if (number_of_handles > max_subscribers_) { + throw std::runtime_error("Requested size exceeded maximum subscribers."); + } + + return subscriber_pool_; case HandleType::service_handle: - return _service_pool; + if (number_of_handles > max_services_) { + throw std::runtime_error("Requested size exceeded maximum services."); + } + + return service_pool_; case HandleType::client_handle: - return _client_pool; + if (number_of_handles > max_clients_) { + throw std::runtime_error("Requested size exceeded maximum clients."); + } + + return client_pool_; case HandleType::guard_condition_handle: - return _guard_condition_pool; + if (number_of_handles > max_guard_conditions_) { + throw std::runtime_error("Requested size exceeded maximum guard conditions."); + } + + return guard_condition_pool_; default: break; } @@ -74,84 +90,82 @@ public: { switch (type) { case HandleType::subscriber_handle: - memset(_subscriber_pool, 0, _max_subscribers); + memset(subscriber_pool_, 0, max_subscribers_); break; case HandleType::service_handle: - memset(_service_pool, 0, _max_services); + memset(service_pool_, 0, max_services_); break; case HandleType::client_handle: - memset(_client_pool, 0, _max_clients); + memset(client_pool_, 0, max_clients_); break; case HandleType::guard_condition_handle: - memset(_guard_condition_pool, 0, _max_guard_conditions); + memset(guard_condition_pool_, 0, max_guard_conditions_); break; default: throw std::runtime_error("Unrecognized enum, could not return handle memory."); - break; } } executor::AnyExecutable::SharedPtr instantiate_next_executable() { - if (_exec_seq >= _max_executables) { + if (exec_seq_ >= max_executables_) { // wrap around - _exec_seq = 0; + exec_seq_ = 0; } - size_t prev_exec_seq = _exec_seq; - ++_exec_seq; + size_t prev_exec_seq_ = exec_seq_; + ++exec_seq_; - return _executable_pool[prev_exec_seq]; + return executable_pool_[prev_exec_seq_]; } void * alloc(size_t size) { // Extremely naive static allocation strategy // Keep track of block size at a given pointer - if (_pool_seq + size > _pool_size) { + if (pool_seq_ + size > pool_size_) { // Start at 0 - _pool_seq = 0; + pool_seq_ = 0; } - void * ptr = _memory_pool[_pool_seq]; - if (_memory_map.count(ptr) == 0) { - // We expect to have the state for all blocks pre-mapped into _memory_map + void * ptr = memory_pool_[pool_seq_]; + if (memory_map_.count(ptr) == 0) { + // We expect to have the state for all blocks pre-mapped into memory_map_ throw std::runtime_error("Unexpected pointer in rcl_malloc."); } - _memory_map[ptr] = size; - size_t prev_pool_seq = _pool_seq; - _pool_seq += size; - return _memory_pool[prev_pool_seq]; + memory_map_[ptr] = size; + size_t prev_pool_seq = pool_seq_; + pool_seq_ += size; + return memory_pool_[prev_pool_seq]; } void free(void * ptr) { - if (_memory_map.count(ptr) == 0) { - // We expect to have the state for all blocks pre-mapped into _memory_map + if (memory_map_.count(ptr) == 0) { + // We expect to have the state for all blocks pre-mapped into memory_map_ throw std::runtime_error("Unexpected pointer in rcl_free."); } - memset(ptr, 0, _memory_map[ptr]); + memset(ptr, 0, memory_map_[ptr]); } -protected: private: - static const size_t _pool_size = 1024; - static const size_t _max_subscribers = 10; - 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; + static const size_t pool_size_ = 1024; + static const size_t max_subscribers_ = 10; + 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::AnyExecutable::SharedPtr _executable_pool[_max_executables]; + 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::AnyExecutable::SharedPtr executable_pool_[max_executables_]; - size_t _pool_seq; - size_t _exec_seq; + size_t pool_seq_; + size_t exec_seq_; - std::unordered_map _memory_map; + std::unordered_map memory_map_; }; } /* static_memory_strategy */