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) }