niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

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

Browse source 
Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>
This commit is contained in:
fujitatomoya 2019-09-13 00:26:19 +09:00 • committed by ivanpauno
parent 64c0f86f14
commit dfb144d3cb
5 changed files with 106 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

4
rclcpp/include/rclcpp/executor.hpp
View file

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

15
rclcpp/include/rclcpp/memory_strategy.hpp
View file

@ -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,

35
rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
View file

@ -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)
{

31
rclcpp/src/rclcpp/executor.cpp
View file

@ -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;
}

54
rclcpp/src/rclcpp/memory_strategy.cpp
View file

@ -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,