diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index e570c786e..407bbc0d7 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -186,6 +186,7 @@ function(test_target_function) SRCS rcl/test_publisher.cpp ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ LIBRARIES ${PROJECT_NAME} AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) diff --git a/rcl/test/rcl/test_init.cpp b/rcl/test/rcl/test_init.cpp index e5446e63c..da638f1e8 100644 --- a/rcl/test/rcl/test_init.cpp +++ b/rcl/test/rcl/test_init.cpp @@ -111,6 +111,10 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_shutdown EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); ASSERT_FALSE(rcl_context_is_valid(&context)); + // Already init + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + rcl_reset_error(); // If argc is not 0, but argv is, it should be an invalid argument. ret = rcl_init(42, nullptr, &init_options, &context); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); @@ -122,6 +126,11 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_shutdown EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); ASSERT_FALSE(rcl_context_is_valid(&context)); + // If argc is less than 1, argv is not null, it should be an invalid argument. + ret = rcl_init(0, invalid_args, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); // If either the allocate or deallocate function pointers are not set, it should be invalid arg. init_options.impl->allocator.allocate = nullptr; ret = rcl_init(0, nullptr, &init_options, &context); @@ -271,3 +280,26 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_get_instance_id) ret = rcl_context_fini(&context); EXPECT_EQ(ret, RCL_RET_OK); } + +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_options_access) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t 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; + }); + + rmw_init_options_t * options = rcl_init_options_get_rmw_init_options(&init_options); + ASSERT_NE(nullptr, options); + EXPECT_EQ(0u, options->instance_id); + EXPECT_EQ(nullptr, options->impl); + + const rcl_allocator_t * options_allocator = rcl_init_options_get_allocator(&init_options); + EXPECT_TRUE(rcutils_allocator_is_valid(options_allocator)); + + rcl_init_options_t init_options_dst = rcl_get_zero_initialized_init_options(); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_copy(&init_options, &init_options_dst)); + EXPECT_EQ(RCL_RET_ALREADY_INIT, rcl_init_options_copy(&init_options, &init_options_dst)); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options_dst)); +} diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index 5205817c9..40cc36c12 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/rcl/test/rcl/test_publisher.cpp @@ -25,6 +25,8 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" +#include "./publisher_impl.h" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -202,10 +204,18 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_init_ ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &default_publisher_options); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_TRUE(rcl_publisher_is_valid(&publisher)); + // Try init a publisher already init + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; ret = rcl_publisher_fini(&publisher, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); + // Pass invalid node to fini + ret = rcl_publisher_fini(&publisher, nullptr); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Try passing null for publisher in init. ret = rcl_publisher_init(nullptr, this->node_ptr, ts, topic_name, &default_publisher_options); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; @@ -290,3 +300,193 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_init_ EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; rcl_reset_error(); } + +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_loan) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + const char * topic_name = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = + rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + test_msgs__msg__Strings * msg_loaned = nullptr; + test_msgs__msg__Strings ** msg_loaned_ptr = &msg_loaned; + if (rcl_publisher_can_loan_messages(&publisher)) { + EXPECT_EQ( + RCL_RET_OK, rcl_borrow_loaned_message( + &publisher, + ts, + reinterpret_cast(msg_loaned_ptr))); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&(msg_loaned->string_value), "testing")); + EXPECT_EQ( + RCL_RET_OK, rcl_publish_loaned_message( + &publisher, + msg_loaned, + nullptr)); + } +} + +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_invalid_publisher) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + const char * topic_name = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = + rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + const rcl_publisher_options_t * publisher_options_rcv = rcl_publisher_get_options(&publisher); + ASSERT_NE(nullptr, publisher_options_rcv); + EXPECT_EQ(rmw_qos_profile_default.reliability, publisher_options_rcv->qos.reliability); + EXPECT_EQ(rmw_qos_profile_default.history, publisher_options_rcv->qos.history); + EXPECT_EQ(rmw_qos_profile_default.depth, publisher_options_rcv->qos.depth); + EXPECT_EQ(rmw_qos_profile_default.durability, publisher_options_rcv->qos.durability); + EXPECT_TRUE(rcutils_allocator_is_valid(&(publisher_options_rcv->allocator))); + + rmw_publisher_t * pub_rmw_handle = rcl_publisher_get_rmw_handle(&publisher); + EXPECT_NE(nullptr, pub_rmw_handle); + + rcl_context_t * pub_context = rcl_publisher_get_context(&publisher); + EXPECT_TRUE(rcl_context_is_valid(pub_context)); + EXPECT_EQ(rcl_context_get_instance_id(context_ptr), rcl_context_get_instance_id(pub_context)); + + EXPECT_EQ(RCL_RET_OK, rcl_publisher_assert_liveliness(&publisher)); + + size_t count_size; + test_msgs__msg__BasicTypes msg; + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + rcl_publisher_impl_t * saved_impl = publisher.impl; + rcl_context_t * saved_context = publisher.impl->context; + rmw_publisher_t * saved_rmw_handle = publisher.impl->rmw_handle; + + // Change internal context to nullptr + publisher.impl->context = nullptr; + EXPECT_TRUE(rcl_publisher_is_valid_except_context(&publisher)); + EXPECT_NE(nullptr, rcl_publisher_get_topic_name(&publisher)); + EXPECT_NE(nullptr, rcl_publisher_get_rmw_handle(&publisher)); + EXPECT_NE(nullptr, rcl_publisher_get_actual_qos(&publisher)); + EXPECT_NE(nullptr, rcl_publisher_get_options(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_is_valid(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_publisher_get_subscription_count(&publisher, &count_size)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_assert_liveliness(&publisher)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr)); + rcl_reset_error(); + publisher.impl->context = saved_context; + + // Change internal rmw_handle to nullptr + publisher.impl->rmw_handle = nullptr; + EXPECT_FALSE(rcl_publisher_is_valid_except_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_is_valid(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_topic_name(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_rmw_handle(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_actual_qos(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_options(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_publisher_get_subscription_count(&publisher, &count_size)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_assert_liveliness(&publisher)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr)); + rcl_reset_error(); + publisher.impl->rmw_handle = saved_rmw_handle; + + // Change internal implementation to nullptr + publisher.impl = nullptr; + EXPECT_FALSE(rcl_publisher_is_valid_except_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_is_valid(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_topic_name(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_rmw_handle(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_actual_qos(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_options(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_publisher_get_subscription_count(&publisher, &count_size)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_assert_liveliness(&publisher)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr)); + rcl_reset_error(); + publisher.impl = saved_impl; + + // Null tests + EXPECT_FALSE(rcl_publisher_is_valid_except_context(nullptr)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_is_valid(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_topic_name(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_rmw_handle(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_actual_qos(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_options(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_context(nullptr)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_can_loan_messages(nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_publisher_get_subscription_count(nullptr, &count_size)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_assert_liveliness(nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(nullptr, &msg, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_serialized_message(nullptr, &serialized_msg, nullptr)); + rcl_reset_error(); +}