get next executable instantiation, move any_executable to own file, remove dynamic_memory_strategy

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 <memory>
+
+#include <rclcpp/node.hpp>
+
+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<AnyExecutable> AnyExecutableSharedPtr;
+}
+}
+
+#endif

rclcpp/include/rclcpp/executor.hpp

@@ -24,10 +24,11 @@
 #include <memory>
 #include <vector>
 
+#include <rclcpp/any_executable.hpp>
 #include <rclcpp/macros.hpp>
+#include <rclcpp/memory_strategy.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/utilities.hpp>
-#include <rclcpp/memory_strategies.hpp>
 
 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<AnyExecutable> 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<AnyExecutable> 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<AnyExecutable> & any_exec)
+  execute_any_executable(AnyExecutableSharedPtr & any_exec)
   {
     if (!any_exec) {
       return;
@@ -588,7 +573,7 @@ protected:
   }
 
   void
-  get_next_timer(std::shared_ptr<AnyExecutable> & 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<AnyExecutable> & 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<AnyExecutable> & 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<AnyExecutable> & 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<AnyExecutable>
+  AnyExecutableSharedPtr
   get_next_ready_executable()
   {
-    std::shared_ptr<AnyExecutable> 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<AnyExecutable>
+  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::MemoryStrategy> _memory_strategy;
-
   std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
   typedef std::list<void *> SubscriberHandles;
   SubscriberHandles subscriber_handles_;

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<AnyExecutable> any_exec;
+      std::shared_ptr<executor::AnyExecutable> any_exec;
       {
         std::lock_guard<std::mutex> wait_lock(wait_mutex_);
         if (!rclcpp::utilities::ok()) {

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -27,6 +27,7 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/utilities.hpp>
 #include <rclcpp/rate.hpp>
+#include <rclcpp/memory_strategies.hpp>
 
 namespace rclcpp
 {

rclcpp/include/rclcpp/memory_strategies.hpp

@@ -15,7 +15,6 @@
 #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
@@ -23,16 +22,8 @@ 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());
-}
-
 }
 }
 

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -14,27 +14,53 @@
 
 #ifndef RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_
 #define RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_
+#include <rclcpp/any_executable.hpp>
 
 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<void**>(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<MemoryStrategy> MemoryStrategySharedPtr;
+
+MemoryStrategySharedPtr create_default_strategy()
+{
+  return std::make_shared<MemoryStrategy>(MemoryStrategy());
+}
+
 }  /* memory_strategy */
 
 }  /* rclcpp */

rclcpp/include/rclcpp/strategies/dynamic_memory_strategy.hpp

@@ -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 <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

@@ -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>(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<void*, size_t> _memory_map;
 };