Skip to content

Commit

Permalink
Limit spin_some to only service individual timers and waitables once …
Browse files Browse the repository at this point in the history
…per execution (#471)

Distribution Statement A; OPSEC #2893

Signed-off-by: Roger Strain <rstrain@swri.org>
  • Loading branch information
Roger Strain committed Sep 4, 2019
1 parent 17841d6 commit dd6170f
Show file tree
Hide file tree
Showing 2 changed files with 35 additions and 9 deletions.
8 changes: 7 additions & 1 deletion rclcpp/include/rclcpp/executor.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -337,6 +337,10 @@ class Executor
void
get_next_timer(AnyExecutable & any_exec);

RCLCPP_PUBLIC
void
cache_ready_timers();

RCLCPP_PUBLIC
bool
get_next_ready_executable(AnyExecutable & any_executable);
Expand All @@ -345,7 +349,8 @@ class Executor
bool
get_next_executable(
AnyExecutable & any_executable,
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1),
bool allow_wait = true);

/// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
std::atomic_bool spinning;
Expand All @@ -367,6 +372,7 @@ class Executor

std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
std::list<const rcl_guard_condition_t *> guard_conditions_;
std::list<rclcpp::TimerBase::SharedPtr> ready_timer_handles_;
};

} // namespace executor
Expand Down
36 changes: 28 additions & 8 deletions rclcpp/src/rclcpp/executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -232,13 +232,18 @@ Executor::spin_some(std::chrono::nanoseconds max_duration)
throw std::runtime_error("spin_some() called while already spinning");
}
RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
// during the first run only, the process is allowed to check for new work
bool allow_wait = true;
while (spinning.load() && max_duration_not_elapsed()) {
AnyExecutable any_exec;
if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
if (get_next_executable(any_exec, std::chrono::milliseconds::zero(), allow_wait)) {
execute_any_executable(any_exec);
} else {
break;
}
// after the first run, remaining work may be performed,
// but new work may not be added
allow_wait = false;
}
}

Expand Down Expand Up @@ -426,6 +431,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
// Collect the subscriptions and timers to be waited on
memory_strategy_->clear_handles();
bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
cache_ready_timers();

// Clean up any invalid nodes, if they were detected
if (has_invalid_weak_nodes) {
Expand Down Expand Up @@ -523,8 +529,9 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
}

void
Executor::get_next_timer(AnyExecutable & any_exec)
Executor::cache_ready_timers()
{
ready_timer_handles_.clear();
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (!node) {
Expand All @@ -538,16 +545,25 @@ Executor::get_next_timer(AnyExecutable & any_exec)
for (auto & timer_ref : group->get_timer_ptrs()) {
auto timer = timer_ref.lock();
if (timer && timer->is_ready()) {
any_exec.timer = timer;
any_exec.callback_group = group;
any_exec.node_base = node;
return;
ready_timer_handles_.push_back(timer);
}
}
}
}
}

void
Executor::get_next_timer(AnyExecutable & any_exec)
{
for (auto timer : ready_timer_handles_) {
any_exec.timer = timer;
any_exec.callback_group = get_group_by_timer(timer);
any_exec.node_base = get_node_by_group(any_exec.callback_group);
ready_timer_handles_.remove(timer);
return;
}
}

bool
Executor::get_next_ready_executable(AnyExecutable & any_executable)
{
Expand Down Expand Up @@ -581,7 +597,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
}

bool
Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout, bool allow_wait)
{
bool success = false;
// Check to see if there are any subscriptions or timers needing service
Expand All @@ -590,7 +606,11 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos
// If there are none
if (!success) {
// Wait for subscriptions or timers to work on
wait_for_work(timeout);
if (allow_wait) {
// In some cases, we may not want to wait for additional work, instead
// returning control to the caller
wait_for_work(timeout);
}
if (!spinning.load()) {
return false;
}
Expand Down

0 comments on commit dd6170f

Please sign in to comment.