: spinning(false),

entities_need_rebuild_(true),

collector_(nullptr),

wait_set_({}, {}, {}, {}, {}, {}, context)

{

: spinning(false),

interrupt_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),

shutdown_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),

context_(options.context),

notify_waitable_(std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(

[this]() {

this->entities_need_rebuild_.store(true);

})),

entities_need_rebuild_(true),

collector_(notify_waitable_),

wait_set_({}, {}, {}, {}, {}, {}, options.context),

current_notify_waitable_(notify_waitable_),

impl_(std::make_unique<rclcpp::ExecutorImplementation>())

{

shutdown_callback_handle_ = context_->add_on_shutdown_callback(

[weak_gc = std::weak_ptr<rclcpp::GuardCondition>{shutdown_guard_condition_}]() {

auto strong_gc = weak_gc.lock();

if (strong_gc) {

strong_gc->trigger();

}

});

notify_waitable_->add_guard_condition(interrupt_guard_condition_);

notify_waitable_->add_guard_condition(shutdown_guard_condition_);

wait_set_.add_waitable(notify_waitable_);

{

this->entities_need_rebuild_.store(true);

if (!spinning.load() && entities_need_rebuild_.exchange(false)) {

std::lock_guard<std::mutex> guard(mutex_);

this->collect_entities();

}

if (notify) {

interrupt_guard_condition_->trigger();

}

{

this->collector_.update_collections();

return this->collector_.get_all_callback_groups();

{

this->collector_.update_collections();

return this->collector_.get_manually_added_callback_groups();

{

this->collector_.update_collections();

return this->collector_.get_automatically_added_callback_groups();

rclcpp::CallbackGroup::SharedPtr group_ptr,

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,

bool notify)

{

(void) node_ptr;

this->collector_.add_callback_group(group_ptr);

try {

this->handle_updated_entities(notify);

} catch (const rclcpp::exceptions::RCLError & ex) {

throw std::runtime_error(

std::string(

"Failed to handle entities update on callback group add: ") + ex.what());

}

{

this->collector_.add_node(node_ptr);

try {

this->handle_updated_entities(notify);

} catch (const rclcpp::exceptions::RCLError & ex) {

throw std::runtime_error(

std::string(

"Failed to handle entities update on node add: ") + ex.what());

}

rclcpp::CallbackGroup::SharedPtr group_ptr,

bool notify)

{

this->collector_.remove_callback_group(group_ptr);

try {

this->handle_updated_entities(notify);

} catch (const rclcpp::exceptions::RCLError & ex) {

throw std::runtime_error(

std::string(

"Failed to handle entities update on callback group remove: ") + ex.what());

}

{

this->collector_.remove_node(node_ptr);

try {

this->handle_updated_entities(notify);

} catch (const rclcpp::exceptions::RCLError & ex) {

throw std::runtime_error(

std::string(

"Failed to handle entities update on node remove: ") + ex.what());

}

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,

std::chrono::nanoseconds timeout)

{

this->add_node(node, false);

// non-blocking = true

spin_once(timeout);

this->remove_node(node, false);

std::chrono::nanoseconds timeout,

const std::function<std::future_status(std::chrono::nanoseconds wait_time)> & wait_for_future)

{

// TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete

// inside a callback executed by an executor.

// Check the future before entering the while loop.

// If the future is already complete, don't try to spin.

std::future_status status = wait_for_future(std::chrono::seconds(0));

if (status == std::future_status::ready) {

return FutureReturnCode::SUCCESS;

}

auto end_time = std::chrono::steady_clock::now();

std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(

timeout);

if (timeout_ns > std::chrono::nanoseconds::zero()) {

end_time += timeout_ns;

}

std::chrono::nanoseconds timeout_left = timeout_ns;

if (spinning.exchange(true)) {

throw std::runtime_error("spin_until_future_complete() called while already spinning");

}

RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););

while (rclcpp::ok(this->context_) && spinning.load()) {

// Do one item of work.

spin_once_impl(timeout_left);

// Check if the future is set, return SUCCESS if it is.

status = wait_for_future(std::chrono::seconds(0));

if (status == std::future_status::ready) {

return FutureReturnCode::SUCCESS;

}

// If the original timeout is < 0, then this is blocking, never TIMEOUT.

if (timeout_ns < std::chrono::nanoseconds::zero()) {

continue;

}

// Otherwise check if we still have time to wait, return TIMEOUT if not.

auto now = std::chrono::steady_clock::now();

if (now >= end_time) {

return FutureReturnCode::TIMEOUT;

}

// Subtract the elapsed time from the original timeout.

timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);

}

// The future did not complete before ok() returned false, return INTERRUPTED.

return FutureReturnCode::INTERRUPTED;

{

this->add_node(node, false);

spin_some();

this->remove_node(node, false);

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,

std::chrono::nanoseconds max_duration)

{

this->add_node(node, false);

spin_all(max_duration);

this->remove_node(node, false);

{

if (max_duration < 0ns) {

throw std::invalid_argument("max_duration must be greater than or equal to 0");

}

return this->spin_some_impl(max_duration, true);

{

auto start = std::chrono::steady_clock::now();

auto max_duration_not_elapsed = [max_duration, start]() {

if (std::chrono::nanoseconds(0) == max_duration) {

// told to spin forever if need be

return true;

} else if (std::chrono::steady_clock::now() - start < max_duration) {

// told to spin only for some maximum amount of time

return true;

}

// spun too long

return false;

};

if (spinning.exchange(true)) {

throw std::runtime_error("spin_some() called while already spinning");

}

RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););

// clear the wait result and wait for work without blocking to collect the work

// for the first time

// both spin_some and spin_all wait for work at the beginning

wait_result_.reset();

wait_for_work(std::chrono::milliseconds(0));

bool just_waited = true;

// The logic of this while loop is as follows:

//

// - while not shutdown, and spinning (not canceled), and not max duration reached...

// - try to get an executable item to execute, and execute it if available

// - otherwise, reset the wait result, and ...

// - if there was no work available just after waiting, break the loop unconditionally

// - this is appropriate for both spin_some and spin_all which use this function

// - else if exhaustive = true, then wait for work again

// - this is only used for spin_all and not spin_some

// - else break

// - this only occurs with spin_some

//

// The logic of this loop is subtle and should be carefully changed if at all.

// See also:

// https://github.com/ros2/rclcpp/issues/2508

// https://github.com/ros2/rclcpp/pull/2517

while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {

AnyExecutable any_exec;

if (get_next_ready_executable(any_exec)) {

execute_any_executable(any_exec);

just_waited = false;

} else {

// if nothing is ready, reset the result to clear it

wait_result_.reset();

if (just_waited) {

// there was no work after just waiting, always exit in this case

// before the exhaustive condition can be checked

break;

}

if (exhaustive) {

// if exhaustive, wait for work again

// this only happens for spin_all; spin_some only waits at the start

wait_for_work(std::chrono::milliseconds(0));

just_waited = true;

} else {

break;

}

}

}

{

AnyExecutable any_exec;

if (get_next_executable(any_exec, timeout)) {

execute_any_executable(any_exec);

}

{

if (spinning.exchange(true)) {

throw std::runtime_error("spin_once() called while already spinning");

}

RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););

spin_once_impl(timeout);

{

spinning.store(false);

try {

interrupt_guard_condition_->trigger();

} catch (const rclcpp::exceptions::RCLError & ex) {

throw std::runtime_error(

std::string("Failed to trigger guard condition in cancel: ") + ex.what());

}

{

if (!spinning.load()) {

return;

}

assert(

(void("cannot execute an AnyExecutable without a valid callback group"),

any_exec.callback_group));

if (any_exec.timer) {

TRACETOOLS_TRACEPOINT(

rclcpp_executor_execute,

static_cast<const void *>(any_exec.timer->get_timer_handle().get()));

execute_timer(any_exec.timer, any_exec.data);

}

if (any_exec.subscription) {

TRACETOOLS_TRACEPOINT(

rclcpp_executor_execute,

static_cast<const void *>(any_exec.subscription->get_subscription_handle().get()));

execute_subscription(any_exec.subscription);

}

if (any_exec.service) {

execute_service(any_exec.service);

}

if (any_exec.client) {

execute_client(any_exec.client);

}

if (any_exec.waitable) {

const std::shared_ptr<void> & const_data = any_exec.data;

any_exec.waitable->execute(const_data);

}

// Reset the callback_group, regardless of type

any_exec.callback_group->can_be_taken_from().store(true);

const char * action_description,

const char * topic_or_service_name,

Taker take_action,

Handler handle_action)

{

bool taken = false;

try {

taken = take_action();

} catch (const rclcpp::exceptions::RCLError & rcl_error) {

RCLCPP_ERROR(

rclcpp::get_logger("rclcpp"),

"executor %s '%s' unexpectedly failed: %s",

action_description,

topic_or_service_name,

rcl_error.what());

}

if (taken) {

handle_action();

} else {

// Message or Service was not taken for some reason.

// Note that this can be normal, if the underlying middleware needs to

// interrupt wait spuriously it is allowed.

// So in that case the executor cannot tell the difference in a

// spurious wake up and an entity actually having data until trying

// to take the data.

RCLCPP_DEBUG(

rclcpp::get_logger("rclcpp"),

"executor %s '%s' failed to take anything",

action_description,

topic_or_service_name);

}

{

using rclcpp::dynamic_typesupport::DynamicMessage;

rclcpp::MessageInfo message_info;

message_info.get_rmw_message_info().from_intra_process = false;

switch (subscription->get_delivered_message_kind()) {

// Deliver ROS message

case rclcpp::DeliveredMessageKind::ROS_MESSAGE:

{

if (subscription->can_loan_messages()) {

// This is the case where a loaned message is taken from the middleware via

// inter-process communication, given to the user for their callback,

// and then returned.

void * loaned_msg = nullptr;

// TODO(wjwwood): refactor this into methods on subscription when LoanedMessage

// is extened to support subscriptions as well.

take_and_do_error_handling(

"taking a loaned message from topic",

subscription->get_topic_name(),

[&]()

{

rcl_ret_t ret = rcl_take_loaned_message(

subscription->get_subscription_handle().get(),

&loaned_msg,

&message_info.get_rmw_message_info(),

nullptr);

if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {

return false;

} else if (RCL_RET_OK != ret) {

rclcpp::exceptions::throw_from_rcl_error(ret);

}

return true;

},

[&]() {subscription->handle_loaned_message(loaned_msg, message_info);});

if (nullptr != loaned_msg) {

rcl_ret_t ret = rcl_return_loaned_message_from_subscription(

subscription->get_subscription_handle().get(), loaned_msg);

if (RCL_RET_OK != ret) {

RCLCPP_ERROR(

rclcpp::get_logger("rclcpp"),

"rcl_return_loaned_message_from_subscription() failed for subscription on topic "

"'%s': %s",

subscription->get_topic_name(), rcl_get_error_string().str);

}

loaned_msg = nullptr;

}

} else {

// This case is taking a copy of the message data from the middleware via

// inter-process communication.

std::shared_ptr<void> message = subscription->create_message();

take_and_do_error_handling(

"taking a message from topic",

subscription->get_topic_name(),

[&]() {return subscription->take_type_erased(message.get(), message_info);},

[&]() {subscription->handle_message(message, message_info);});

// TODO(clalancette): In the case that the user is using the MessageMemoryPool,

// and they take a shared_ptr reference to the message in the callback, this can

// inadvertently return the message to the pool when the user is still using it.

// This is a bug that needs to be fixed in the pool, and we should probably have

// a custom deleter for the message that actually does the return_message().

subscription->return_message(message);

}

break;

}

// Deliver serialized message

case rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE:

{

// This is the case where a copy of the serialized message is taken from

// the middleware via inter-process communication.

std::shared_ptr<SerializedMessage> serialized_msg =

subscription->create_serialized_message();

take_and_do_error_handling(

"taking a serialized message from topic",

subscription->get_topic_name(),

[&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},

[&]()

{

subscription->handle_serialized_message(serialized_msg, message_info);

});

subscription->return_serialized_message(serialized_msg);

break;

}

// DYNAMIC SUBSCRIPTION ========================================================================

// Deliver dynamic message

case rclcpp::DeliveredMessageKind::DYNAMIC_MESSAGE:

{

throw std::runtime_error("Unimplemented");

}

default:

{

throw std::runtime_error("Delivered message kind is not supported");

}

}

{

auto request_header = service->create_request_header();

std::shared_ptr<void> request = service->create_request();

take_and_do_error_handling(

"taking a service server request from service",

service->get_service_name(),

[&]() {return service->take_type_erased_request(request.get(), *request_header);},

[&]() {service->handle_request(request_header, request);});

{

auto request_header = client->create_request_header();

std::shared_ptr<void> response = client->create_response();

take_and_do_error_handling(

"taking a service client response from service",

client->get_service_name(),

[&]() {return client->take_type_erased_response(response.get(), *request_header);},

[&]() {client->handle_response(request_header, response);});

{

// Updating the entity collection and waitset expires any active result

this->wait_result_.reset();

// Get the current list of available waitables from the collector.

rclcpp::executors::ExecutorEntitiesCollection collection;

this->collector_.update_collections();

auto callback_groups = this->collector_.get_all_callback_groups();

rclcpp::executors::build_entities_collection(callback_groups, collection);

// Make a copy of notify waitable so we can continue to mutate the original

// one outside of the execute loop.

// This prevents the collection of guard conditions in the waitable from changing

// while we are waiting on it.

if (notify_waitable_) {

current_notify_waitable_ = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(

*notify_waitable_);

auto notify_waitable = std::static_pointer_cast<rclcpp::Waitable>(current_notify_waitable_);

collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}});

}

// We must remove expired entities here, so that we don't continue to use older entities.

// See https://github.com/ros2/rclcpp/issues/2180 for more information.

current_collection_.remove_expired_entities();

// Update each of the groups of entities in the current collection, adding or removing

// from the wait set as necessary.

current_collection_.timers.update(

collection.timers,

[this](auto timer) {wait_set_.add_timer(timer);},

[this](auto timer) {wait_set_.remove_timer(timer);});

current_collection_.subscriptions.update(

collection.subscriptions,

[this](auto subscription) {

wait_set_.add_subscription(subscription, kDefaultSubscriptionMask);

},

[this](auto subscription) {

wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask);

});

current_collection_.clients.update(

collection.clients,

[this](auto client) {wait_set_.add_client(client);},

[this](auto client) {wait_set_.remove_client(client);});

current_collection_.services.update(

collection.services,

[this](auto service) {wait_set_.add_service(service);},

[this](auto service) {wait_set_.remove_service(service);});

current_collection_.guard_conditions.update(

collection.guard_conditions,

[this](auto guard_condition) {wait_set_.add_guard_condition(guard_condition);},

[this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);});

current_collection_.waitables.update(

collection.waitables,

[this](auto waitable) {wait_set_.add_waitable(waitable);},

[this](auto waitable) {wait_set_.remove_waitable(waitable);});

// In the case that an entity already has an expired weak pointer

// before being removed from the waitset, additionally prune the waitset.

this->wait_set_.prune_deleted_entities();

{

TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());

// Clear any previous wait result

this->wait_result_.reset();

{

std::lock_guard<std::mutex> guard(mutex_);

if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {

this->collect_entities();

}

}

this->wait_result_.emplace(wait_set_.wait(timeout));

if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) {

RCUTILS_LOG_WARN_NAMED(

"rclcpp",

"empty wait set received in wait(). This should never happen.");

} else {

if (this->wait_result_->kind() == WaitResultKind::Ready && current_notify_waitable_) {

auto & rcl_wait_set = this->wait_result_->get_wait_set().get_rcl_wait_set();

if (current_notify_waitable_->is_ready(rcl_wait_set)) {

current_notify_waitable_->execute(current_notify_waitable_->take_data());

}

}

}

{

TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready);

bool valid_executable = false;

if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) {

return false;

}

if (!valid_executable) {

size_t current_timer_index = 0;

while (true) {

auto [timer, timer_index] = wait_result_->peek_next_ready_timer(current_timer_index);

if (nullptr == timer) {

break;

}

current_timer_index = timer_index;

auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get());

if (entity_iter != current_collection_.timers.end()) {

auto callback_group = entity_iter->second.callback_group.lock();

if (!callback_group || !callback_group->can_be_taken_from()) {

current_timer_index++;

continue;

}

// At this point the timer is either ready for execution or was perhaps

// it was canceled, based on the result of call(), but either way it

// should not be checked again from peek_next_ready_timer(), so clear

// it from the wait result.

wait_result_->clear_timer_with_index(current_timer_index);

// Check that the timer should be called still, i.e. it wasn't canceled.

any_executable.data = timer->call();

if (!any_executable.data) {

current_timer_index++;

continue;

}

any_executable.timer = timer;

any_executable.callback_group = callback_group;

valid_executable = true;

break;

}

current_timer_index++;

}

}

if (!valid_executable) {

while (auto subscription = wait_result_->next_ready_subscription()) {

auto entity_iter = current_collection_.subscriptions.find(

subscription->get_subscription_handle().get());

if (entity_iter != current_collection_.subscriptions.end()) {

auto callback_group = entity_iter->second.callback_group.lock();

if (!callback_group || !callback_group->can_be_taken_from()) {

continue;

}

any_executable.subscription = subscription;

any_executable.callback_group = callback_group;

valid_executable = true;

break;

}

}

}

if (!valid_executable) {

while (auto service = wait_result_->next_ready_service()) {

auto entity_iter = current_collection_.services.find(service->get_service_handle().get());

if (entity_iter != current_collection_.services.end()) {

auto callback_group = entity_iter->second.callback_group.lock();

if (!callback_group || !callback_group->can_be_taken_from()) {

continue;

}

any_executable.service = service;

any_executable.callback_group = callback_group;

valid_executable = true;

break;

}

}

}

if (!valid_executable) {

while (auto client = wait_result_->next_ready_client()) {

auto entity_iter = current_collection_.clients.find(client->get_client_handle().get());

if (entity_iter != current_collection_.clients.end()) {

auto callback_group = entity_iter->second.callback_group.lock();

if (!callback_group || !callback_group->can_be_taken_from()) {

continue;

}

any_executable.client = client;

any_executable.callback_group = callback_group;

valid_executable = true;

break;

}

}

}

if (!valid_executable) {

while (auto waitable = wait_result_->next_ready_waitable()) {

auto entity_iter = current_collection_.waitables.find(waitable.get());

if (entity_iter != current_collection_.waitables.end()) {

auto callback_group = entity_iter->second.callback_group.lock();

if (!callback_group || !callback_group->can_be_taken_from()) {

continue;

}

any_executable.waitable = waitable;

any_executable.callback_group = callback_group;

any_executable.data = waitable->take_data();

valid_executable = true;

break;

}

}

}

if (any_executable.callback_group) {

if (any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) {

assert(any_executable.callback_group->can_be_taken_from().load());

any_executable.callback_group->can_be_taken_from().store(false);

}

}

return valid_executable;

{

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;