diff --git a/rcl/src/rcl/context.c b/rcl/src/rcl/context.c index 0f6df4da3..b400c09ef 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -95,11 +95,6 @@ rcl_context_get_rmw_context(rcl_context_t * context) void __cleanup_context(rcl_context_t * context) { - // if null, nothing can be done - if (NULL == context) { - return; - } - // reset the instance id to 0 to indicate "invalid" (should already be 0, but this is defensive) rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index 3b9174356..21ed84812 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -171,11 +171,8 @@ rcl_ros_clock_fini( return RCL_RET_ERROR; } rcl_clock_generic_fini(clock); - if (!clock->data) { - RCL_SET_ERROR_MSG("clock data invalid"); - return RCL_RET_ERROR; - } - clock->allocator.deallocate((rcl_ros_clock_storage_t *)clock->data, clock->allocator.state); + clock->allocator.deallocate(clock->data, clock->allocator.state); + clock->data = NULL; return RCL_RET_OK; } @@ -293,10 +290,8 @@ rcl_enable_ros_time_override(rcl_clock_t * clock) return RCL_RET_ERROR; } rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; - if (!storage) { - RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot enable override."); - return RCL_RET_ERROR; - } + RCL_CHECK_FOR_NULL_WITH_MSG( + storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR); if (!storage->active) { rcl_time_jump_t time_jump; time_jump.delta.nanoseconds = 0; @@ -316,12 +311,9 @@ rcl_disable_ros_time_override(rcl_clock_t * clock) RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot disable override."); return RCL_RET_ERROR; } - rcl_ros_clock_storage_t * storage = \ - (rcl_ros_clock_storage_t *)clock->data; - if (!storage) { - RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot disable override."); - return RCL_RET_ERROR; - } + rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; + RCL_CHECK_FOR_NULL_WITH_MSG( + storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR); if (storage->active) { rcl_time_jump_t time_jump; time_jump.delta.nanoseconds = 0; @@ -344,12 +336,9 @@ rcl_is_enabled_ros_time_override( RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot query override state."); return RCL_RET_ERROR; } - rcl_ros_clock_storage_t * storage = \ - (rcl_ros_clock_storage_t *)clock->data; - if (!storage) { - RCL_SET_ERROR_MSG("Clock storage is not initialized, cannot query override state."); - return RCL_RET_ERROR; - } + rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; + RCL_CHECK_FOR_NULL_WITH_MSG( + storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR); *is_enabled = storage->active; return RCL_RET_OK; } @@ -364,8 +353,10 @@ rcl_set_ros_time_override( RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot set time override."); return RCL_RET_ERROR; } - rcl_time_jump_t time_jump; rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; + RCL_CHECK_FOR_NULL_WITH_MSG( + storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR); + rcl_time_jump_t time_jump; if (storage->active) { time_jump.clock_change = RCL_ROS_TIME_NO_CHANGE; rcl_time_point_value_t current_time; @@ -453,12 +444,12 @@ rcl_clock_remove_jump_callback( } // Shrink size of the callback array - if (clock->num_jump_callbacks == 1) { + if (--(clock->num_jump_callbacks) == 0) { clock->allocator.deallocate(clock->jump_callbacks, clock->allocator.state); clock->jump_callbacks = NULL; } else { rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate( - clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * (clock->num_jump_callbacks - 1), + clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * clock->num_jump_callbacks, clock->allocator.state); if (NULL == callbacks) { RCL_SET_ERROR_MSG("Failed to shrink jump callbacks"); @@ -466,6 +457,5 @@ rcl_clock_remove_jump_callback( } clock->jump_callbacks = callbacks; } - --(clock->num_jump_callbacks); return RCL_RET_OK; } diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 1c1a92ec6..73e4eda70 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -50,6 +50,23 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} ) + # TODO(hidmic): re-enable timer tests against RTI Connext once + # https://github.com/ros2/rcl/issues/687 is resolved + set(AMENT_GTEST_ARGS "") + if(rmw_implementation STREQUAL "rmw_connext_cpp") + message(STATUS "Skipping test_timer${target_suffix} test.") + set(AMENT_GTEST_ARGS "SKIP_TEST") + endif() + + rcl_add_custom_gtest(test_timer${target_suffix} + SRCS rcl/test_timer.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools + AMENT_DEPENDENCIES ${rmw_implementation} + ${AMENT_GTEST_ARGS} + ) + rcl_add_custom_gtest(test_context${target_suffix} SRCS rcl/test_context.cpp ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} @@ -350,13 +367,6 @@ rcl_add_custom_gtest(test_expand_topic_name LIBRARIES ${PROJECT_NAME} ) -rcl_add_custom_gtest(test_timer${target_suffix} - SRCS rcl/test_timer.cpp - APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} - AMENT_DEPENDENCIES "osrf_testing_tools_cpp" -) - rcl_add_custom_gtest(test_security SRCS rcl/test_security.cpp APPEND_LIBRARY_DIRS ${extra_lib_dirs} diff --git a/rcl/test/rcl/test_context.cpp b/rcl/test/rcl/test_context.cpp index 56a8e0842..9ef189fed 100644 --- a/rcl/test/rcl/test_context.cpp +++ b/rcl/test/rcl/test_context.cpp @@ -124,3 +124,8 @@ TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), nominal) { ret = rcl_init_options_fini(&init_options); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } + +TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), bad_fini) { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_context_fini(nullptr)); + rcl_reset_error(); +} diff --git a/rcl/test/rcl/test_expand_topic_name.cpp b/rcl/test/rcl/test_expand_topic_name.cpp index 3f51b8c2d..fedd54041 100644 --- a/rcl/test/rcl/test_expand_topic_name.cpp +++ b/rcl/test/rcl/test_expand_topic_name.cpp @@ -23,6 +23,8 @@ #include "rcl/error_handling.h" +#include "./allocator_testing_utils.h" + using namespace std::string_literals; TEST(test_expand_topic_name, normal) { @@ -121,6 +123,16 @@ TEST(test_expand_topic_name, invalid_arguments) { rcl_reset_error(); } + // pass failing allocator + { + rcl_allocator_t bad_allocator = get_failing_allocator(); + EXPECT_EQ( + RCL_RET_BAD_ALLOC, + rcl_expand_topic_name("/absolute", node, ns, &subs, bad_allocator, &expanded_topic)); + EXPECT_STREQ(NULL, expanded_topic); + rcl_reset_error(); + } + ret = rcutils_string_map_fini(&subs); ASSERT_EQ(RCL_RET_OK, ret); } diff --git a/rcl/test/rcl/test_init.cpp b/rcl/test/rcl/test_init.cpp index da638f1e8..da96012dc 100644 --- a/rcl/test/rcl/test_init.cpp +++ b/rcl/test/rcl/test_init.cpp @@ -14,15 +14,17 @@ #include -#include "rcl/rcl.h" - -#include "./failing_allocator_functions.hpp" #include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" #include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/arguments.h" #include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/security.h" +#include "rcutils/env.h" #include "rcutils/format_string.h" #include "rcutils/snprintf.h" +#include "./allocator_testing_utils.h" #include "../src/rcl/init_options_impl.h" #ifdef RMW_IMPLEMENTATION @@ -98,65 +100,148 @@ struct FakeTestArgv FakeTestArgv(const FakeTestArgv &) = delete; }; -/* Tests the rcl_init() and rcl_shutdown() functions. +/* Tests rcl_init_options_init() and rcl_init_options_fini() functions. */ -TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_shutdown) { - rcl_ret_t ret; +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_options_init) { rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); - ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + 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; - rcl_context_t context = rcl_get_zero_initialized_context(); - // A shutdown before any init has been called should fail. - ret = rcl_shutdown(&context); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); - rcl_reset_error(); - ASSERT_FALSE(rcl_context_is_valid(&context)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); // 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); - rcl_reset_error(); - ASSERT_FALSE(rcl_context_is_valid(&context)); - // If argc is not 0, argv is not null but contains one, it should be an invalid argument. - const char * invalid_args[] = {"some-arg", nullptr}; - ret = rcl_init(2, invalid_args, &init_options, &context); - 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); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); - rcl_reset_error(); - ASSERT_FALSE(rcl_context_is_valid(&context)); - init_options.impl->allocator.allocate = rcl_get_default_allocator().allocate; - init_options.impl->allocator.deallocate = nullptr; - ret = rcl_init(0, nullptr, &init_options, &context); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); - rcl_reset_error(); - ASSERT_FALSE(rcl_context_is_valid(&context)); - // If the malloc call fails (with some valid arguments to copy), it should be a bad alloc. +} + +/* Tests calling rcl_init() with invalid arguments fails. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_invalid_arguments) { + 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; + }); + + { + // If argc is not 0, but argv is, it should be an invalid argument. + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(42, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If argc is not 0, argv is not null but contains one, it should be an invalid argument. + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * null_args[] = {"some-arg", nullptr}; + ret = rcl_init(2, null_args, &init_options, &context); + 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. + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * some_args[] = {"some-arg"}; + ret = rcl_init(0, some_args, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If an invalid ROS arg is given, init should fail. + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * bad_remap_args[] = { + "some-arg", RCL_ROS_ARGS_FLAG, RCL_REMAP_FLAG, "name:="}; + const size_t argc = sizeof(bad_remap_args) / sizeof(const char *); + ret = rcl_init(argc, bad_remap_args, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ROS_ARGS, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If an invalid enclave is given, init should fail. + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * bad_enclave_args[] = { + "some-arg", RCL_ROS_ARGS_FLAG, RCL_ENCLAVE_FLAG, "1foo"}; + const size_t argc = sizeof(bad_enclave_args) / sizeof(const char *); + ret = rcl_init(argc, bad_enclave_args, &init_options, &context); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } { + // If security keystore is invalid, init should fail. + ASSERT_TRUE(rcutils_set_env(ROS_SECURITY_ENABLE_VAR_NAME, "true")); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_TRUE(rcutils_set_env(ROS_SECURITY_ENABLE_VAR_NAME, "")); + }); + ASSERT_TRUE(rcutils_set_env(ROS_SECURITY_STRATEGY_VAR_NAME, "Enforce")); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_TRUE(rcutils_set_env(ROS_SECURITY_STRATEGY_VAR_NAME, "")); + }); + ASSERT_TRUE(rcutils_set_env(ROS_SECURITY_KEYSTORE_VAR_NAME, "/not/a/real/secure/root")); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_TRUE(rcutils_set_env(ROS_SECURITY_KEYSTORE_VAR_NAME, "")); + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_ERROR, 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. + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_allocator_t allocator = init_options.impl->allocator; + init_options.impl->allocator = + (rcl_allocator_t)rcutils_get_zero_initialized_allocator(); + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + init_options.impl->allocator = allocator; + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // 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.reallocate = failing_realloc; - failing_allocator.zero_allocate = failing_calloc; - init_options.impl->allocator = failing_allocator; + rcl_allocator_t allocator = init_options.impl->allocator; + init_options.impl->allocator = get_failing_allocator(); + rcl_context_t context = rcl_get_zero_initialized_context(); ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); rcl_reset_error(); + init_options.impl->allocator = allocator; ASSERT_FALSE(rcl_context_is_valid(&context)); } - init_options.impl->allocator = rcl_get_default_allocator(); +} + +/* Tests the rcl_init() and rcl_shutdown() functions. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_shutdown) { + 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; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + // A shutdown before an init should fail. + ret = rcl_shutdown(&context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); // If argc is 0 and argv is nullptr and the allocator is valid, it should succeed. ret = rcl_init(0, nullptr, &init_options, &context); EXPECT_EQ(RCL_RET_OK, ret); @@ -210,10 +295,6 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_shutdown ret = rcl_context_fini(&context); EXPECT_EQ(ret, RCL_RET_OK); context = rcl_get_zero_initialized_context(); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; - }); } /* Tests the rcl_get_instance_id() function. diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 910775a1c..2dcbd4490 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -29,6 +29,8 @@ #include "rcl/error_handling.h" #include "wait_for_entity_helpers.hpp" +#include "./allocator_testing_utils.h" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -78,6 +80,38 @@ class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing } }; +class CLASSNAME (TestSubscriptionFixtureInit, RMW_IMPLEMENTATION) + : public CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION) +{ +public: + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic = "/chatter"; + rcl_subscription_options_t subscription_options; + rcl_subscription_t subscription; + rcl_subscription_t subscription_zero_init; + rcl_ret_t ret; + rcutils_allocator_t allocator; + + void SetUp() override + { + CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION) ::SetUp(); + allocator = rcutils_get_default_allocator(); + subscription_options = rcl_subscription_get_default_options(); + subscription = rcl_get_zero_initialized_subscription(); + subscription_zero_init = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION) ::TearDown(); + } +}; + /* Test subscription init, fini and is_valid functions */ TEST_F( @@ -109,6 +143,56 @@ TEST_F( rcl_reset_error(); } +// Bad arguments for init and fini +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_bad_init) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic = "/chatter"; + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + + ASSERT_FALSE(rcl_node_is_valid_except_context(&invalid_node)); + EXPECT_EQ(nullptr, rcl_node_get_rmw_handle(&invalid_node)); + + EXPECT_EQ( + RCL_RET_NODE_INVALID, + rcl_subscription_init(&subscription, nullptr, ts, topic, &subscription_options)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_NODE_INVALID, + rcl_subscription_init(&subscription, &invalid_node, ts, topic, &subscription_options)); + rcl_reset_error(); + + rcl_ret_t ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, "spaced name", &subscription_options); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, "sub{ros_not_match}", &subscription_options); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_subscription_options_t bad_subscription_options = rcl_subscription_get_default_options(); + bad_subscription_options.allocator = get_failing_allocator(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &bad_subscription_options); + + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(rcl_subscription_is_valid(&subscription)); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_subscription_fini(&subscription, nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_subscription_fini(&subscription, &invalid_node)); + rcl_reset_error(); + + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + /* Basic nominal test of a subscription */ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_nominal) { @@ -528,3 +612,140 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) ASSERT_EQ(NULL, rcl_subscription_get_options(nullptr)); } + +/* bad take() +*/ +TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscription_bad_take) { + test_msgs__msg__BasicTypes msg; + rmw_message_info_t message_info = rmw_get_zero_initialized_message_info(); + ASSERT_TRUE(test_msgs__msg__BasicTypes__init(&msg)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__BasicTypes__fini(&msg); + }); + EXPECT_EQ(RCL_RET_SUBSCRIPTION_INVALID, rcl_take(nullptr, &msg, &message_info, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, rcl_take(&subscription_zero_init, &msg, &message_info, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_TAKE_FAILED, rcl_take(&subscription, &msg, &message_info, nullptr)); + rcl_reset_error(); +} + +/* bad take_serialized +*/ +TEST_F( + CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), + test_subscription_bad_take_serialized) { + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + size_t initial_capacity_ser = 0u; + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_take_serialized_message(nullptr, &serialized_msg, nullptr, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_take_serialized_message(&subscription_zero_init, &serialized_msg, nullptr, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_TAKE_FAILED, + rcl_take_serialized_message(&subscription, &serialized_msg, nullptr, nullptr)); + rcl_reset_error(); +} + +/* Bad arguments take_sequence + */ +TEST_F( + CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscription_bad_take_sequence) +{ + size_t seq_size = 3u; + rmw_message_sequence_t messages; + ASSERT_EQ(RMW_RET_OK, rmw_message_sequence_init(&messages, seq_size, &allocator)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RMW_RET_OK, rmw_message_sequence_fini(&messages)); + }); + rmw_message_info_sequence_t message_infos_short; + ASSERT_EQ( + RMW_RET_OK, rmw_message_info_sequence_init(&message_infos_short, seq_size - 1u, &allocator)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RMW_RET_OK, rmw_message_info_sequence_fini(&message_infos_short)); + }); + rmw_message_info_sequence_t message_infos; + ASSERT_EQ( + RMW_RET_OK, rmw_message_info_sequence_init(&message_infos, seq_size, &allocator)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RMW_RET_OK, rmw_message_info_sequence_fini(&message_infos)); + }); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_take_sequence(nullptr, seq_size, &messages, &message_infos, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_take_sequence(&subscription_zero_init, seq_size, &messages, &message_infos, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_sequence(&subscription, seq_size + 1, &messages, &message_infos, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos_short, nullptr)); + rcl_reset_error(); + + // This test fails for rmw_cyclonedds_cpp function rmw_take_sequence + // Tracked here: https://github.com/ros2/rmw_cyclonedds/issues/193 + /* + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_TAKE_FAILED, + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos, nullptr)); + rcl_reset_error(); + */ +} + +/* Using bad arguments subscription methods + */ +TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscription_bad_argument) { + size_t pub_count = 0; + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, rcl_subscription_get_publisher_count(nullptr, &pub_count)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(nullptr)); + rcl_reset_error(); + EXPECT_FALSE(rcl_subscription_can_loan_messages(nullptr)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_rmw_handle(nullptr)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_topic_name(nullptr)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_options(nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_publisher_count(&subscription_zero_init, &pub_count)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_FALSE(rcl_subscription_can_loan_messages(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_rmw_handle(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_topic_name(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_options(&subscription_zero_init)); + rcl_reset_error(); +} diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index f267f0871..ab944c193 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -24,6 +24,8 @@ #include "rcl/error_handling.h" #include "rcl/time.h" +#include "./allocator_testing_utils.h" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -317,23 +319,6 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) { } TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) { - rcl_allocator_t allocator = rcl_get_default_allocator(); - auto * ros_clock = - static_cast(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - allocator.deallocate(ros_clock, allocator.state); - }); - rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator); - ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(ros_clock)) << rcl_get_error_string().str; - }); - EXPECT_TRUE(ros_clock != nullptr); - EXPECT_TRUE(ros_clock->data != nullptr); - EXPECT_EQ(ros_clock->type, RCL_ROS_TIME); - rcl_ret_t ret; rcl_time_point_t a, b; @@ -354,23 +339,10 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) { b.clock_type = RCL_SYSTEM_TIME; EXPECT_EQ(rcl_difference_times(&a, &b, &d), RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); } TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference_signed) { - rcl_allocator_t allocator = rcl_get_default_allocator(); - auto * ros_clock = - static_cast(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - allocator.deallocate(ros_clock, allocator.state); - }); - rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator); - ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(ros_clock)) << rcl_get_error_string().str; - }); - rcl_time_point_t a, b; a.nanoseconds = RCL_S_TO_NS(0LL) + 0LL; b.nanoseconds = RCL_S_TO_NS(10LL) + 0LL; @@ -441,20 +413,14 @@ void reset_callback_triggers(void) TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_clock_change_callbacks) { rcl_allocator_t allocator = rcl_get_default_allocator(); - auto * ros_clock = - static_cast(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - allocator.deallocate(ros_clock, allocator.state); - }); - rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator); - ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; + rcl_clock_t ros_clock; + rcl_ret_t ret = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock)); + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)); }); rcl_time_point_value_t query_now; - rcl_ret_t ret; // set callbacks rcl_time_jump_t time_jump; @@ -464,18 +430,18 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_clock_change_callbacks) { threshold.min_backward.nanoseconds = 0; ASSERT_EQ( RCL_RET_OK, - rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) << + rcl_clock_add_jump_callback(&ros_clock, threshold, clock_callback, &time_jump)) << rcl_get_error_string().str; reset_callback_triggers(); // Query time, no changes expected. - ret = rcl_clock_get_now(ros_clock, &query_now); + ret = rcl_clock_get_now(&ros_clock, &query_now); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); // Clock change callback called when ROS time is enabled - ret = rcl_enable_ros_time_override(ros_clock); + ret = rcl_enable_ros_time_override(&ros_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_TRUE(pre_callback_called); EXPECT_TRUE(post_callback_called); @@ -483,14 +449,14 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_clock_change_callbacks) { reset_callback_triggers(); // Clock change callback not called because ROS time is already enabled. - ret = rcl_enable_ros_time_override(ros_clock); + ret = rcl_enable_ros_time_override(&ros_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); reset_callback_triggers(); // Clock change callback called when ROS time is disabled - ret = rcl_disable_ros_time_override(ros_clock); + ret = rcl_disable_ros_time_override(&ros_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_TRUE(pre_callback_called); EXPECT_TRUE(post_callback_called); @@ -498,7 +464,7 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_clock_change_callbacks) { reset_callback_triggers(); // Clock change callback not called because ROS time is already disabled. - ret = rcl_disable_ros_time_override(ros_clock); + ret = rcl_disable_ros_time_override(&ros_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); @@ -533,21 +499,15 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_fail_set_jump_callbacks) } TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_forward_jump_callbacks) { + rcl_clock_t ros_clock; rcl_allocator_t allocator = rcl_get_default_allocator(); - auto * ros_clock = - static_cast(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - allocator.deallocate(ros_clock, allocator.state); - }); - rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator); - ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; + rcl_ret_t ret = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock)); + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)); }); - rcl_ret_t ret; rcl_time_point_value_t set_point1 = 1000L * 1000L * 1000L; rcl_time_point_value_t set_point2 = 2L * 1000L * 1000L * 1000L; @@ -558,24 +518,24 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_forward_jump_callbacks) { threshold.min_backward.nanoseconds = 0; ASSERT_EQ( RCL_RET_OK, - rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) << + rcl_clock_add_jump_callback(&ros_clock, threshold, clock_callback, &time_jump)) << rcl_get_error_string().str; reset_callback_triggers(); // Set the time before it's enabled. Should be no callbacks - ret = rcl_set_ros_time_override(ros_clock, set_point1); + ret = rcl_set_ros_time_override(&ros_clock, set_point1); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); // enable no callbacks - ret = rcl_enable_ros_time_override(ros_clock); + ret = rcl_enable_ros_time_override(&ros_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); // Set the time now that it's enabled, now get callbacks - ret = rcl_set_ros_time_override(ros_clock, set_point2); + ret = rcl_set_ros_time_override(&ros_clock, set_point2); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_TRUE(pre_callback_called); EXPECT_TRUE(post_callback_called); @@ -584,33 +544,27 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_forward_jump_callbacks) { reset_callback_triggers(); // Setting same value as previous time, not a jump - ret = rcl_set_ros_time_override(ros_clock, set_point2); + ret = rcl_set_ros_time_override(&ros_clock, set_point2); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); // disable no callbacks - ret = rcl_disable_ros_time_override(ros_clock); + ret = rcl_disable_ros_time_override(&ros_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); } TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_backward_jump_callbacks) { + rcl_clock_t ros_clock; rcl_allocator_t allocator = rcl_get_default_allocator(); - auto * ros_clock = - static_cast(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - allocator.deallocate(ros_clock, allocator.state); - }); - rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator); - ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; + rcl_ret_t ret = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock)); + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)); }); - rcl_ret_t ret; rcl_time_point_value_t set_point1 = 1000000000; rcl_time_point_value_t set_point2 = 2000000000; @@ -621,24 +575,24 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_backward_jump_callbacks) threshold.min_backward.nanoseconds = -1; ASSERT_EQ( RCL_RET_OK, - rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) << + rcl_clock_add_jump_callback(&ros_clock, threshold, clock_callback, &time_jump)) << rcl_get_error_string().str; reset_callback_triggers(); // Set the time before it's enabled. Should be no callbacks - ret = rcl_set_ros_time_override(ros_clock, set_point2); + ret = rcl_set_ros_time_override(&ros_clock, set_point2); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); // enable no callbacks - ret = rcl_enable_ros_time_override(ros_clock); + ret = rcl_enable_ros_time_override(&ros_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); // Set the time now that it's enabled, now get callbacks - ret = rcl_set_ros_time_override(ros_clock, set_point1); + ret = rcl_set_ros_time_override(&ros_clock, set_point1); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_TRUE(pre_callback_called); EXPECT_TRUE(post_callback_called); @@ -647,31 +601,26 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_backward_jump_callbacks) reset_callback_triggers(); // Setting same value as previous time, not a jump - ret = rcl_set_ros_time_override(ros_clock, set_point1); + ret = rcl_set_ros_time_override(&ros_clock, set_point1); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); // disable no callbacks - ret = rcl_disable_ros_time_override(ros_clock); + ret = rcl_disable_ros_time_override(&ros_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(pre_callback_called); EXPECT_FALSE(post_callback_called); } TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_add_jump_callback) { + rcl_clock_t clock; rcl_allocator_t allocator = rcl_get_default_allocator(); - auto * clock = - static_cast(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - allocator.deallocate(clock, allocator.state); - }); - rcl_ret_t retval = rcl_ros_clock_init(clock, &allocator); - ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string().str; + rcl_ret_t ret = rcl_ros_clock_init(&clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(clock)); + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&clock)); }); rcl_jump_threshold_t threshold; @@ -681,35 +630,31 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_add_jump_callback) { rcl_jump_callback_t cb = reinterpret_cast(0xBEEF); void * user_data = reinterpret_cast(0xCAFE); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_clock_add_jump_callback(clock, threshold, NULL, NULL)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_clock_add_jump_callback(&clock, threshold, NULL, user_data)); rcl_reset_error(); - EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, NULL)) << + EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, NULL)) << rcl_get_error_string().str; - EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(clock, threshold, cb, NULL)); + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(&clock, threshold, cb, NULL)); rcl_reset_error(); - EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data)) << + EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data)) << rcl_get_error_string().str; - EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(clock, threshold, cb, user_data)); + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data)); rcl_reset_error(); - EXPECT_EQ(2u, clock->num_jump_callbacks); + EXPECT_EQ(2u, clock.num_jump_callbacks); } TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_remove_jump_callback) { + rcl_clock_t clock; rcl_allocator_t allocator = rcl_get_default_allocator(); - auto * clock = - static_cast(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - allocator.deallocate(clock, allocator.state); - }); - rcl_ret_t retval = rcl_ros_clock_init(clock, &allocator); - ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string().str; + rcl_ret_t ret = rcl_ros_clock_init(&clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(clock)); + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&clock)); }); rcl_jump_threshold_t threshold; @@ -722,44 +667,41 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_remove_jump_callback) { void * user_data3 = reinterpret_cast(0xBEAD); void * user_data4 = reinterpret_cast(0xDEED); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_clock_remove_jump_callback(clock, NULL, NULL)); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_clock_remove_jump_callback(&clock, NULL, user_data1)); rcl_reset_error(); - EXPECT_EQ(RCL_RET_ERROR, rcl_clock_remove_jump_callback(clock, cb, NULL)); + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_remove_jump_callback(&clock, cb, NULL)); rcl_reset_error(); - ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data1)) << + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data1)) << rcl_get_error_string().str; - ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data2)) << + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data2)) << rcl_get_error_string().str; - ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data3)) << + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data3)) << rcl_get_error_string().str; - ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data4)) << + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data4)) << rcl_get_error_string().str; - EXPECT_EQ(4u, clock->num_jump_callbacks); - - EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data3)); - EXPECT_EQ(3u, clock->num_jump_callbacks); - EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data4)); - EXPECT_EQ(2u, clock->num_jump_callbacks); - EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data1)); - EXPECT_EQ(1u, clock->num_jump_callbacks); - EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data2)); - EXPECT_EQ(0u, clock->num_jump_callbacks); + EXPECT_EQ(4u, clock.num_jump_callbacks); + + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data3)); + EXPECT_EQ(3u, clock.num_jump_callbacks); + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data4)); + EXPECT_EQ(2u, clock.num_jump_callbacks); + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data1)); + EXPECT_EQ(1u, clock.num_jump_callbacks); + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data2)); + EXPECT_EQ(0u, clock.num_jump_callbacks); } TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), add_remove_add_jump_callback) { - rcl_allocator_t allocator = rcl_get_default_allocator(); - auto * clock = - static_cast(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - allocator.deallocate(clock, allocator.state); - }); - rcl_ret_t retval = rcl_ros_clock_init(clock, &allocator); - ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string().str; + rcl_allocator_t failing_allocator = get_failing_allocator(); + set_failing_allocator_is_failing(failing_allocator, false); + + rcl_clock_t clock; + rcl_ret_t ret = rcl_clock_init(RCL_SYSTEM_TIME, &clock, &failing_allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(clock)); + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)); }); rcl_jump_threshold_t threshold; @@ -767,16 +709,38 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), add_remove_add_jump_callback) { threshold.min_forward.nanoseconds = 0; threshold.min_backward.nanoseconds = 0; rcl_jump_callback_t cb = reinterpret_cast(0xBEEF); - void * user_data = reinterpret_cast(0xCAFE); + void * user_data1 = reinterpret_cast(0xCAFE); + void * user_data2 = reinterpret_cast(0xFACE); + void * user_data3 = reinterpret_cast(0xDEED); - ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data)) << + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data1)) << rcl_get_error_string().str; - EXPECT_EQ(1u, clock->num_jump_callbacks); - EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data)); - EXPECT_EQ(0u, clock->num_jump_callbacks); - EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data)) << + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data2)) << rcl_get_error_string().str; - EXPECT_EQ(1u, clock->num_jump_callbacks); + EXPECT_EQ(2u, clock.num_jump_callbacks); + + set_failing_allocator_is_failing(failing_allocator, true); + + // Fail to add callback + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data3)); + rcl_reset_error(); + + // Remove callback but fail to shrink storage + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_clock_remove_jump_callback(&clock, cb, user_data1)); + rcl_reset_error(); + + set_failing_allocator_is_failing(failing_allocator, false); + + // Callback has already been removed + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_remove_jump_callback(&clock, cb, user_data1)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data2)); + EXPECT_EQ(0u, clock.num_jump_callbacks); + + EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data1)) << + rcl_get_error_string().str; + EXPECT_EQ(1u, clock.num_jump_callbacks); } TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), failed_get_now) { @@ -788,19 +752,38 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), failed_get_now) { EXPECT_EQ(uninitialized_clock.type, RCL_CLOCK_UNINITIALIZED); uninitialized_clock.get_now = NULL; EXPECT_EQ(RCL_RET_ERROR, rcl_clock_get_now(&uninitialized_clock, &query_now)); + rcl_reset_error(); } -TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), fail_override) { - rcl_clock_t ros_clock; - rcl_allocator_t allocator = rcl_get_default_allocator(); +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), fail_ros_time_override) { bool result; rcl_time_point_value_t set_point = 1000000000ull; - ASSERT_EQ( - RCL_RET_OK, rcl_clock_init( - RCL_CLOCK_UNINITIALIZED, &ros_clock, &allocator)) << rcl_get_error_string().str; + + rcl_clock_t ros_clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_CLOCK_UNINITIALIZED, &ros_clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(RCL_RET_ERROR, rcl_enable_ros_time_override(&ros_clock)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_disable_ros_time_override(&ros_clock)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_is_enabled_ros_time_override(&ros_clock, &result)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_set_ros_time_override(&ros_clock, set_point)); + rcl_reset_error(); + + ret = rcl_clock_init(RCL_ROS_TIME, &ros_clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_clock_fini(&ros_clock); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_EQ(RCL_RET_ERROR, rcl_enable_ros_time_override(&ros_clock)); + rcl_reset_error(); EXPECT_EQ(RCL_RET_ERROR, rcl_disable_ros_time_override(&ros_clock)); + rcl_reset_error(); EXPECT_EQ(RCL_RET_ERROR, rcl_is_enabled_ros_time_override(&ros_clock, &result)); + rcl_reset_error(); EXPECT_EQ(RCL_RET_ERROR, rcl_set_ros_time_override(&ros_clock, set_point)); + rcl_reset_error(); } diff --git a/rcl/test/rcl/test_timer.cpp b/rcl/test/rcl/test_timer.cpp index 4801e24da..ae3984bed 100644 --- a/rcl/test/rcl/test_timer.cpp +++ b/rcl/test/rcl/test_timer.cpp @@ -114,6 +114,102 @@ class TestPreInitTimer : public TestTimerFixture } }; +TEST_F(TestTimerFixture, test_timer_init_with_invalid_arguments) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + + ret = rcl_timer_init( + nullptr, &clock, this->context_ptr, RCL_MS_TO_NS(50), nullptr, allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_timer_init( + &timer, nullptr, this->context_ptr, RCL_MS_TO_NS(50), nullptr, allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_timer_init( + &timer, &clock, nullptr, RCL_MS_TO_NS(50), nullptr, allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, -1, nullptr, allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + rcl_allocator_t invalid_allocator = rcutils_get_zero_initialized_allocator(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(50), nullptr, invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); +} + +TEST_F(TestTimerFixture, test_timer_with_invalid_clock) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_CLOCK_UNINITIALIZED, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 0, nullptr, allocator); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 0, nullptr, allocator); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_clock_t * timer_clock; + ret = rcl_timer_clock(&timer, &timer_clock); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + timer_clock->get_now = nullptr; + + // Trigger clock jump callbacks + ret = rcl_enable_ros_time_override(timer_clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_timer_call(&timer); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + int64_t time_until_next_call; + ret = rcl_timer_get_time_until_next_call(&timer, &time_until_next_call); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + bool ready; + ret = rcl_timer_is_ready(&timer, &ready); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + rcl_time_point_value_t time_since_last_call; + ret = rcl_timer_get_time_since_last_call(&timer, &time_since_last_call); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + ret = rcl_timer_reset(&timer); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); +} + TEST_F(TestTimerFixture, test_two_timers) { rcl_ret_t ret; @@ -182,8 +278,9 @@ TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) { rcl_timer_t timer = rcl_get_zero_initialized_timer(); rcl_timer_t timer2 = rcl_get_zero_initialized_timer(); + // Keep the first timer period low enough so that rcl_wait() doesn't timeout too early. ret = rcl_timer_init( - &timer, &clock, this->context_ptr, RCL_MS_TO_NS(50), nullptr, rcl_get_default_allocator()); + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator()); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_timer_init( @@ -274,6 +371,95 @@ TEST_F(TestTimerFixture, test_timer_not_ready) { EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } +TEST_F(TestTimerFixture, test_timer_overrun) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(200), nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Force multiple timer timeouts. + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(500)); + EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + bool is_ready = false; + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(is_ready); + + EXPECT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Ensure period is re-aligned. + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(10)); + EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(is_ready); +} + +TEST_F(TestTimerFixture, test_timer_with_zero_period) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + bool is_ready = false; + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(is_ready) << rcl_get_error_string().str; + + int64_t time_until_next_call = 0; + ret = rcl_timer_get_time_until_next_call(&timer, &time_until_next_call); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_LE(time_until_next_call, 0); + + EXPECT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; +} + TEST_F(TestTimerFixture, test_canceled_timer) { rcl_ret_t ret; @@ -567,6 +753,7 @@ TEST_F(TestPreInitTimer, test_timer_get_allocator) { EXPECT_TRUE(rcutils_allocator_is_valid(allocator_returned)); EXPECT_EQ(NULL, rcl_timer_get_allocator(nullptr)); + rcl_reset_error(); } TEST_F(TestPreInitTimer, test_timer_clock) { @@ -599,8 +786,15 @@ TEST_F(TestPreInitTimer, test_timer_call) { EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_end)); EXPECT_GT(next_call_start, next_call_end); + EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&this->clock)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, -1)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_ERROR, rcl_timer_call(&timer)); + rcl_reset_error(); + EXPECT_EQ(times_called, 4); + EXPECT_EQ(RCL_RET_OK, rcl_timer_cancel(&timer)) << rcl_get_error_string().str; EXPECT_EQ(RCL_RET_TIMER_CANCELED, rcl_timer_call(&timer)); + rcl_reset_error(); EXPECT_EQ(times_called, 4); } @@ -654,13 +848,15 @@ TEST_F(TestPreInitTimer, test_invalid_init_fini) { RCL_RET_ALREADY_INIT, rcl_timer_init( &timer, &clock, this->context_ptr, 500, nullptr, rcl_get_default_allocator())) << rcl_get_error_string().str; + rcl_reset_error(); ASSERT_EQ( RCL_RET_BAD_ALLOC, rcl_timer_init( &timer_fail, &clock, this->context_ptr, RCL_S_TO_NS(1), timer_callback_test, bad_allocator)) << rcl_get_error_string().str; + rcl_reset_error(); - EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(nullptr)); + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(nullptr)) << rcl_get_error_string().str; } TEST_F(TestPreInitTimer, test_timer_get_period) { @@ -669,7 +865,9 @@ TEST_F(TestPreInitTimer, test_timer_get_period) { EXPECT_EQ(RCL_S_TO_NS(1), period); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_timer_get_period(nullptr, &period)); + rcl_reset_error(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_timer_get_period(&timer, nullptr)); + rcl_reset_error(); } TEST_F(TestPreInitTimer, test_time_since_last_call) { @@ -677,6 +875,8 @@ TEST_F(TestPreInitTimer, test_time_since_last_call) { rcl_time_point_value_t time_sice_next_call_end = 0u; ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_since_last_call(&timer, &time_sice_next_call_start)); + // Cope with coarse system time resolution. + std::this_thread::sleep_for(std::chrono::milliseconds(1)); ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_since_last_call(&timer, &time_sice_next_call_end)); EXPECT_GT(time_sice_next_call_end, time_sice_next_call_start); } diff --git a/rcl/test/rcl/test_validate_enclave_name.cpp b/rcl/test/rcl/test_validate_enclave_name.cpp index 689d216ab..408bdcccb 100644 --- a/rcl/test/rcl/test_validate_enclave_name.cpp +++ b/rcl/test/rcl/test_validate_enclave_name.cpp @@ -64,7 +64,10 @@ TEST(TestValidateEnclaveName, test_validation_string) { {"/foo/$", RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS, 5}, {"/bar#", RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS, 4}, {"/foo//bar", RCL_ENCLAVE_NAME_INVALID_CONTAINS_REPEATED_FORWARD_SLASH, 5}, - {"/1bar", RCL_ENCLAVE_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER, 1} + {"/1bar", RCL_ENCLAVE_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER, 1}, + {"/" + std::string(RCL_ENCLAVE_NAME_MAX_LENGTH, 'o'), + RCL_ENCLAVE_NAME_INVALID_TOO_LONG, + RCL_ENCLAVE_NAME_MAX_LENGTH - 1} }; for (const auto & case_tuple : enclave_cases_that_should_fail) { std::string enclave = case_tuple.enclave; @@ -82,4 +85,8 @@ TEST(TestValidateEnclaveName, test_validation_string) { "Enclave '" << enclave << "' failed with '" << validation_result << "'."; EXPECT_NE(nullptr, rcl_enclave_name_validation_result_string(validation_result)) << enclave; } + EXPECT_STREQ( + "unknown result code for rcl context name validation", + rcl_enclave_name_validation_result_string(-1)); // invalid result + EXPECT_EQ(nullptr, rcl_enclave_name_validation_result_string(RCL_ENCLAVE_NAME_VALID)); } diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index d8a70162d..82244406f 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -29,6 +29,8 @@ #include "rcutils/logging_macros.h" +#include "./allocator_testing_utils.h" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -36,7 +38,11 @@ # define CLASSNAME(NAME, SUFFIX) NAME #endif +#ifndef _WIN32 #define TOLERANCE RCL_MS_TO_NS(6) +#else +#define TOLERANCE RCL_MS_TO_NS(15) +#endif class CLASSNAME (WaitSetTestFixture, RMW_IMPLEMENTATION) : public ::testing::Test { @@ -86,6 +92,25 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_is_valid) { EXPECT_FALSE(rcl_wait_set_is_valid(&wait_set)); } +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_failed_resize) { + // Initialize a wait set with a subscription and then resize it to zero. + rcl_allocator_t allocator = get_failing_allocator(); + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + set_failing_allocator_is_failing(allocator, false); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + set_failing_allocator_is_failing(allocator, true); + ret = rcl_wait_set_resize(&wait_set, 0, 1, 0, 0, 0, 0); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcl_reset_error(); + + set_failing_allocator_is_failing(allocator, false); + ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) { // Initialize a wait set with a subscription and then resize it to zero. rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); @@ -257,6 +282,49 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout_triggered EXPECT_LE(diff, TOLERANCE); } +// Test rcl_wait with a timeout value and an overrun timer +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout_overrun_timer) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 0, nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Time spent during wait should be negligible, definitely less than the given timeout + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(100)); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // We don't expect a timeout here (since the guard condition had already been triggered) + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, RCL_MS_TO_NS(50)); +} + // Check that a canceled timer doesn't wake up rcl_wait TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();