Avoid redundant allocation in wait_for_work

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -46,15 +46,39 @@ class MemoryStrategy
 
   /// Borrow memory for storing data for subscriptions, services, clients, or guard conditions.
   /**
-   * The default implementation ignores the handle type and dynamically allocates the memory.
+   * The default implementation stores std::vectors for each handle type and resizes the vectors
+   * as necessary based on the requested number of handles.
    * \param[in] The type of entity that this function is requesting for.
    * \param[in] The number of handles to borrow.
    * \return Pointer to the allocated handles.
    */
   virtual void ** borrow_handles(HandleType type, size_t number_of_handles)
   {
-    (void)type;
-    return static_cast<void **>(alloc(sizeof(void *) * number_of_handles));
+    switch (type) {
+      case HandleType::subscription_handle:
+        if (subscription_handles.size() < number_of_handles) {
+          subscription_handles.resize(number_of_handles, 0);
+        }
+        return static_cast<void **>(subscription_handles.data());
+      case HandleType::service_handle:
+        if (service_handles.size() < number_of_handles) {
+          service_handles.resize(number_of_handles, 0);
+        }
+        return static_cast<void **>(service_handles.data());
+      case HandleType::client_handle:
+        if (client_handles.size() < number_of_handles) {
+          client_handles.resize(number_of_handles, 0);
+        }
+        return static_cast<void **>(client_handles.data());
+      case HandleType::guard_condition_handle:
+        if (number_of_handles > 2) {
+          throw std::runtime_error("Too many guard condition handles requested!");
+        }
+        return guard_cond_handles.data();
+      default:
+        throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast<int>(type)) +
+                ", could not borrow handle memory.");
+    }
   }
 
   /// Return the memory borrowed in borrow_handles.
@@ -65,8 +89,39 @@ class MemoryStrategy
    */
   virtual void return_handles(HandleType type, void ** handles)
   {
-    (void)type;
-    this->free(handles);
+    switch (type) {
+      case HandleType::subscription_handle:
+        if (handles != subscription_handles.data()) {
+          throw std::runtime_error(
+                  "tried to return memory that isn't handled by this MemoryStrategy");
+        }
+        memset(handles, 0, subscription_handles.size());
+        break;
+      case HandleType::service_handle:
+        if (handles != service_handles.data()) {
+          throw std::runtime_error(
+                  "tried to return memory that isn't handled by this MemoryStrategy");
+        }
+        memset(handles, 0, service_handles.size());
+        break;
+      case HandleType::client_handle:
+        if (handles != client_handles.data()) {
+          throw std::runtime_error(
+                  "tried to return memory that isn't handled by this MemoryStrategy");
+        }
+        memset(handles, 0, client_handles.size());
+        break;
+      case HandleType::guard_condition_handle:
+        if (handles != guard_cond_handles.data()) {
+          throw std::runtime_error(
+                  "tried to return memory that isn't handled by this MemoryStrategy");
+        }
+        guard_cond_handles.fill(0);
+        break;
+      default:
+        throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast<int>(type)) +
+                ", could not borrow handle memory.");
+    }
   }
 
   /// Provide a newly initialized AnyExecutable object.
@@ -101,6 +156,11 @@ class MemoryStrategy
   std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr> subs;
   std::vector<rclcpp::service::ServiceBase::SharedPtr> services;
   std::vector<rclcpp::client::ClientBase::SharedPtr> clients;
+
+  std::vector<void *> subscription_handles;
+  std::vector<void *> service_handles;
+  std::vector<void *> client_handles;
+  std::array<void *, 2> guard_cond_handles;
 };