/* Tests the rcl_system_time_point_now() function.
*/
TEST_F(TestTimeFixture, test_rcl_system_time_point_now) {
assert_no_realloc_begin();
rcl_ret_t ret;
// Check for invalid argument error condition (allowed to alloc).
ret = rcl_system_time_point_now(nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
assert_no_malloc_begin();
assert_no_free_begin();
// Check for normal operation (not allowed to alloc).
rcl_system_time_point_t now = {0};
ret = rcl_system_time_point_now(&now);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_NE(now.nanoseconds, 0u);
// Compare to std::chrono::system_clock time (within a second).
now = {0};
ret = rcl_system_time_point_now(&now);
{
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
int64_t now_ns_int = now_ns.count();
int64_t now_diff = now.nanoseconds - now_ns_int;
const int k_tolerance_ms = 1000;
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs";
}
}
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;
}
void TearDown()
{
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
set_on_unexpected_malloc_callback(nullptr);
set_on_unexpected_realloc_callback(nullptr);
set_on_unexpected_free_callback(nullptr);
rcl_ret_t ret = rcl_node_fini(this->node_ptr);
delete this->node_ptr;
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
}
/**
* This function is templated on the input message type, MessageT.
* \param[in] msg A shared pointer to the message to send.
*/
virtual void
publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
{
this->do_inter_process_publish(msg.get());
if (store_intra_process_message_) {
// Take the pointer from the unique_msg, release it and pass as a void *
// to the ipm. The ipm should then capture it again as a unique_ptr of
// the correct type.
// TODO(wjwwood):
// investigate how to transfer the custom deleter (if there is one)
// from the incoming unique_ptr through to the ipm's unique_ptr.
// See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr
MessageT * msg_ptr = msg.get();
msg.release();
uint64_t message_seq =
store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
rcl_interfaces::msg::IntraProcessMessage ipm;
ipm.publisher_id = intra_process_publisher_id_;
ipm.message_sequence = message_seq;
auto status = rcl_publish(&intra_process_publisher_handle_, &ipm);
if (status != RCL_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("failed to publish intra process message: ") + rcl_get_error_string_safe());
// *INDENT-ON*
}
} else {
// Always destroy the message, even if we don't consume it, for consistency.
msg.reset();
}
}
void SetUp()
{
stop_memory_checking();
rcl_ret_t ret;
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
this->node_ptr = new rcl_node_t;
*this->node_ptr = rcl_get_zero_initialized_node();
const char * name = "node_name";
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(this->node_ptr, name, &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
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;
}
class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing::Test
{
public:
rcl_node_t * node_ptr;
void SetUp()
{
stop_memory_checking();
rcl_ret_t ret;
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
this->node_ptr = new rcl_node_t;
*this->node_ptr = rcl_get_zero_initialized_node();
const char * name = "node_name";
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(this->node_ptr, name, &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
void
rclcpp::utilities::init(int argc, char * argv[])
{
g_is_interrupted.store(false);
if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("failed to initialize rmw implementation: ") + rcl_get_error_string_safe());
// *INDENT-ON*
}
#ifdef HAS_SIGACTION
struct sigaction action;
memset(&action, 0, sizeof(action));
sigemptyset(&action.sa_mask);
action.sa_sigaction = ::signal_handler;
action.sa_flags = SA_SIGINFO;
ssize_t ret = sigaction(SIGINT, &action, &old_action);
if (ret == -1)
#else
::old_signal_handler = std::signal(SIGINT, ::signal_handler);
// NOLINTNEXTLINE(readability/braces)
if (::old_signal_handler == SIG_ERR)
#endif
{
const size_t error_length = 1024;
// NOLINTNEXTLINE(runtime/arrays)
char error_string[error_length];
#ifndef _WIN32
#ifdef _GNU_SOURCE
char * msg = strerror_r(errno, error_string, error_length);
if (msg != error_string) {
strncpy(error_string, msg, error_length);
msg[error_length - 1] = '\0';
}
#else
int error_status = strerror_r(errno, error_string, error_length);
if (error_status != 0) {
throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errno));
}
#endif
#else
strerror_s(error_string, error_length, errno);
#endif
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
error_string);
// *INDENT-ON*
}
}
/* Tests the rcl_steady_time_point_now() function.
*/
TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) {
assert_no_realloc_begin();
rcl_ret_t ret;
// Check for invalid argument error condition (allowed to alloc).
ret = rcl_steady_time_point_now(nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
assert_no_malloc_begin();
assert_no_free_begin();
// Check for normal operation (not allowed to alloc).
rcl_steady_time_point_t now = {0};
ret = rcl_steady_time_point_now(&now);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_NE(now.nanoseconds, 0u);
// Compare to std::chrono::steady_clock difference of two times (within a second).
now = {0};
ret = rcl_steady_time_point_now(&now);
std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now();
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// Wait for a little while.
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// Then take a new timestamp with each and compare.
rcl_steady_time_point_t later;
ret = rcl_steady_time_point_now(&later);
std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now();
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
int64_t steady_diff = later.nanoseconds - now.nanoseconds;
int64_t sc_diff =
std::chrono::duration_cast<std::chrono::nanoseconds>(later_sc - now_sc).count();
const int k_tolerance_ms = 1;
EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "steady_clock differs";
}
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
rclcpp::utilities::release_sigint_guard_condition(rcl_wait_set_t * waitset)
{
std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
auto kv = g_sigint_guard_cond_handles.find(waitset);
if (kv != g_sigint_guard_cond_handles.end()) {
if (rcl_guard_condition_fini(&kv->second) != RCL_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(std::string(
"Failed to destroy sigint guard condition: ") +
rcl_get_error_string_safe());
// *INDENT-ON*
}
g_sigint_guard_cond_handles.erase(kv);
} else {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(std::string(
"Tried to release sigint guard condition for nonexistent waitset"));
// *INDENT-ON*
}
}
rcl_guard_condition_t *
rclcpp::utilities::get_sigint_guard_condition(rcl_wait_set_t * waitset)
{
std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
auto kv = g_sigint_guard_cond_handles.find(waitset);
if (kv != g_sigint_guard_cond_handles.end()) {
return &kv->second;
} else {
rcl_guard_condition_t handle =
rcl_get_zero_initialized_guard_condition();
rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
if (rcl_guard_condition_init(&handle, options) != RCL_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(std::string(
"Couldn't initialize guard condition: ") + rcl_get_error_string_safe());
// *INDENT-ON*
}
g_sigint_guard_cond_handles[waitset] = handle;
return &g_sigint_guard_cond_handles[waitset];
}
}
signal_handler(int signal_value)
#endif
{
// TODO(wjwwood): remove? move to console logging at some point?
printf("signal_handler(%d)\n", signal_value);
#ifdef HAS_SIGACTION
if (old_action.sa_flags & SA_SIGINFO) {
if (old_action.sa_sigaction != NULL) {
old_action.sa_sigaction(signal_value, siginfo, context);
}
} else {
// *INDENT-OFF*
if (
old_action.sa_handler != NULL && // Is set
old_action.sa_handler != SIG_DFL && // Is not default
old_action.sa_handler != SIG_IGN) // Is not ignored
// *INDENT-ON*
{
old_action.sa_handler(signal_value);
}
}
#else
if (old_signal_handler) {
old_signal_handler(signal_value);
}
#endif
g_signal_status = signal_value;
{
std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
for (auto const & kv : g_sigint_guard_cond_handles) {
rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second));
if (status != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe());
}
}
}
g_is_interrupted.store(true);
g_interrupt_condition_variable.notify_all();
}
void
rclcpp::utilities::shutdown()
{
g_signal_status = SIGINT;
{
std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
for (auto const & kv : g_sigint_guard_cond_handles) {
if (rcl_trigger_guard_condition(&(kv.second)) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to trigger sigint guard condition: %s\n",
rcl_get_error_string_safe());
}
}
}
g_is_interrupted.store(true);
g_interrupt_condition_variable.notify_all();
{
std::lock_guard<std::mutex> lock(on_shutdown_mutex_);
for (auto & on_shutdown_callback : on_shutdown_callbacks_) {
on_shutdown_callback();
}
}
}
RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
: ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
formatted_message(rcl_get_error_string_safe())
{}
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;
}
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;
}
/* 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);
}
