Crash in callback group pointer vector iterator (#814)

Signed-off-by: Guillaume Autran <gautran@clearpath.ai>

rclcpp/include/rclcpp/callback_group.hpp

@@ -62,25 +62,40 @@ public:
   RCLCPP_PUBLIC
   explicit CallbackGroup(CallbackGroupType group_type);
 
-  RCLCPP_PUBLIC
+  template<typename Function>
-  const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
+  rclcpp::SubscriptionBase::SharedPtr
-  get_subscription_ptrs() const;
+  find_subscription_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::SubscriptionBase, Function>(func, subscription_ptrs_);
+  }
 
-  RCLCPP_PUBLIC
+  template<typename Function>
-  const std::vector<rclcpp::TimerBase::WeakPtr> &
+  rclcpp::TimerBase::SharedPtr
-  get_timer_ptrs() const;
+  find_timer_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::TimerBase, Function>(func, timer_ptrs_);
+  }
 
-  RCLCPP_PUBLIC
+  template<typename Function>
-  const std::vector<rclcpp::ServiceBase::WeakPtr> &
+  rclcpp::ServiceBase::SharedPtr
-  get_service_ptrs() const;
+  find_service_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::ServiceBase, Function>(func, service_ptrs_);
+  }
 
-  RCLCPP_PUBLIC
+  template<typename Function>
-  const std::vector<rclcpp::ClientBase::WeakPtr> &
+  rclcpp::ClientBase::SharedPtr
-  get_client_ptrs() const;
+  find_client_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::ClientBase, Function>(func, client_ptrs_);
+  }
 
-  RCLCPP_PUBLIC
+  template<typename Function>
-  const std::vector<rclcpp::Waitable::WeakPtr> &
+  rclcpp::Waitable::SharedPtr
-  get_waitable_ptrs() const;
+  find_waitable_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
+  }
 
   RCLCPP_PUBLIC
   std::atomic_bool &
@@ -130,6 +145,21 @@ protected:
   std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
+
+private:
+  template<typename TypeT, typename Function>
+  typename TypeT::SharedPtr _find_ptrs_if_impl(
+    Function func, const std::vector<typename TypeT::WeakPtr> & vect_ptrs) const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    for (auto & weak_ptr : vect_ptrs) {
+      auto ref_ptr = weak_ptr.lock();
+      if (ref_ptr && func(ref_ptr)) {
+        return ref_ptr;
+      }
+    }
+    return typename TypeT::SharedPtr();
+  }
 };
 
 }  // namespace callback_group

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
+#include <chrono>
 #include <memory>
 #include <mutex>
 #include <set>
@@ -53,7 +54,8 @@ public:
   MultiThreadedExecutor(
     const executor::ExecutorArgs & args = executor::ExecutorArgs(),
     size_t number_of_threads = 0,
-    bool yield_before_execute = false);
+    bool yield_before_execute = false,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();
@@ -77,6 +79,7 @@ private:
   std::mutex wait_mutex_;
   size_t number_of_threads_;
   bool yield_before_execute_;
+  std::chrono::nanoseconds next_exec_timeout_;
 
   std::set<TimerBase::SharedPtr> scheduled_timers_;
 };

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -164,40 +164,31 @@ public:
         if (!group || !group->can_be_taken_from().load()) {
           continue;
         }
-        for (auto & weak_subscription : group->get_subscription_ptrs()) {
+        group->find_subscription_ptrs_if(
-          auto subscription = weak_subscription.lock();
+          [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
-          if (subscription) {
             subscription_handles_.push_back(subscription->get_subscription_handle());
             if (subscription->get_intra_process_subscription_handle()) {
               subscription_handles_.push_back(
                 subscription->get_intra_process_subscription_handle());
             }
-          }
+            return false;
-        }
+          });
-        for (auto & weak_service : group->get_service_ptrs()) {
+        group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) {
-          auto service = weak_service.lock();
-          if (service) {
             service_handles_.push_back(service->get_service_handle());
-          }
+            return false;
-        }
+          });
-        for (auto & weak_client : group->get_client_ptrs()) {
+        group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) {
-          auto client = weak_client.lock();
-          if (client) {
             client_handles_.push_back(client->get_client_handle());
-          }
+            return false;
-        }
+          });
-        for (auto & weak_timer : group->get_timer_ptrs()) {
+        group->find_timer_ptrs_if([this](const rclcpp::TimerBase::SharedPtr & timer) {
-          auto timer = weak_timer.lock();
-          if (timer) {
             timer_handles_.push_back(timer->get_timer_handle());
-          }
+            return false;
-        }
+          });
-        for (auto & weak_waitable : group->get_waitable_ptrs()) {
+        group->find_waitable_ptrs_if([this](const rclcpp::Waitable::SharedPtr & waitable) {
-          auto waitable = weak_waitable.lock();
-          if (waitable) {
             waitable_handles_.push_back(waitable);
-          }
+            return false;
-        }
+          });
       }
     }
     return has_invalid_weak_nodes;

rclcpp/src/rclcpp/callback_group.cpp

@@ -23,40 +23,6 @@ CallbackGroup::CallbackGroup(CallbackGroupType group_type)
 : type_(group_type), can_be_taken_from_(true)
 {}
 
-const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
-CallbackGroup::get_subscription_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return subscription_ptrs_;
-}
-
-const std::vector<rclcpp::TimerBase::WeakPtr> &
-CallbackGroup::get_timer_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return timer_ptrs_;
-}
-
-const std::vector<rclcpp::ServiceBase::WeakPtr> &
-CallbackGroup::get_service_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return service_ptrs_;
-}
-
-const std::vector<rclcpp::ClientBase::WeakPtr> &
-CallbackGroup::get_client_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return client_ptrs_;
-}
-
-const std::vector<rclcpp::Waitable::WeakPtr> &
-CallbackGroup::get_waitable_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return waitable_ptrs_;
-}
 
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
@@ -76,6 +42,12 @@ CallbackGroup::add_subscription(
 {
   std::lock_guard<std::mutex> lock(mutex_);
   subscription_ptrs_.push_back(subscription_ptr);
+  subscription_ptrs_.erase(
+    std::remove_if(
+      subscription_ptrs_.begin(),
+      subscription_ptrs_.end(),
+      [](rclcpp::SubscriptionBase::WeakPtr x) {return x.expired();}),
+    subscription_ptrs_.end());
 }
 
 void
@@ -83,6 +55,12 @@ CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   timer_ptrs_.push_back(timer_ptr);
+  timer_ptrs_.erase(
+    std::remove_if(
+      timer_ptrs_.begin(),
+      timer_ptrs_.end(),
+      [](rclcpp::TimerBase::WeakPtr x) {return x.expired();}),
+    timer_ptrs_.end());
 }
 
 void
@@ -90,6 +68,12 @@ CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   service_ptrs_.push_back(service_ptr);
+  service_ptrs_.erase(
+    std::remove_if(
+      service_ptrs_.begin(),
+      service_ptrs_.end(),
+      [](rclcpp::ServiceBase::WeakPtr x) {return x.expired();}),
+    service_ptrs_.end());
 }
 
 void
@@ -97,6 +81,12 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   client_ptrs_.push_back(client_ptr);
+  client_ptrs_.erase(
+    std::remove_if(
+      client_ptrs_.begin(),
+      client_ptrs_.end(),
+      [](rclcpp::ClientBase::WeakPtr x) {return x.expired();}),
+    client_ptrs_.end());
 }
 
 void
@@ -104,6 +94,12 @@ CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   waitable_ptrs_.push_back(waitable_ptr);
+  waitable_ptrs_.erase(
+    std::remove_if(
+      waitable_ptrs_.begin(),
+      waitable_ptrs_.end(),
+      [](rclcpp::Waitable::WeakPtr x) {return x.expired();}),
+    waitable_ptrs_.end());
 }
 
 void

rclcpp/src/rclcpp/executor.cpp

@@ -511,11 +511,12 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
       if (!group) {
         continue;
       }
-      for (auto & weak_timer : group->get_timer_ptrs()) {
+      auto timer_ref = group->find_timer_ptrs_if(
-        auto t = weak_timer.lock();
+        [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
-        if (t == timer) {
+          return timer_ptr == timer;
-          return group;
+        });
-        }
+      if (timer_ref) {
+        return group;
       }
     }
   }
@@ -535,14 +536,15 @@ Executor::get_next_timer(AnyExecutable & any_exec)
       if (!group || !group->can_be_taken_from().load()) {
         continue;
       }
-      for (auto & timer_ref : group->get_timer_ptrs()) {
+      auto timer_ref = group->find_timer_ptrs_if(
-        auto timer = timer_ref.lock();
+        [](const rclcpp::TimerBase::SharedPtr & timer) -> bool {
-        if (timer && timer->is_ready()) {
+          return timer->is_ready();
-          any_exec.timer = timer;
+        });
-          any_exec.callback_group = group;
+      if (timer_ref) {
-          any_exec.node_base = node;
+        any_exec.timer = timer_ref;
-          return;
+        any_exec.callback_group = group;
-        }
+        any_exec.node_base = node;
+        return;
       }
     }
   }

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -27,8 +27,11 @@ using rclcpp::executors::MultiThreadedExecutor;
 MultiThreadedExecutor::MultiThreadedExecutor(
   const rclcpp::executor::ExecutorArgs & args,
   size_t number_of_threads,
-  bool yield_before_execute)
+  bool yield_before_execute,
-: executor::Executor(args), yield_before_execute_(yield_before_execute)
+  std::chrono::nanoseconds next_exec_timeout)
+: executor::Executor(args),
+  yield_before_execute_(yield_before_execute),
+  next_exec_timeout_(next_exec_timeout)
 {
   number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
   if (number_of_threads_ == 0) {
@@ -77,7 +80,7 @@ MultiThreadedExecutor::run(size_t)
       if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
-      if (!get_next_executable(any_exec)) {
+      if (!get_next_executable(any_exec, next_exec_timeout_)) {
         continue;
       }
       if (any_exec.timer) {

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -32,16 +32,14 @@ MemoryStrategy::get_subscription_by_handle(
       if (!group) {
         continue;
       }
-      for (auto & weak_subscription : group->get_subscription_ptrs()) {
+      auto match_subscription = group->find_subscription_ptrs_if(
-        auto subscription = weak_subscription.lock();
+        [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool {
-        if (subscription) {
+          return
-          if (subscription->get_subscription_handle() == subscriber_handle) {
+          (subscription->get_subscription_handle() == subscriber_handle) ||
-            return subscription;
+          (subscription->get_intra_process_subscription_handle() == subscriber_handle);
-          }
+        });
-          if (subscription->get_intra_process_subscription_handle() == subscriber_handle) {
+      if (match_subscription) {
-            return subscription;
+        return match_subscription;
-          }
-        }
       }
     }
   }
@@ -63,11 +61,12 @@ MemoryStrategy::get_service_by_handle(
       if (!group) {
         continue;
       }
-      for (auto & weak_service : group->get_service_ptrs()) {
+      auto service_ref = group->find_service_ptrs_if(
-        auto service = weak_service.lock();
+        [&service_handle](const rclcpp::ServiceBase::SharedPtr & service) -> bool {
-        if (service && service->get_service_handle() == service_handle) {
+          return service->get_service_handle() == service_handle;
-          return service;
+        });
-        }
+      if (service_ref) {
+        return service_ref;
       }
     }
   }
@@ -89,11 +88,12 @@ MemoryStrategy::get_client_by_handle(
       if (!group) {
         continue;
       }
-      for (auto & weak_client : group->get_client_ptrs()) {
+      auto client_ref = group->find_client_ptrs_if(
-        auto client = weak_client.lock();
+        [&client_handle](const rclcpp::ClientBase::SharedPtr & client) -> bool {
-        if (client && client->get_client_handle() == client_handle) {
+          return client->get_client_handle() == client_handle;
-          return client;
+        });
-        }
+      if (client_ref) {
+        return client_ref;
       }
     }
   }
@@ -138,11 +138,12 @@ MemoryStrategy::get_group_by_subscription(
       if (!group) {
         continue;
       }
-      for (auto & weak_sub : group->get_subscription_ptrs()) {
+      auto match_sub = group->find_subscription_ptrs_if(
-        auto sub = weak_sub.lock();
+        [&subscription](const rclcpp::SubscriptionBase::SharedPtr & sub) -> bool {
-        if (sub == subscription) {
+          return sub == subscription;
-          return group;
+        });
-        }
+      if (match_sub) {
+        return group;
       }
     }
   }
@@ -164,11 +165,12 @@ MemoryStrategy::get_group_by_service(
       if (!group) {
         continue;
       }
-      for (auto & weak_serv : group->get_service_ptrs()) {
+      auto service_ref = group->find_service_ptrs_if(
-        auto serv = weak_serv.lock();
+        [&service](const rclcpp::ServiceBase::SharedPtr & serv) -> bool {
-        if (serv && serv == service) {
+          return serv == service;
-          return group;
+        });
-        }
+      if (service_ref) {
+        return group;
       }
     }
   }
@@ -190,11 +192,12 @@ MemoryStrategy::get_group_by_client(
       if (!group) {
         continue;
       }
-      for (auto & weak_client : group->get_client_ptrs()) {
+      auto client_ref = group->find_client_ptrs_if(
-        auto cli = weak_client.lock();
+        [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool {
-        if (cli && cli == client) {
+          return cli == client;
-          return group;
+        });
-        }
+      if (client_ref) {
+        return group;
       }
     }
   }
@@ -216,11 +219,12 @@ MemoryStrategy::get_group_by_waitable(
       if (!group) {
         continue;
       }
-      for (auto & weak_waitable : group->get_waitable_ptrs()) {
+      auto waitable_ref = group->find_waitable_ptrs_if(
-        auto group_waitable = weak_waitable.lock();
+        [&waitable](const rclcpp::Waitable::SharedPtr & group_waitable) -> bool {
-        if (group_waitable && group_waitable == waitable) {
+          return group_waitable == waitable;
-          return group;
+        });
-        }
+      if (waitable_ref) {
+        return group;
       }
     }
   }