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; }
bool wait_for_service_to_be_ready( rcl_service_t * service, 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, 0, 1, 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 waiting for service to be ready"); } }); size_t iteration = 0; do { ++iteration; if (rcl_wait_set_clear_services(&wait_set) != RCL_RET_OK) { fprintf(stderr, "Error in wait_set_clear_services: %s\n", rcl_get_error_string_safe()); return false; } if (rcl_wait_set_add_service(&wait_set, service) != RCL_RET_OK) { fprintf(stderr, "Error in wait_set_add_service: %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_services; ++i) { if (wait_set.services[i] && wait_set.services[i] == service) { 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; }