{

RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(WaitSetTemplate)

using typename StoragePolicy::SubscriptionEntry;

using typename StoragePolicy::WaitableEntry;

/// Construct a wait set with optional initial waitable entities and optional custom context.

/**

explicit

WaitSetTemplate(

const typename StoragePolicy::SubscriptionsIterable & subscriptions = {},

const typename StoragePolicy::GuardConditionsIterable & guard_conditions = {},

const typename StoragePolicy::TimersIterable & timers = {},

const typename StoragePolicy::ClientsIterable & clients = {},

const typename StoragePolicy::ServicesIterable & services = {},

const typename StoragePolicy::WaitablesIterable & waitables = {},

rclcpp::Context::SharedPtr context =

rclcpp::contexts::get_global_default_context())

: SynchronizationPolicy(context),

StoragePolicy(

subscriptions,

guard_conditions,

// this method comes from the SynchronizationPolicy

this->get_extra_guard_conditions(),

timers,

clients,

services,

waitables,

context)

{}

/// Return the internal rcl wait set object.

/**

const rcl_wait_set_t &

get_rcl_wait_set() const

{

// this method comes from the StoragePolicy

return this->storage_get_rcl_wait_set();

}

/// Add a subscription to this wait set.

/**

void

add_subscription(

std::shared_ptr<rclcpp::SubscriptionBase> subscription,

rclcpp::SubscriptionWaitSetMask mask = {})

{

if (nullptr == subscription) {

throw std::invalid_argument("subscription is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_add_subscription(

std::move(subscription),

mask,

[this](

std::shared_ptr<rclcpp::SubscriptionBase> && inner_subscription,

const rclcpp::SubscriptionWaitSetMask & mask)

{

// These methods comes from the StoragePolicy, and may not exist for

// fixed sized storage policies.

// It will throw if the subscription has already been added.

if (mask.include_subscription) {

auto local_subscription = inner_subscription;

bool already_in_use =

local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), true);

if (already_in_use) {

throw std::runtime_error("subscription already associated with a wait set");

}

this->storage_add_subscription(std::move(local_subscription));

}

if (mask.include_events) {

for (auto event : inner_subscription->get_event_handlers()) {

auto local_subscription = inner_subscription;

bool already_in_use =

local_subscription->exchange_in_use_by_wait_set_state(event.get(), true);

if (already_in_use) {

throw std::runtime_error("subscription event already associated with a wait set");

}

this->storage_add_waitable(std::move(event), std::move(local_subscription));

}

}

if (mask.include_intra_process_waitable) {

auto local_subscription = inner_subscription;

auto waitable = inner_subscription->get_intra_process_waitable();

if (nullptr != waitable) {

bool already_in_use = local_subscription->exchange_in_use_by_wait_set_state(

waitable.get(),

true);

if (already_in_use) {

throw std::runtime_error(

"subscription intra-process waitable already associated with a wait set");

}

this->storage_add_waitable(

std::move(inner_subscription->get_intra_process_waitable()),

std::move(local_subscription));

}

}

});

}

/// Remove a subscription from this wait set.

/**

void

remove_subscription(

std::shared_ptr<rclcpp::SubscriptionBase> subscription,

rclcpp::SubscriptionWaitSetMask mask = {})

{

if (nullptr == subscription) {

throw std::invalid_argument("subscription is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_remove_subscription(

std::move(subscription),

mask,

[this](

std::shared_ptr<rclcpp::SubscriptionBase> && inner_subscription,

const rclcpp::SubscriptionWaitSetMask & mask)

{

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the subscription is not in the wait set.

if (mask.include_subscription) {

auto local_subscription = inner_subscription;

local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false);

this->storage_remove_subscription(std::move(local_subscription));

}

if (mask.include_events) {

for (auto event : inner_subscription->get_event_handlers()) {

auto local_subscription = inner_subscription;

local_subscription->exchange_in_use_by_wait_set_state(event.get(), false);

this->storage_remove_waitable(std::move(event));

}

}

if (mask.include_intra_process_waitable) {

auto local_waitable = inner_subscription->get_intra_process_waitable();

if (nullptr != local_waitable) {

// This is the case when intra process is enabled for the subscription.

inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);

this->storage_remove_waitable(std::move(local_waitable));

}

}

});

}

/// Add a guard condition to this wait set.

/**

void

add_guard_condition(std::shared_ptr<rclcpp::GuardCondition> guard_condition)

{

if (nullptr == guard_condition) {

throw std::invalid_argument("guard_condition is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_add_guard_condition(

std::move(guard_condition),

[this](std::shared_ptr<rclcpp::GuardCondition> && inner_guard_condition) {

bool already_in_use = inner_guard_condition->exchange_in_use_by_wait_set_state(true);

if (already_in_use) {

throw std::runtime_error("guard condition already in use by another wait set");

}

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the guard condition has already been added.

this->storage_add_guard_condition(std::move(inner_guard_condition));

});

}

/// Remove a guard condition from this wait set.

/**

void

remove_guard_condition(std::shared_ptr<rclcpp::GuardCondition> guard_condition)

{

if (nullptr == guard_condition) {

throw std::invalid_argument("guard_condition is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_remove_guard_condition(

std::move(guard_condition),

[this](std::shared_ptr<rclcpp::GuardCondition> && inner_guard_condition) {

inner_guard_condition->exchange_in_use_by_wait_set_state(false);

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the guard condition is not in the wait set.

this->storage_remove_guard_condition(std::move(inner_guard_condition));

});

}

/// Add a timer to this wait set.

/**

void

add_timer(std::shared_ptr<rclcpp::TimerBase> timer)

{

if (nullptr == timer) {

throw std::invalid_argument("timer is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_add_timer(

std::move(timer),

[this](std::shared_ptr<rclcpp::TimerBase> && inner_timer) {

bool already_in_use = inner_timer->exchange_in_use_by_wait_set_state(true);

if (already_in_use) {

throw std::runtime_error("timer already in use by another wait set");

}

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the timer has already been added.

this->storage_add_timer(std::move(inner_timer));

});

}

/// Remove a timer from this wait set.

/**

void

remove_timer(std::shared_ptr<rclcpp::TimerBase> timer)

{

if (nullptr == timer) {

throw std::invalid_argument("timer is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_remove_timer(

std::move(timer),

[this](std::shared_ptr<rclcpp::TimerBase> && inner_timer) {

inner_timer->exchange_in_use_by_wait_set_state(false);

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the timer is not in the wait set.

this->storage_remove_timer(std::move(inner_timer));

});

}

/// Add a client to this wait set.

/**

void

add_client(std::shared_ptr<rclcpp::ClientBase> client)

{

if (nullptr == client) {

throw std::invalid_argument("client is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_add_client(

std::move(client),

[this](std::shared_ptr<rclcpp::ClientBase> && inner_client) {

bool already_in_use = inner_client->exchange_in_use_by_wait_set_state(true);

if (already_in_use) {

throw std::runtime_error("client already in use by another wait set");

}

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the client has already been added.

this->storage_add_client(std::move(inner_client));

});

}

/// Remove a client from this wait set.

/**

void

remove_client(std::shared_ptr<rclcpp::ClientBase> client)

{

if (nullptr == client) {

throw std::invalid_argument("client is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_remove_client(

std::move(client),

[this](std::shared_ptr<rclcpp::ClientBase> && inner_client) {

inner_client->exchange_in_use_by_wait_set_state(false);

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the client is not in the wait set.

this->storage_remove_client(std::move(inner_client));

});

}

/// Add a service to this wait set.

/**

void

add_service(std::shared_ptr<rclcpp::ServiceBase> service)

{

if (nullptr == service) {

throw std::invalid_argument("service is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_add_service(

std::move(service),

[this](std::shared_ptr<rclcpp::ServiceBase> && inner_service) {

bool already_in_use = inner_service->exchange_in_use_by_wait_set_state(true);

if (already_in_use) {

throw std::runtime_error("service already in use by another wait set");

}

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the service has already been added.

this->storage_add_service(std::move(inner_service));

});

}

/// Remove a service from this wait set.

/**

void

remove_service(std::shared_ptr<rclcpp::ServiceBase> service)

{

if (nullptr == service) {

throw std::invalid_argument("service is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_remove_service(

std::move(service),

[this](std::shared_ptr<rclcpp::ServiceBase> && inner_service) {

inner_service->exchange_in_use_by_wait_set_state(false);

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the service is not in the wait set.

this->storage_remove_service(std::move(inner_service));

});

}

/// Add a waitable to this wait set.

/**

void

add_waitable(

std::shared_ptr<rclcpp::Waitable> waitable,

std::shared_ptr<void> associated_entity = nullptr)

{

if (nullptr == waitable) {

throw std::invalid_argument("waitable is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_add_waitable(

std::move(waitable),

std::move(associated_entity),

[this](

std::shared_ptr<rclcpp::Waitable> && inner_waitable,

std::shared_ptr<void> && associated_entity)

{

bool already_in_use = inner_waitable->exchange_in_use_by_wait_set_state(true);

if (already_in_use) {

throw std::runtime_error("waitable already in use by another wait set");

}

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the waitable has already been added.

this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity));

});

}

/// Remove a waitable from this wait set.

/**

void

remove_waitable(std::shared_ptr<rclcpp::Waitable> waitable)

{

if (nullptr == waitable) {

throw std::invalid_argument("waitable is nullptr");

}

// this method comes from the SynchronizationPolicy

this->sync_remove_waitable(

std::move(waitable),

[this](std::shared_ptr<rclcpp::Waitable> && inner_waitable) {

inner_waitable->exchange_in_use_by_wait_set_state(false);

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

// It will throw if the waitable is not in the wait set.

this->storage_remove_waitable(std::move(inner_waitable));

});

}

/// Remove any destroyed entities from the wait set.

/**

void

prune_deleted_entities()

{

// this method comes from the SynchronizationPolicy

this->sync_prune_deleted_entities(

[this]() {

// This method comes from the StoragePolicy, and it may not exist for

// fixed sized storage policies.

this->storage_prune_deleted_entities();

});

}

/// Wait for any of the entities in the wait set to be ready, or a period of time to pass.

/**

template<class Rep = int64_t, class Period = std::milli>

RCUTILS_WARN_UNUSED

WaitResult<WaitSetTemplate>

wait(std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))

{

auto time_to_wait_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(time_to_wait);

// ensure the ownership of the entities in the wait set is shared for the duration of wait

this->storage_acquire_ownerships();

RCPPUTILS_SCOPE_EXIT({this->storage_release_ownerships();});

// this method comes from the SynchronizationPolicy

return this->template sync_wait<WaitResult<WaitSetTemplate>>(

// pass along the time_to_wait duration as nanoseconds

time_to_wait_ns,

// this method provides the ability to rebuild the wait set, if needed

[this]() {

// This method comes from the StoragePolicy

this->storage_rebuild_rcl_wait_set(

// This method comes from the SynchronizationPolicy

this->get_extra_guard_conditions()

);

},

// this method provides access to the rcl wait set

[this]() -> rcl_wait_set_t & {

// This method comes from the StoragePolicy

return this->storage_get_rcl_wait_set();

},

// this method provides a way to create the WaitResult

[this](WaitResultKind wait_result_kind) -> WaitResult<WaitSetTemplate> {

// convert the result into a WaitResult

switch (wait_result_kind) {

case WaitResultKind::Ready:

return WaitResult<WaitSetTemplate>::from_ready_wait_result_kind(*this);

case WaitResultKind::Timeout:

return WaitResult<WaitSetTemplate>::from_timeout_wait_result_kind();

case WaitResultKind::Empty:

return WaitResult<WaitSetTemplate>::from_empty_wait_result_kind();

default:

auto msg = "unknown WaitResultKind with value: " + std::to_string(wait_result_kind);

throw std::runtime_error(msg);

}

}

);

}

// Add WaitResult type as a friend so it can call private methods for

// acquiring and releasing resources as the WaitResult is initialized and

// destructed, respectively.

friend WaitResult<WaitSetTemplate>;

/// Called by the WaitResult's constructor to place a hold on ownership and thread-safety.

/**

void

wait_result_acquire()

{

if (wait_result_holding_) {

throw std::runtime_error("wait_result_acquire() called while already holding");

}

wait_result_holding_ = true;

// this method comes from the SynchronizationPolicy

this->sync_wait_result_acquire();

// this method comes from the StoragePolicy

this->storage_acquire_ownerships();

}

/// Called by the WaitResult's destructor to release resources.

/**

void

wait_result_release()

{

if (!wait_result_holding_) {

throw std::runtime_error("wait_result_release() called while not holding");

}

wait_result_holding_ = false;

// this method comes from the StoragePolicy

this->storage_release_ownerships();

// this method comes from the SynchronizationPolicy

this->sync_wait_result_release();

}

bool wait_result_holding_ = false;