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