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 Registry::Initialize(IAllocator* allocator) {
s_Allocator = allocator;
#if PROFI_ALLOCATOR == PROFI_LINEAR_ALLOCATOR
// allocate the internal allocator (ugh)
auto internalAllocator = static_cast<InternalAllocator*>(allocator->Allocate(sizeof(InternalAllocator)));
new(internalAllocator) InternalAllocator(allocator);
s_InternalAllocator = internalAllocator;
#else
s_InternalAllocator = s_Allocator;
#endif
auto instance = static_cast<Registry*>(s_InternalAllocator->Allocate(sizeof(Registry)));
auto ex = make_scope_exit([&] {
s_InternalAllocator->Deallocate(instance);
s_Allocator = s_InternalAllocator = nullptr;
});
new(instance) Registry(allocator);
ex.dismiss();
s_Instance = instance;
}
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;
}
int main(int argc, char ** argv)
{
int main_ret = 0;
{
if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
fprintf(stderr, "Error in rcl init: %s\n", rcl_get_error_string_safe());
return -1;
}
rcl_node_t node = rcl_get_zero_initialized_node();
const char * name = "node_name";
rcl_node_options_t node_options = rcl_node_get_default_options();
if (rcl_node_init(&node, name, &node_options) != RCL_RET_OK) {
fprintf(stderr, "Error in node init: %s\n", rcl_get_error_string_safe());
return -1;
}
auto node_exit = make_scope_exit([&main_ret, &node]() {
if (rcl_node_fini(&node) != RCL_RET_OK) {
fprintf(stderr, "Error in node fini: %s\n", rcl_get_error_string_safe());
main_ret = -1;
}
});
const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
example_interfaces, srv, AddTwoInts);
const char * topic = "add_two_ints";
rcl_client_t client = rcl_get_zero_initialized_client();
rcl_client_options_t client_options = rcl_client_get_default_options();
rcl_ret_t ret = rcl_client_init(&client, &node, ts, topic, &client_options);
if (ret != RCL_RET_OK) {
fprintf(stderr, "Error in client init: %s\n", rcl_get_error_string_safe());
return -1;
}
auto client_exit = make_scope_exit([&client, &main_ret, &node]() {
if (rcl_client_fini(&client, &node)) {
fprintf(stderr, "Error in client fini: %s\n", rcl_get_error_string_safe());
main_ret = -1;
}
});
// Initialize a request.
example_interfaces__srv__AddTwoInts_Request client_request;
example_interfaces__srv__AddTwoInts_Request__init(&client_request);
client_request.a = 1;
client_request.b = 2;
int64_t sequence_number;
if (rcl_send_request(&client, &client_request, &sequence_number)) {
fprintf(stderr, "Error in send request: %s\n", rcl_get_error_string_safe());
return -1;
}
if (sequence_number != 1) {
fprintf(stderr, "Got invalid sequence number\n");
return -1;
}
example_interfaces__srv__AddTwoInts_Request__fini(&client_request);
// Initialize the response owned by the client and take the response.
example_interfaces__srv__AddTwoInts_Response client_response;
example_interfaces__srv__AddTwoInts_Response__init(&client_response);
if (!wait_for_client_to_be_ready(&client, 1000, 100)) {
fprintf(stderr, "Client never became ready\n");
return -1;
}
rmw_request_id_t header;
if (rcl_take_response(&client, &header, &client_response) != RCL_RET_OK) {
fprintf(stderr, "Error in send response: %s\n", rcl_get_error_string_safe());
return -1;
}
example_interfaces__srv__AddTwoInts_Response__fini(&client_response);
}
return main_ret;
}
void
SignalThread::run_(const SignalSet& sigset)
{
// boost::asio has support for signals via signal_set. It does however not
// accept a sigset_t and neither can a sigset_t nor a ba::signal_set be inspected.
int sigfd = signalfd(-1,
&sigset.sigset(),
SFD_NONBLOCK);
if (sigfd < 0)
{
LOG_ERROR("Failed to create signalfd: " << strerror(errno));
throw Exception("Failed to create signalfd");
}
auto exit(make_scope_exit([&]
{
close(sigfd);
}));
std::function<void()> g;
auto f([&](const bs::error_code& ec,
std::size_t bytes)
{
VERIFY(bytes == 0);
if (ec)
{
LOG_ERROR("error in signal handler: " <<
ec.message());
}
else
{
signalfd_siginfo si;
ssize_t ret = ::read(sigfd,
&si,
sizeof(si));
if (ret < 0)
{
if (ret != EAGAIN)
{
ret = errno;
LOG_ERROR("failed to read siginfo from signalfd: " <<
strerror(ret) << " (" << ret << ")");
}
g();
}
else if (ret != sizeof(si))
{
LOG_ERROR("read less (" << ret << " ) than expected (" <<
sizeof(si) << ") from signalfd");
}
else
{
LOG_INFO("caught signal " << si.ssi_signo);
handler_(si.ssi_signo);
g();
}
}
});
ba::posix::stream_descriptor sigdesc(io_service_,
sigfd);
g = [&]()
{
sigdesc.async_read_some(ba::null_buffers(),
f);
};
g();
bs::error_code ec;
io_service_.run(ec);
if (ec)
{
LOG_ERROR("error running I/O service of signal thread: " <<
ec.message());
};
LOG_INFO("exiting signal handler thread");
}
int main(int argc, char ** argv)
{
int main_ret = 0;
{
if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
fprintf(stderr, "Error in rcl init: %s\n", rcl_get_error_string_safe());
return -1;
}
rcl_node_t node = rcl_get_zero_initialized_node();
const char * name = "node_name";
rcl_node_options_t node_options = rcl_node_get_default_options();
if (rcl_node_init(&node, name, &node_options) != RCL_RET_OK) {
fprintf(stderr, "Error in node init: %s\n", rcl_get_error_string_safe());
return -1;
}
auto node_exit = make_scope_exit([&main_ret, &node]() {
if (rcl_node_fini(&node) != RCL_RET_OK) {
fprintf(stderr, "Error in node fini: %s\n", rcl_get_error_string_safe());
main_ret = -1;
}
});
const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
example_interfaces, srv, AddTwoInts);
const char * topic = "add_two_ints";
rcl_service_t service = rcl_get_zero_initialized_service();
rcl_service_options_t service_options = rcl_service_get_default_options();
rcl_ret_t ret = rcl_service_init(&service, &node, ts, topic, &service_options);
if (ret != RCL_RET_OK) {
fprintf(stderr, "Error in service init: %s\n", rcl_get_error_string_safe());
return -1;
}
auto service_exit = make_scope_exit([&main_ret, &service, &node]() {
if (rcl_service_fini(&service, &node)) {
fprintf(stderr, "Error in service fini: %s\n", rcl_get_error_string_safe());
main_ret = -1;
}
});
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// Initialize a response.
example_interfaces__srv__AddTwoInts_Response service_response;
example_interfaces__srv__AddTwoInts_Response__init(&service_response);
auto response_exit = make_scope_exit([&service_response]() {
example_interfaces__srv__AddTwoInts_Response__fini(&service_response);
});
// Block until a client request comes in.
if (!wait_for_service_to_be_ready(&service, 1000, 100)) {
fprintf(stderr, "Service never became ready\n");
return -1;
}
// Take the pending request.
example_interfaces__srv__AddTwoInts_Request service_request;
example_interfaces__srv__AddTwoInts_Request__init(&service_request);
auto request_exit = make_scope_exit([&service_request]() {
example_interfaces__srv__AddTwoInts_Request__fini(&service_request);
});
rmw_request_id_t header;
// TODO(jacquelinekay) May have to check for timeout error codes
if (rcl_take_request(&service, &header, &service_request) != RCL_RET_OK) {
fprintf(stderr, "Error in take_request: %s\n", rcl_get_error_string_safe());
return -1;
}
// Sum the request and send the response.
service_response.sum = service_request.a + service_request.b;
if (rcl_send_response(&service, &header, &service_response) != RCL_RET_OK) {
fprintf(stderr, "Error in send_response: %s\n", rcl_get_error_string_safe());
return -1;
}
// Our scope exits should take care of fini for everything
}
return main_ret;
}
/* Basic nominal test of a service.
*/
TEST_F(TestServiceFixture, test_service_nominal) {
stop_memory_checking();
rcl_ret_t ret;
const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
example_interfaces, srv, AddTwoInts);
const char * topic = "add_two_ints";
rcl_service_t service = rcl_get_zero_initialized_service();
rcl_service_options_t service_options = rcl_service_get_default_options();
ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
// Check that the service name matches what we assigned.
EXPECT_EQ(strcmp(rcl_service_get_service_name(&service), topic), 0);
auto service_exit = make_scope_exit([&service, this]() {
stop_memory_checking();
rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
});
rcl_client_t client = rcl_get_zero_initialized_client();
rcl_client_options_t client_options = rcl_client_get_default_options();
ret = rcl_client_init(&client, this->node_ptr, ts, topic, &client_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
auto client_exit = make_scope_exit([&client, this]() {
stop_memory_checking();
rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
});
// TODO(wjwwood): add logic to wait for the connection to be established
// use count_services busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// Initialize a request.
example_interfaces__srv__AddTwoInts_Request client_request;
example_interfaces__srv__AddTwoInts_Request__init(&client_request);
client_request.a = 1;
client_request.b = 2;
int64_t sequence_number;
ret = rcl_send_request(&client, &client_request, &sequence_number);
EXPECT_EQ(sequence_number, 1);
example_interfaces__srv__AddTwoInts_Request__fini(&client_request);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
bool success;
wait_for_service_to_be_ready(&service, 10, 100, success);
ASSERT_TRUE(success);
// This scope simulates the service responding in a different context so that we can
// test take_request/send_response in a single-threaded, deterministic execution.
{
// Initialize a response.
example_interfaces__srv__AddTwoInts_Response service_response;
example_interfaces__srv__AddTwoInts_Response__init(&service_response);
auto msg_exit = make_scope_exit([&service_response]() {
stop_memory_checking();
example_interfaces__srv__AddTwoInts_Response__fini(&service_response);
});
// Initialize a separate instance of the request and take the pending request.
example_interfaces__srv__AddTwoInts_Request service_request;
example_interfaces__srv__AddTwoInts_Request__init(&service_request);
rmw_request_id_t header;
ret = rcl_take_request(&service, &header, &service_request);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
EXPECT_EQ(1, service_request.a);
EXPECT_EQ(2, service_request.b);
// Simulate a response callback by summing the request and send the response..
service_response.sum = service_request.a + service_request.b;
ret = rcl_send_response(&service, &header, &service_response);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
}
wait_for_service_to_be_ready(&service, 10, 100, success);
// Initialize the response owned by the client and take the response.
example_interfaces__srv__AddTwoInts_Response client_response;
example_interfaces__srv__AddTwoInts_Response__init(&client_response);
rmw_request_id_t header;
ret = rcl_take_response(&client, &header, &client_response);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
EXPECT_EQ(client_response.sum, 3);
EXPECT_EQ(header.sequence_number, 1);
}
