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

Only check for new work once in spin_some (#471) (#844)

Browse source 
To prevent the Executor::spin_some() method for potentially running
indefinitely long.

Distribution Statement A; OPSEC #2893

Signed-off-by: Roger Strain <rstrain@swri.org>
This commit is contained in:
roger-strain 2019-11-22 10:19:47 -06:00 • committed by Ivan Santiago Paunovic
parent 3e08d9e43f
commit b1dc6f36b9
1 changed files with 51 additions and 40 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -234,9 +234,11 @@ Executor::spin_some(std::chrono::nanoseconds max_duration)
throw std::runtime_error("spin_some() called while already spinning"); throw std::runtime_error("spin_some() called while already spinning");
} }
RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
// non-blocking call to pre-load all available work
wait_for_work(std::chrono::milliseconds::zero());
while (spinning.load() && max_duration_not_elapsed()) { while (spinning.load() && max_duration_not_elapsed()) {
AnyExecutable any_exec; AnyExecutable any_exec;
if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) { if (get_next_ready_executable(any_exec)) {
execute_any_executable(any_exec); execute_any_executable(any_exec);
} else { } else {
break; break;
@ -531,51 +533,39 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
bool bool
Executor::get_next_ready_executable(AnyExecutable & any_executable) Executor::get_next_ready_executable(AnyExecutable & any_executable)
{ {
bool success = false;
// Check the timers to see if there are any that are ready // Check the timers to see if there are any that are ready
memory_strategy_->get_next_timer(any_executable, weak_nodes_); memory_strategy_->get_next_timer(any_executable, weak_nodes_);
if (any_executable.timer) { if (any_executable.timer) {
return true; success = true;
} }
if (!success) {
// Check the subscriptions to see if there are any that are ready // Check the subscriptions to see if there are any that are ready
memory_strategy_->get_next_subscription(any_executable, weak_nodes_); memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
if (any_executable.subscription) { if (any_executable.subscription) {
return true; success = true;
} }
}
if (!success) {
// Check the services to see if there are any that are ready // Check the services to see if there are any that are ready
memory_strategy_->get_next_service(any_executable, weak_nodes_); memory_strategy_->get_next_service(any_executable, weak_nodes_);
if (any_executable.service) { if (any_executable.service) {
return true; success = true;
} }
}
if (!success) {
// Check the clients to see if there are any that are ready // Check the clients to see if there are any that are ready
memory_strategy_->get_next_client(any_executable, weak_nodes_); memory_strategy_->get_next_client(any_executable, weak_nodes_);
if (any_executable.client) { if (any_executable.client) {
return true; success = true;
} }
}
if (!success) {
// Check the waitables to see if there are any that are ready // Check the waitables to see if there are any that are ready
memory_strategy_->get_next_waitable(any_executable, weak_nodes_); memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
if (any_executable.waitable) { if (any_executable.waitable) {
return true; success = true;
} }
// If there is no ready executable, return a null ptr
return false;
}
bool
Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
{
bool success = false;
// Check to see if there are any subscriptions or timers needing service
// TODO(wjwwood): improve run to run efficiency of this function
success = get_next_ready_executable(any_executable);
// If there are none
if (!success) {
// Wait for subscriptions or timers to work on
wait_for_work(timeout);
if (!spinning.load()) {
return false;
}
// Try again
success = get_next_ready_executable(any_executable);
} }
// At this point any_exec should be valid with either a valid subscription // At this point any_exec should be valid with either a valid subscription
// or a valid timer, or it should be a null shared_ptr // or a valid timer, or it should be a null shared_ptr
@ -595,6 +585,27 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos
any_executable.callback_group->can_be_taken_from().store(false); any_executable.callback_group->can_be_taken_from().store(false);
} }
} }
// If there is no ready executable, return a null ptr
return success;
}
bool
Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
{
bool success = false;
// Check to see if there are any subscriptions or timers needing service
// TODO(wjwwood): improve run to run efficiency of this function
success = get_next_ready_executable(any_executable);
// If there are none
if (!success) {
// Wait for subscriptions or timers to work on
wait_for_work(timeout);
if (!spinning.load()) {
return false;
}
// Try again
success = get_next_ready_executable(any_executable);
}
return success; return success;
} }