From dfb144d3cbadd3f5147d95ca6fcff7ad871804b5 Mon Sep 17 00:00:00 2001 From: fujitatomoya <43395114+fujitatomoya@users.noreply.github.com> Date: Fri, 13 Sep 2019 00:26:19 +0900 Subject: [PATCH] check valid timer handler 1st to reduce the time window for scan. (#841) Signed-off-by: Tomoya.Fujita --- rclcpp/include/rclcpp/executor.hpp | 4 -- rclcpp/include/rclcpp/memory_strategy.hpp | 15 ++++++ .../strategies/allocator_memory_strategy.hpp | 35 ++++++++++++ rclcpp/src/rclcpp/executor.cpp | 31 +---------- rclcpp/src/rclcpp/memory_strategy.cpp | 54 +++++++++++++++++++ 5 files changed, 106 insertions(+), 33 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 23e4079..095a765 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/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); diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 9b367ec..aaf730b 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/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 client_handle, const WeakNodeList & weak_nodes); + static rclcpp::TimerBase::SharedPtr + get_timer_by_handle( + std::shared_ptr 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, diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index b70d849..1ada134 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/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) { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 00b04eb..a00d8f3 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/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 - get_next_timer(any_executable); + // Check the timers to see if there are any that are ready + memory_strategy_->get_next_timer(any_executable, weak_nodes_); if (any_executable.timer) { return true; } diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 0a886e4..26c3e3c 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/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 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,