/* Tests the rcl_get_instance_id() and rcl_ok() functions.
*/
TEST_F(TestRCLFixture, test_rcl_get_instance_id_and_ok) {
rcl_ret_t ret;
// Instance id should be 0 before rcl_init().
EXPECT_EQ(0u, rcl_get_instance_id());
ASSERT_FALSE(rcl_ok());
// It should still return 0 after an invalid init.
ret = rcl_init(1, nullptr, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
EXPECT_EQ(0u, rcl_get_instance_id());
ASSERT_FALSE(rcl_ok());
// A non-zero instance id should be returned after a valid init.
{
FakeTestArgv test_args;
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
}
// And it should be allocation free.
uint64_t first_instance_id;
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
first_instance_id = rcl_get_instance_id();
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
EXPECT_NE(0u, first_instance_id);
EXPECT_EQ(first_instance_id, rcl_get_instance_id()); // Repeat calls should return the same.
EXPECT_EQ(true, rcl_ok());
// Calling after a shutdown should return 0.
ret = rcl_shutdown();
EXPECT_EQ(ret, RCL_RET_OK);
EXPECT_EQ(0u, rcl_get_instance_id());
ASSERT_FALSE(rcl_ok());
// It should return a different value after another valid init.
{
FakeTestArgv test_args;
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
}
EXPECT_NE(0u, rcl_get_instance_id());
EXPECT_NE(first_instance_id, rcl_get_instance_id());
ASSERT_TRUE(rcl_ok());
// Shutting down a second time should result in 0 again.
ret = rcl_shutdown();
EXPECT_EQ(ret, RCL_RET_OK);
EXPECT_EQ(0u, rcl_get_instance_id());
ASSERT_FALSE(rcl_ok());
}
/*
* Expected usage:
*
* rcl_node_t node = rcl_get_zero_initialized_node();
* rcl_node_options_t * node_ops = rcl_node_get_default_options();
* rcl_ret_t ret = rcl_node_init(&node, "node_name", node_ops);
* // ... error handling and then use the node, but finally deinitialize it:
* ret = rcl_node_fini(&node);
* // ... error handling for rcl_node_fini()
*/
int main(int argc, char ** argv)
{
rcl_ret_t ret1 = rcl_init(argc, argv, rcl_get_default_allocator());
// std::cout << "ret1: " << ret1 << std::endl;
rcl_node_t node_ptr;
node_ptr = rcl_get_zero_initialized_node();
const char * name = "ros_test_node_victor";
rcl_node_options_t node_options = rcl_node_get_default_options();
rcl_ret_t ret = rcl_node_init(&node_ptr, name, &node_options);
rcl_strings_t node_names;
node_names = rcl_get_zero_initialized_strings();
rcl_ret_t ret3 = rcl_get_node_names(&node_names);
//std::cout << "ret: " << ret3 << std::endl;
for(unsigned int i = 0; i < node_names.count; i++){
std::cout << "->/" << node_names.data[i] << std::endl;
}
rcl_topic_names_and_types_t topic_names_and_types;
topic_names_and_types.topic_count = 0;
topic_names_and_types.topic_names = NULL;
topic_names_and_types.type_names = NULL;
rcl_ret_t result = rcl_get_topic_names_and_types(&node_ptr, &topic_names_and_types);
std::cout << "result: " << result << std::endl;
for(unsigned int i = 0; i < topic_names_and_types.topic_count; i++){
std::cout << topic_names_and_types.topic_names[i] << std::endl;
}
}
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;
}
}
/* Tests the default allocator.
*/
TEST_F(TestAllocatorFixture, test_default_allocator_normal) {
#if defined(WIN32)
printf("Allocator tests disabled on Windows.\n");
return;
#endif // defined(WIN32)
ASSERT_NO_MALLOC(
rcl_allocator_t allocator = rcl_get_default_allocator();
)
rcl_service_options_t
rcl_service_get_default_options()
{
static rcl_service_options_t default_options;
// Must set the allocator and qos after because they are not a compile time constant.
default_options.qos = rmw_qos_profile_services_default;
default_options.allocator = rcl_get_default_allocator();
return default_options;
}
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*
}
}
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 (TestMessagesFixture, RMW_IMPLEMENTATION) : public ::testing::Test
{
public:
rcl_context_t * context_ptr;
rcl_node_t * node_ptr;
void SetUp()
{
rcl_ret_t ret;
{
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
ret = rcl_init_options_init(&init_options, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str;
});
this->context_ptr = new rcl_context_t;
*this->context_ptr = rcl_get_zero_initialized_context();
ret = rcl_init(0, nullptr, &init_options, this->context_ptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
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
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;
}
/* Tests the rcl_init(), rcl_ok(), and rcl_shutdown() functions.
*/
TEST_F(TestRCLFixture, test_rcl_init_and_ok_and_shutdown) {
rcl_ret_t ret;
// A shutdown before any init has been called should fail.
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_NOT_INIT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
// If argc is not 0, but argv is, it should be an invalid argument.
ret = rcl_init(42, nullptr, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
// If either the allocate or deallocate function pointers are not set, it should be invalid arg.
rcl_allocator_t invalid_allocator = rcl_get_default_allocator();
invalid_allocator.allocate = nullptr;
ret = rcl_init(0, nullptr, invalid_allocator);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
invalid_allocator.allocate = rcl_get_default_allocator().allocate;
invalid_allocator.deallocate = nullptr;
ret = rcl_init(0, nullptr, invalid_allocator);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
// If the malloc call fails (with some valid arguments to copy), it should be a bad alloc.
{
FakeTestArgv test_args;
rcl_allocator_t failing_allocator = rcl_get_default_allocator();
failing_allocator.allocate = &failing_malloc;
failing_allocator.deallocate = failing_free;
failing_allocator.reallocate = failing_realloc;
ret = rcl_init(test_args.argc, test_args.argv, failing_allocator);
EXPECT_EQ(RCL_RET_BAD_ALLOC, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
}
// If argc is 0 and argv is nullptr and the allocator is valid, it should succeed.
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
// Then shutdown should work.
ret = rcl_shutdown();
EXPECT_EQ(ret, RCL_RET_OK);
ASSERT_FALSE(rcl_ok());
// Valid argc/argv values and a valid allocator should succeed.
{
FakeTestArgv test_args;
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
}
// Then shutdown should work.
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_FALSE(rcl_ok());
// A repeat call to shutdown should not work.
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_NOT_INIT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
// Repeat, but valid, calls to rcl_init() should fail.
{
FakeTestArgv test_args;
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_ALREADY_INIT, ret);
rcl_reset_error();
ASSERT_TRUE(rcl_ok());
}
// But shutdown should still work.
ret = rcl_shutdown();
EXPECT_EQ(ret, RCL_RET_OK);
ASSERT_FALSE(rcl_ok());
}
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;
}