bool wait_for_client_to_be_ready( rcl_client_t * client, size_t max_tries, int64_t period_ms) { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, rcl_get_default_allocator()); if (ret != RCL_RET_OK) { fprintf(stderr, "Error in wait set init: %s\n", rcl_get_error_string_safe()); return false; } auto wait_set_exit = make_scope_exit([&wait_set]() { if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { fprintf(stderr, "Error in wait set fini: %s\n", rcl_get_error_string_safe()); throw std::runtime_error("error while waiting for client"); } }); size_t iteration = 0; do { ++iteration; if (rcl_wait_set_clear_clients(&wait_set) != RCL_RET_OK) { fprintf(stderr, "Error in wait_set_clear_clients: %s\n", rcl_get_error_string_safe()); return false; } if (rcl_wait_set_add_client(&wait_set, client) != RCL_RET_OK) { fprintf(stderr, "Error in wait_set_add_client: %s\n", rcl_get_error_string_safe()); return false; } ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); if (ret == RCL_RET_TIMEOUT) { continue; } if (ret != RCL_RET_OK) { fprintf(stderr, "Error in wait: %s\n", rcl_get_error_string_safe()); return false; } for (size_t i = 0; i < wait_set.size_of_clients; ++i) { if (wait_set.clients[i] && wait_set.clients[i] == client) { return true; } } } while (iteration < max_tries); return false; }
void wait_for_service_to_be_ready( rcl_service_t * service, size_t max_tries, int64_t period_ms, bool & success) { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator()); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); auto wait_set_exit = make_scope_exit([&wait_set]() { stop_memory_checking(); rcl_ret_t ret = rcl_wait_set_fini(&wait_set); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); }); size_t iteration = 0; do { ++iteration; ret = rcl_wait_set_clear_services(&wait_set); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait_set_add_service(&wait_set, service); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); if (ret == RCL_RET_TIMEOUT) { continue; } ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); for (size_t i = 0; i < wait_set.size_of_services; ++i) { if (wait_set.services[i] && wait_set.services[i] == service) { success = true; return; } } } while (iteration < max_tries); success = false; }
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) }