bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) { for (auto subscription : subscription_handles_) { if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add subscription to wait set: %s", rcl_get_error_string_safe()); return false; } } for (auto client : client_handles_) { if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add client to wait set: %s", rcl_get_error_string_safe()); return false; } } for (auto service : service_handles_) { if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add service to wait set: %s", rcl_get_error_string_safe()); return false; } } for (auto timer : timer_handles_) { if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add timer to wait set: %s", rcl_get_error_string_safe()); return false; } } for (auto guard_condition : guard_conditions_) { if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add guard_condition to wait set: %s", rcl_get_error_string_safe()); return false; } } return true; }
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) }