check valid timer handler 1st to reduce the time window for scan. (#841)

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>

rclcpp/include/rclcpp/executor.hpp

@@ -334,10 +334,6 @@ protected:
   rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
 
-  RCLCPP_PUBLIC
-  void
-  get_next_timer(AnyExecutable & any_exec);
-
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable(AnyExecutable & any_executable);

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -79,6 +79,11 @@ public:
     rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeList & weak_nodes) = 0;
 
+  virtual void
+  get_next_timer(
+    rclcpp::executor::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) = 0;
+
   virtual void
   get_next_waitable(
     rclcpp::executor::AnyExecutable & any_exec,
@@ -102,6 +107,11 @@ public:
     std::shared_ptr<const rcl_client_t> client_handle,
     const WeakNodeList & weak_nodes);
 
+  static rclcpp::TimerBase::SharedPtr
+  get_timer_by_handle(
+    std::shared_ptr<const rcl_timer_t> timer_handle,
+    const WeakNodeList & weak_nodes);
+
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
     rclcpp::callback_group::CallbackGroup::SharedPtr group,
@@ -122,6 +132,11 @@ public:
     rclcpp::ClientBase::SharedPtr client,
     const WeakNodeList & weak_nodes);
 
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_timer(
+    rclcpp::TimerBase::SharedPtr timer,
+    const WeakNodeList & weak_nodes);
+
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_waitable(
     rclcpp::Waitable::SharedPtr waitable,

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -365,6 +365,41 @@ public:
     }
   }
 
+  virtual void
+  get_next_timer(
+    executor::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes)
+  {
+    auto it = timer_handles_.begin();
+    while (it != timer_handles_.end()) {
+      auto timer = get_timer_by_handle(*it, weak_nodes);
+      if (timer) {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_timer(timer, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the timer is not valid...
+          // Remove it from the ready list and continue looking
+          it = timer_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec.timer = timer;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        timer_handles_.erase(it);
+        return;
+      }
+      // Else, the service is no longer valid, remove it and continue
+      it = timer_handles_.erase(it);
+    }
+  }
+
   virtual void
   get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes)
   {

rclcpp/src/rclcpp/executor.cpp

@@ -529,38 +529,11 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
   return rclcpp::callback_group::CallbackGroup::SharedPtr();
 }
 
-void
-Executor::get_next_timer(AnyExecutable & any_exec)
-{
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (!node) {
-      continue;
-    }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group || !group->can_be_taken_from().load()) {
-        continue;
-      }
-      auto timer_ref = group->find_timer_ptrs_if(
-        [](const rclcpp::TimerBase::SharedPtr & timer) -> bool {
-          return timer->is_ready();
-        });
-      if (timer_ref) {
-        any_exec.timer = timer_ref;
-        any_exec.callback_group = group;
-        any_exec.node_base = node;
-        return;
-      }
-    }
-  }
-}
-
 bool
 Executor::get_next_ready_executable(AnyExecutable & any_executable)
 {
-  // Check the timers to see if there are any that are ready, if so return
+  // Check the timers to see if there are any that are ready
-  get_next_timer(any_executable);
+  memory_strategy_->get_next_timer(any_executable, weak_nodes_);
   if (any_executable.timer) {
     return true;
   }

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -100,6 +100,33 @@ MemoryStrategy::get_client_by_handle(
   return nullptr;
 }
 
+rclcpp::TimerBase::SharedPtr
+MemoryStrategy::get_timer_by_handle(
+  std::shared_ptr<const rcl_timer_t> timer_handle,
+  const WeakNodeList & weak_nodes)
+{
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto group = weak_group.lock();
+      if (!group) {
+        continue;
+      }
+      auto timer_ref = group->find_timer_ptrs_if(
+        [&timer_handle](const rclcpp::TimerBase::SharedPtr & timer) -> bool {
+          return timer->get_timer_handle() == timer_handle;
+        });
+      if (timer_ref) {
+        return timer_ref;
+      }
+    }
+  }
+  return nullptr;
+}
+
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 MemoryStrategy::get_node_by_group(
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
@@ -204,6 +231,33 @@ MemoryStrategy::get_group_by_client(
   return nullptr;
 }
 
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_timer(
+  rclcpp::TimerBase::SharedPtr timer,
+  const WeakNodeList & weak_nodes)
+{
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto group = weak_group.lock();
+      if (!group) {
+        continue;
+      }
+      auto timer_ref = group->find_timer_ptrs_if(
+        [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool {
+          return time == timer;
+        });
+      if (timer_ref) {
+        return group;
+      }
+    }
+  }
+  return nullptr;
+}
+
 rclcpp::callback_group::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_waitable(
   rclcpp::Waitable::SharedPtr waitable,