First pass at dependency injection of memory strategy objects

rclcpp/include/rclcpp/executor.hpp

@@ -27,6 +27,7 @@
 #include <rclcpp/macros.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/utilities.hpp>
+#include <rclcpp/memory_strategies.hpp>
 
 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<void **>(
-      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<void **>(
-      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<void **>(
-      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<void **>(
-      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::MemoryStrategy> _memory_strategy;
+
   std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
   typedef std::list<void *> SubscriberHandles;
   SubscriberHandles subscriber_handles_;

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 <rclcpp/strategies/dynamic_memory_strategy.hpp>
+#include <rclcpp/strategies/static_memory_strategy.hpp>
+
+namespace rclcpp
+{
+namespace memory_strategies
+{
+
+typedef std::shared_ptr<memory_strategy::MemoryStrategy> 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>(DynamicMemoryStrategy());
+}
+
+}
+}
+
+#endif

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

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 <rclcpp/memory_strategy.hpp>
+
+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<void**>(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

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 <rclcpp/memory_strategy.hpp>
+
+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<void*, size_t> _memory_map;
+};
+
+}
+
+}  /* memory_strategies */
+
+}  /* rclcpp */
+
+#endif