William's suggetions

rclcpp/include/rclcpp/any_executable.hpp

@@ -17,15 +17,17 @@
 
 #include <memory>
 
+#include <rclcpp/macros.hpp>
 #include <rclcpp/node.hpp>
 
 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<AnyExecutable> AnyExecutableSharedPtr;
-}
-}
+} /* executor */
+} /* rclcpp */
 
 #endif

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);

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

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<void **>(rcl_malloc(sizeof(void *) * number_of_handles));
+    return static_cast<void **>(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<MemoryStrategy> MemoryStrategySharedPtr;
 
-MemoryStrategySharedPtr create_default_strategy()
+MemoryStrategy::SharedPtr create_default_strategy()
 {
   return std::make_shared<MemoryStrategy>(MemoryStrategy());
 }

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 <unordered_map>
+
 #include <rclcpp/memory_strategy.hpp>
 
 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<void *, size_t> _memory_map;
+  std::unordered_map<void *, size_t> _memory_map;
 };
 
 }