void GraphListener::shutdown() { std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_); if (!is_shutdown_.exchange(true)) { if (is_started_) { interrupt_(&interrupt_guard_condition_); listener_thread_.join(); } rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to finalize interrupt guard condition"); } if (shutdown_guard_condition_) { rclcpp::utilities::release_sigint_guard_condition(&wait_set_); shutdown_guard_condition_ = nullptr; } if (is_started_) { ret = rcl_wait_set_fini(&wait_set_); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to finalize wait set"); } } } }
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; }