void
GraphListener::start_if_not_started()
{
std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
if (is_shutdown_.load()) {
throw GraphListenerShutdownError();
}
if (!is_started_) {
// Initialize the wait set before starting.
rcl_ret_t ret = rcl_wait_set_init(
&wait_set_,
0, // number_of_subscriptions
2, // number_of_guard_conditions
0, // number_of_timers
0, // number_of_clients
0, // number_of_services
rcl_get_default_allocator());
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to initialize wait set");
}
// Register an on_shutdown hook to shtudown the graph listener.
// This is important to ensure that the wait set is finalized before
// destruction of static objects occurs.
std::weak_ptr<GraphListener> weak_this = shared_from_this();
rclcpp::utilities::on_shutdown([weak_this]() {
auto shared_this = weak_this.lock();
if (shared_this) {
shared_this->shutdown();
}
});
// Start the listener thread.
listener_thread_ = std::thread(&GraphListener::run, this);
is_started_ = true;
}
}
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;
}
