rcl_wait_set_t
rcl_get_zero_initialized_wait_set()
{
static rcl_wait_set_t null_wait_set = {
.subscriptions = NULL,
.size_of_subscriptions = 0,
.guard_conditions = NULL,
.size_of_guard_conditions = 0,
.timers = NULL,
.size_of_timers = 0,
.impl = NULL,
};
return null_wait_set;
}
static bool
__wait_set_is_valid(const rcl_wait_set_t * wait_set)
{
return wait_set && wait_set->impl;
}
static void
__wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator)
{
if (wait_set->subscriptions) {
rcl_ret_t ret = rcl_wait_set_resize_subscriptions(wait_set, 0);
(void)ret; // NO LINT
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->guard_conditions) {
rcl_ret_t ret = rcl_wait_set_resize_guard_conditions(wait_set, 0);
(void)ret; // NO LINT
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->timers) {
rcl_ret_t ret = rcl_wait_set_resize_timers(wait_set, 0);
(void)ret; // NO LINT
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->impl) {
allocator.deallocate(wait_set->impl, allocator.state);
wait_set->impl = NULL;
}
}
rcl_ret_t
rcl_wait_set_init(
rcl_wait_set_t * wait_set,
size_t number_of_subscriptions,
size_t number_of_guard_conditions,
size_t number_of_timers,
rcl_allocator_t allocator)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
if (__wait_set_is_valid(wait_set)) {
RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized.");
return RCL_RET_ALREADY_INIT;
}
RCL_CHECK_FOR_NULL_WITH_MSG(
allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(
allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(
allocator.reallocate, "reallocate not set", return RCL_RET_INVALID_ARGUMENT);
// Allocate space for the implementation struct.
wait_set->impl = (rcl_wait_set_impl_t *)allocator.allocate(
sizeof(rcl_wait_set_impl_t), allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG(
wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
wait_set->impl->rmw_subscriptions.subscribers = NULL;
wait_set->impl->rmw_subscriptions.subscriber_count = 0;
wait_set->impl->rmw_guard_conditions.guard_conditions = NULL;
wait_set->impl->rmw_guard_conditions.guard_condition_count = 0;
// Initialize subscription space.
rcl_ret_t ret;
if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_subscriptions(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Initialize guard condition space.
ret = rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions);
if (ret != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_guard_conditions(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Initialize timer space.
ret = rcl_wait_set_resize_timers(wait_set, number_of_timers);
if (ret != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_timers(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Set allocator.
wait_set->impl->allocator = allocator;
return RCL_RET_OK;
fail:
__wait_set_clean_up(wait_set, allocator);
return fail_ret;
}
void
GraphListener::run_loop()
{
while (true) {
// If shutdown() was called, exit.
if (is_shutdown_.load()) {
return;
}
rcl_ret_t ret;
{
// This "barrier" lock ensures that other functions can acquire the
// node_graph_interfaces_mutex_ after waking up rcl_wait.
std::lock_guard<std::mutex> nodes_barrier_lock(node_graph_interfaces_barrier_mutex_);
// This is ownership is passed to nodes_lock in the next line.
node_graph_interfaces_mutex_.lock();
}
// This lock is released when the loop continues or exits.
std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
// Resize the wait set if necessary.
if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) {
ret = rcl_wait_set_resize_guard_conditions(&wait_set_, node_graph_interfaces_.size() + 2);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to resize wait set");
}
}
// Clear the wait set's guard conditions.
ret = rcl_wait_set_clear_guard_conditions(&wait_set_);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to clear wait set");
}
// Put the interrupt guard condition in the wait set.
ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set");
}
// Put the shutdown guard condition in the wait set.
ret = rcl_wait_set_add_guard_condition(&wait_set_, shutdown_guard_condition_);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set");
}
// Put graph guard conditions for each node into the wait set.
for (const auto node_ptr : node_graph_interfaces_) {
// Only wait on graph changes if some user of the node is watching.
if (node_ptr->count_graph_users() == 0) {
continue;
}
// Add the graph guard condition for the node to the wait set.
auto graph_gc = node_ptr->get_graph_guard_condition();
if (!graph_gc) {
throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
}
ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to add graph guard condition to wait set");
}
}
// Wait for: graph changes, interrupt, or shutdown/SIGINT
ret = rcl_wait(&wait_set_, -1); // block for ever until a guard condition is triggered
if (RCL_RET_TIMEOUT == ret) {
throw std::runtime_error("rcl_wait unexpectedly timed out");
}
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to wait on wait set");
}
bool shutdown_guard_condition_triggered = false;
// Check to see if the shutdown guard condition has been triggered.
for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
if (shutdown_guard_condition_ == wait_set_.guard_conditions[i]) {
shutdown_guard_condition_triggered = true;
}
}
// Notify nodes who's guard conditions are set (triggered).
for (const auto node_ptr : node_graph_interfaces_) {
auto graph_gc = node_ptr->get_graph_guard_condition();
if (!graph_gc) {
throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
}
for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
if (graph_gc == wait_set_.guard_conditions[i]) {
node_ptr->notify_graph_change();
}
}
if (shutdown_guard_condition_triggered) {
// If shutdown, then notify the node of this as well.
node_ptr->notify_shutdown();
}
}
} // while (true)
}
