/* 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";
}
}
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);
}
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();
}
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();
}
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();
}
/* 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;
}
/* Tests the allocatation checking tools.
*/
TEST(TestMemoryTools, test_allocation_checking_tools) {
size_t unexpected_mallocs = 0;
auto on_unexpected_malloc = ([&unexpected_mallocs]() {
unexpected_mallocs++;
});
set_on_unexpected_malloc_callback(on_unexpected_malloc);
size_t unexpected_reallocs = 0;
auto on_unexpected_realloc = ([&unexpected_reallocs]() {
unexpected_reallocs++;
});
set_on_unexpected_realloc_callback(on_unexpected_realloc);
size_t unexpected_frees = 0;
auto on_unexpected_free = ([&unexpected_frees]() {
unexpected_frees++;
});
set_on_unexpected_free_callback(on_unexpected_free);
void * mem = nullptr;
void * remem = nullptr;
// First try before enabling, should have no effect.
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 0u);
EXPECT_EQ(unexpected_reallocs, 0u);
EXPECT_EQ(unexpected_frees, 0u);
// Enable checking, but no assert, should have no effect.
start_memory_checking();
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 0u);
EXPECT_EQ(unexpected_reallocs, 0u);
EXPECT_EQ(unexpected_frees, 0u);
// Enable no_* asserts, should increment all once.
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
mem = malloc(1024);
assert_no_malloc_end();
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
assert_no_realloc_end();
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
assert_no_free_end();
EXPECT_EQ(unexpected_mallocs, 1u);
EXPECT_EQ(unexpected_reallocs, 1u);
EXPECT_EQ(unexpected_frees, 1u);
// Enable on malloc assert, only malloc should increment.
assert_no_malloc_begin();
mem = malloc(1024);
assert_no_malloc_end();
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 2u);
EXPECT_EQ(unexpected_reallocs, 1u);
EXPECT_EQ(unexpected_frees, 1u);
// Enable on realloc assert, only realloc should increment.
assert_no_realloc_begin();
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
assert_no_realloc_end();
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 2u);
EXPECT_EQ(unexpected_reallocs, 2u);
EXPECT_EQ(unexpected_frees, 1u);
// Enable on free assert, only free should increment.
assert_no_free_begin();
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
assert_no_free_end();
EXPECT_EQ(unexpected_mallocs, 2u);
EXPECT_EQ(unexpected_reallocs, 2u);
EXPECT_EQ(unexpected_frees, 2u);
// Go again, after disabling asserts, should have no effect.
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 2u);
EXPECT_EQ(unexpected_reallocs, 2u);
EXPECT_EQ(unexpected_frees, 2u);
// Go once more after disabling everything, should have no effect.
stop_memory_checking();
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 2u);
EXPECT_EQ(unexpected_reallocs, 2u);
EXPECT_EQ(unexpected_frees, 2u);
}
TestAllocatorFixture()
{
start_memory_checking();
stop_memory_checking();
}
/* 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);
}
