From e0b92ef9345b368a9cd33fc6a1ea8a12559ad641 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 9 Jun 2020 18:11:10 -0300 Subject: [PATCH 01/42] Add tests for rcl package (#668) * Add tests for node_options usage * Add tests for copying options arguments * Add bad argument tests for wait sets * Add tests for wait_set_get_allocator function * Add test for rcl_take_response function * Change take_request to match take_response without info * Change specific test for sequence_number * Add tests for client take failed * Remove tests already done in nominal setup * Add bad arguments tests * Add test for init returning BAD_ALLOC * Add test for client get_options * Add test for already init client * Add bad argument tests * Add basic test get_default_domain_id * Add test for rmw to rcl return code function * Add test for get_localhost_only * Add tests for localhost expected usage * Address peer review comments * Add test for env variable leading to ULONG_MAX * Change return values to enum in test * Fix rcl_get_localhost_only return value * Address peer review comments * Add unexpected value to get_localhost * Add reset_rcl_error after expected fails Signed-off-by: Jorge Perez From 921c5ffae3f67e7c33b89c285277cd3781370248 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Fri, 3 Jul 2020 18:19:18 -0300 Subject: [PATCH 02/42] Add bad arguments tests for coverage (#698) * Add test context bad_fini * Add bad name service tests * Add default case validation result * Add bad_arg tests rcl_lexer_lookahead2_accept * Add bad_arg test rcl_trigger_guard_condition * Add bad arguments events test * Add message_lost event test * Disable failing test * Address peer review comments * Remove failing test * Separate non related API tests * Add comments to explain error causes * Use test fixture without params * Replace nullptr with NULL Signed-off-by: Jorge Perez --- rcl/test/rcl/test_context.cpp | 22 ++++++++++++ rcl/test/rcl/test_events.cpp | 34 +++++++++++++++++++ rcl/test/rcl/test_guard_condition.cpp | 11 ++++++ rcl/test/rcl/test_lexer_lookahead.cpp | 41 +++++++++++++++++++++++ rcl/test/rcl/test_service.cpp | 18 ++++++++++ rcl/test/rcl/test_validate_topic_name.cpp | 5 +++ 6 files changed, 131 insertions(+) diff --git a/rcl/test/rcl/test_context.cpp b/rcl/test/rcl/test_context.cpp index 9ef189fed..22cc6ff8b 100644 --- a/rcl/test/rcl/test_context.cpp +++ b/rcl/test/rcl/test_context.cpp @@ -128,4 +128,26 @@ TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), nominal) { TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), bad_fini) { EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_context_fini(nullptr)); rcl_reset_error(); + + 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(); + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_OK); + + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_OK); } diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index c5184ac32..d645b4bbb 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -696,6 +696,40 @@ TEST_P(TestEventFixture, test_pubsub_incompatible_qos) tear_down_publisher_subscriber(); } +/* + * Passing bad param subscriber/publisher event ini + */ +TEST_F(TestEventFixture, test_bad_event_ini) +{ + setup_publisher_subscriber(default_qos_profile, default_qos_profile); + const rcl_subscription_event_type_t unknown_sub_type = (rcl_subscription_event_type_t) 5432; + const rcl_publisher_event_type_t unknown_pub_type = (rcl_publisher_event_type_t) 5432; + + publisher_event = rcl_get_zero_initialized_event(); + rcl_ret_t ret = rcl_publisher_event_init( + &publisher_event, + &publisher, + unknown_pub_type); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + + subscription_event = rcl_get_zero_initialized_event(); + ret = rcl_subscription_event_init( + &subscription_event, + &subscription, + unknown_sub_type); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + + tear_down_publisher_subscriber(); +} + +/* + * Passing bad argument to get_rmw_handle + */ +TEST_F(TestEventFixture, test_bad_get_handle) +{ + EXPECT_EQ(NULL, rcl_event_get_rmw_handle(NULL)); +} + static std::array get_test_pubsub_incompatible_qos_inputs() diff --git a/rcl/test/rcl/test_guard_condition.cpp b/rcl/test/rcl/test_guard_condition.cpp index 6a3ee01cf..b34e68d7f 100644 --- a/rcl/test/rcl/test_guard_condition.cpp +++ b/rcl/test/rcl/test_guard_condition.cpp @@ -216,3 +216,14 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret); rcl_reset_error(); } + +/* Tests trigger_guard_condition with bad arguments + */ +TEST_F( + CLASSNAME(TestGuardConditionFixture, RMW_IMPLEMENTATION), test_rcl_guard_condition_bad_arg) { + rcl_guard_condition_t zero_guard_condition = rcl_get_zero_initialized_guard_condition(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_trigger_guard_condition(nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_trigger_guard_condition(&zero_guard_condition)); + rcl_reset_error(); +} diff --git a/rcl/test/rcl/test_lexer_lookahead.cpp b/rcl/test/rcl/test_lexer_lookahead.cpp index fd625e518..0fe8ff05e 100644 --- a/rcl/test/rcl/test_lexer_lookahead.cpp +++ b/rcl/test/rcl/test_lexer_lookahead.cpp @@ -195,6 +195,47 @@ TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_accept) EXPECT_EQ(RCL_LEXEME_EOF, lexeme); } +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_accept_bad_arg) +{ + rcl_lexer_lookahead2_t buffer; + rcl_lexer_lookahead2_t buffer_not_ini = rcl_get_zero_initialized_lexer_lookahead2(); + SCOPE_LOOKAHEAD2(buffer, "foobar/"); + + rcl_lexeme_t lexeme = RCL_LEXEME_NONE; + const char * lexeme_text; + size_t lexeme_text_length = 0; + + // Can't accept without peek first + rcl_ret_t ret = rcl_lexer_lookahead2_accept(&buffer, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Expected usage + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_TOKEN, lexeme); + + // Invalid nullptr parameter + ret = rcl_lexer_lookahead2_accept(nullptr, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Invalid not ini parameter + ret = rcl_lexer_lookahead2_accept(&buffer_not_ini, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Invalid nullptr as lexeme_text_length + ret = rcl_lexer_lookahead2_accept(&buffer, &lexeme_text, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Invalid nullptr as lexeme_text + ret = rcl_lexer_lookahead2_accept(&buffer, nullptr, &lexeme_text_length); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_expect) { rcl_ret_t ret; diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index 024738d46..16d1498e9 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/rcl/test/rcl/test_service.cpp @@ -402,3 +402,21 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_bad_arguments) { &service, this->node_ptr, ts, topic, &service_options_bad_alloc)) << rcl_get_error_string().str; } + +/* Name failed tests + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_fail_name) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * topic = "white space"; + 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, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_SERVICE_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + const char * topic2 = "{invalidbecausecurlybraces}"; + ret = rcl_service_init(&service, this->node_ptr, ts, topic2, &service_options); + EXPECT_EQ(RCL_RET_SERVICE_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} diff --git a/rcl/test/rcl/test_validate_topic_name.cpp b/rcl/test/rcl/test_validate_topic_name.cpp index f54b8567a..eb4a2cecb 100644 --- a/rcl/test/rcl/test_validate_topic_name.cpp +++ b/rcl/test/rcl/test_validate_topic_name.cpp @@ -120,6 +120,11 @@ TEST(test_validate_topic_name, various_valid_topics) { EXPECT_EQ(42u, invalid_index); EXPECT_STREQ(nullptr, rcl_topic_name_validation_result_string(validation_result)); } + + int not_valid_validation_result = 5600; + EXPECT_STREQ( + "unknown result code for rcl topic name validation", + rcl_topic_name_validation_result_string(not_valid_validation_result)); } TEST(test_validate_topic_name, various_invalid_topics) { From 3fa8af83fc2113bc6161314fe0d13fdcdaeec0bf Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 7 Jul 2020 19:43:26 -0300 Subject: [PATCH 03/42] Add coverage tests for `rcl` (#703) * Add tests not empty names_and_types * Add tests graph.c module * Add tests remap basic usage * Add more tests remap module * Add tests ini/fini publisher_node * Refactor style and naming * Add nullptr bad arg * Add tests bad args * Add test bad_alloc * Add missing source file * Remove not working test * Remove extra rcl_reset_error * Change ASSERT to EXPECT when fini scope * Add const for const arguments * Add missing fini parsed arguments * Add error string to output * Add invalid usage comments to test * Fix lint line length issues * Fix uncrustify lint * Modify test to save alloc struct * Remove internal source file * Remove case requiring internal def Signed-off-by: Jorge Perez --- rcl/test/CMakeLists.txt | 2 + rcl/test/rcl/test_arguments.cpp | 188 ++++++++++++++++++------ rcl/test/rcl/test_graph.cpp | 206 ++++++++++++++++++++++++++- rcl/test/rcl/test_logging_rosout.cpp | 18 +++ rcl/test/rcl/test_remap.cpp | 38 +++++ rcl/test/rcl/test_security.cpp | 5 + 6 files changed, 408 insertions(+), 49 deletions(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 73e4eda70..bf8a2d810 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -170,6 +170,7 @@ function(test_target_function) SRCS rcl/test_arguments.cpp ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools AMENT_DEPENDENCIES "osrf_testing_tools_cpp" "rcpputils" ) @@ -178,6 +179,7 @@ function(test_target_function) SRCS rcl/test_remap.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 "osrf_testing_tools_cpp" ) diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp index 5636a5486..c723eb523 100644 --- a/rcl/test/rcl/test_arguments.cpp +++ b/rcl/test/rcl/test_arguments.cpp @@ -26,6 +26,9 @@ #include "rcl_yaml_param_parser/parser.h" +#include "./allocator_testing_utils.h" +#include "./arguments_impl.h" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -274,24 +277,80 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_args) { } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_args) { - int argc = 1; + const int argc = 1; rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret = rcl_parse_arguments(argc, NULL, rcl_get_default_allocator(), &parsed_args); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); } +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_negative_args) { + const int argc = -1; + const char * const argv[] = {"process_name"}; + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_parse_args) { + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, bad_alloc, &parsed_args); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_unparse_args) { + const char * const argv[] = { + "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "}bar:=fiz", "--", "arg"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(2, rcl_arguments_get_count_unparsed(&parsed_args)); + + int * actual_unparsed = NULL; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_arguments_get_unparsed(&parsed_args, bad_alloc, &actual_unparsed)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_arguments_get_unparsed_ros(&parsed_args, bad_alloc, &actual_unparsed)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_params_get_counts) { + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + EXPECT_EQ(-1, rcl_arguments_get_count_unparsed(nullptr)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_count_unparsed_ros(nullptr)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_param_files_count(nullptr)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_count_unparsed(&parsed_args)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_param_files_count(&parsed_args)); + rcl_reset_error(); +} + TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_args_output) { - const char * argv[] = {"process_name"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), NULL); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_ros_args) { - const char * argv[] = {"process_name"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -301,8 +360,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_ros_args) { } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_zero_ros_args) { - const char * argv[] = {"process_name", "--ros-args"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name", "--ros-args"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -312,8 +371,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_zero_ros_args) } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_zero_ros_args_w_trailing_dashes) { - const char * argv[] = {"process_name", "--ros-args", "--"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name", "--ros-args", "--"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -323,10 +382,10 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_zero_ros_args_w } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remap) { - const char * argv[] = { + const char * const argv[] = { "process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--remap", "foo:=/baz" }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret; ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); @@ -337,8 +396,9 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remap) { } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_two_ros_args) { - const char * argv[] = {"process_name", "--ros-args", "--ros-args", "-r", "/foo/bar:=/fiz/buz"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = + {"process_name", "--ros-args", "--ros-args", "-r", "/foo/bar:=/fiz/buz"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret; ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); @@ -349,8 +409,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_two_r } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_trailing_dashes) { - const char * argv[] = {"process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret; ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); @@ -361,8 +421,9 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_tra } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_two_trailing_dashes) { - const char * argv[] = {"process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--", "--"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = + {"process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--", "--"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret; ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); @@ -373,10 +434,10 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_two } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_mix_valid_invalid_rules) { - const char * argv[] = { + const char * const argv[] = { "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "}bar:=fiz", "--", "arg" }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret; ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); @@ -387,11 +448,11 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_mix_valid_inval } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy) { - const char * argv[] = { + const char * const argv[] = { "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "-r", "__ns:=/foo", "--", "arg" }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret; @@ -402,6 +463,11 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy) { ret = rcl_arguments_copy(&parsed_args, &copied_args); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // Can't copy to non empty + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_UNPARSED(parsed_args, 0, 8); EXPECT_UNPARSED_ROS(parsed_args, 2); EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); @@ -411,9 +477,30 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy) { EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&copied_args)); } +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_bad_alloc) { + const char * const argv[] = {"process_name", "--ros-args", "/foo/bar:="}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_allocator_t saved_alloc = parsed_args.impl->allocator; + parsed_args.impl->allocator = bad_alloc; + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + rcl_reset_error(); + parsed_args.impl->allocator = saved_alloc; + + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)) << rcl_get_error_string().str; +} + TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_no_ros_args) { - const char * argv[] = {"process_name", "--ros-args", "--", "arg", "--ros-args"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name", "--ros-args", "--", "arg", "--ros-args"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret; @@ -466,10 +553,10 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_no_args) { } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_two_namespace) { - const char * argv[] = { + const char * const argv[] = { "process_name", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz" }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_ret_t ret; ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); @@ -480,8 +567,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_two_namespace) } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_uninitialized_parsed_args) { - const char * argv[] = {"process_name"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args; int not_null = 1; parsed_args.impl = reinterpret_cast(¬_null); @@ -492,10 +579,10 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_uninitialized_p } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_double_parse) { - const char * argv[] = { + const char * const argv[] = { "process_name", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz" }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); ASSERT_EQ( RCL_RET_OK, @@ -523,8 +610,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_impl_null) } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_twice) { - const char * argv[] = {"process_name"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args)); EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); @@ -533,8 +620,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_twice) { } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_remove_ros_args) { - const char * argv[] = {"process_name"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_allocator_t default_allocator = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); @@ -565,6 +652,12 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_remove_ros_ argv, &parsed_args, zero_initialized_allocator, &nonros_argc, &nonros_argv)); rcl_reset_error(); + rcl_allocator_t bad_alloc = get_failing_allocator(); + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_remove_ros_arguments( + argv, &parsed_args, bad_alloc, &nonros_argc, &nonros_argv)); + rcl_reset_error(); + EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, rcl_remove_ros_arguments( argv, &parsed_args, default_allocator, NULL, &nonros_argv)); @@ -606,11 +699,11 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_remove_ros_ } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args) { - const char * argv[] = { + const char * const argv[] = { "process_name", "-d", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz", "--", "--foo=bar", "--baz", "--ros-args", "--ros-args", "-p", "bar:=baz", "--", "--", "arg", }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); @@ -646,8 +739,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args_if_ros_only) { - const char * argv[] = {"--ros-args", "--disable-rosout-logs"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"--ros-args", "--disable-rosout-logs"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); @@ -713,8 +806,9 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_zero) { - const char * argv[] = {"process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = + {"process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); @@ -732,11 +826,11 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_single) { const std::string parameters_filepath = (test_path / "test_parameters.1.yaml").string(); - const char * argv[] = { + const char * const argv[] = { "process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath.c_str() }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); @@ -784,11 +878,11 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_multiple) { const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); const std::string parameters_filepath2 = (test_path / "test_parameters.2.yaml").string(); - const char * argv[] = { + const char * const argv[] = { "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str(), "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath2.c_str() }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_ret_t ret; rcl_allocator_t alloc = rcl_get_default_allocator(); @@ -850,8 +944,8 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_ } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_param_overrides) { - const char * argv[] = {"process_name"}; - int argc = sizeof(argv) / sizeof(const char *); + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); @@ -886,14 +980,14 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_param_overri TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_overrides) { const std::string parameters_filepath = (test_path / "test_parameters.1.yaml").string(); - const char * argv[] = { + const char * const argv[] = { "process_name", "--ros-args", "--params-file", parameters_filepath.c_str(), "--param", "string_param:=bar", "-p", "some.bool_param:=false", "-p", "some_node:int_param:=4" }; - int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(const char *); rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index d72a08997..6d8d83a2f 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -149,7 +149,7 @@ TEST_F( rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_allocator_t zero_allocator = static_cast( rcutils_get_zero_initialized_allocator()); - rcl_names_and_types_t tnat {}; + rcl_names_and_types_t tnat = rcl_get_zero_initialized_names_and_types(); rcl_node_t zero_node = rcl_get_zero_initialized_node(); // invalid node ret = rcl_get_topic_names_and_types(nullptr, &allocator, false, &tnat); @@ -172,6 +172,11 @@ TEST_F( ret = rcl_get_topic_names_and_types(this->node_ptr, &allocator, false, nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + tnat.names.size = 1; + ret = rcl_get_topic_names_and_types(this->node_ptr, &allocator, false, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + tnat.names.size = 0; // invalid argument to rcl_destroy_topic_names_and_types ret = rcl_names_and_types_fini(nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; @@ -195,7 +200,7 @@ TEST_F( rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_allocator_t zero_allocator = static_cast( rcutils_get_zero_initialized_allocator()); - rcl_names_and_types_t tnat {}; + rcl_names_and_types_t tnat = rcl_get_zero_initialized_names_and_types(); rcl_node_t zero_node = rcl_get_zero_initialized_node(); // invalid node ret = rcl_get_service_names_and_types(nullptr, &allocator, &tnat); @@ -218,6 +223,11 @@ TEST_F( ret = rcl_get_service_names_and_types(this->node_ptr, &allocator, nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + tnat.names.size = 1; + ret = rcl_get_service_names_and_types(this->node_ptr, &allocator, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + tnat.names.size = 0; // invalid argument to rcl_destroy_service_names_and_types ret = rcl_names_and_types_fini(nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; @@ -334,6 +344,12 @@ TEST_F( this->node_ptr, &allocator, false, this->test_graph_node_name, "", nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + nat.names.size = 1; + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 0; // unknown node name ret = rcl_get_publisher_names_and_types_by_node( this->node_ptr, &allocator, false, unknown_node_name, "", &nat); @@ -419,6 +435,12 @@ TEST_F( this->node_ptr, &allocator, false, this->test_graph_node_name, "", nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + nat.names.size = 1; + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 0; // unknown node name ret = rcl_get_subscriber_names_and_types_by_node( this->node_ptr, &allocator, false, unknown_node_name, "", &nat); @@ -501,6 +523,12 @@ TEST_F( this->node_ptr, &allocator, this->test_graph_node_name, "", nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + nat.names.size = 1; + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 0; // unknown node name ret = rcl_get_service_names_and_types_by_node( this->node_ptr, &allocator, unknown_node_name, "", &nat); @@ -584,6 +612,12 @@ TEST_F( this->node_ptr, &allocator, this->test_graph_node_name, "", nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + nat.names.size = 1; + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 0; // unknown node name ret = rcl_get_client_names_and_types_by_node( this->node_ptr, &allocator, unknown_node_name, "", &nat); @@ -1354,3 +1388,171 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_ wait_for_service_state_to_change(false, is_available); ASSERT_FALSE(is_available); } + +/* Test passing invalid params to server_is_available + */ +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_bad_server_available) { + // Create a client which will be used to call the function. + rcl_client_t client = rcl_get_zero_initialized_client(); + auto ts = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + const char * service_name = "/service_test_rcl_service_server_is_available"; + rcl_client_options_t client_options = rcl_client_get_default_options(); + rcl_ret_t ret = rcl_client_init(&client, this->node_ptr, ts, service_name, &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + // Check, knowing there is no service server (created by us at least). + bool is_available; + ret = rcl_service_server_is_available(this->node_ptr, &client, &is_available); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_FALSE(is_available); + + ret = rcl_service_server_is_available(nullptr, &client, &is_available); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + rcl_node_t not_init_node = rcl_get_zero_initialized_node(); + ret = rcl_service_server_is_available(¬_init_node, &client, &is_available); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); +} + +/* Test passing invalid params to get_node_names functions + */ +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_bad_get_node_names) { + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + + rcutils_string_array_t node_names_2 = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces_2 = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_enclaves = rcutils_get_zero_initialized_string_array(); + rcl_ret_t ret = RCL_RET_OK; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcutils_string_array_fini(&node_names); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_namespaces); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_enclaves); + EXPECT_EQ(RCUTILS_RET_OK, ret); + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + + // Invalid nullptr as node + ret = rcl_get_node_names(nullptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_get_node_names_with_enclaves( + nullptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + + // Invalid not init node + rcl_node_t not_init_node = rcl_get_zero_initialized_node(); + ret = rcl_get_node_names(¬_init_node, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_get_node_names_with_enclaves( + ¬_init_node, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + + // Invalid nullptr as node names output + ret = rcl_get_node_names(this->node_ptr, allocator, nullptr, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, nullptr, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Invalid nullptr as node_namespaces output + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, nullptr, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Invalid nullptr as node_enclaves output + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Invalid node_names previously init (size is set) + node_names.size = 1; + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_names.size = 0; + + // Invalid node_names previously init (size is zero, but internal structure size is 1) + ret = rcutils_string_array_init(&node_names, 1, &allocator); + EXPECT_EQ(RCL_RET_OK, ret); + node_names.size = 0; + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_names.size = 1; + ret = rcutils_string_array_fini(&node_names); + EXPECT_EQ(RCL_RET_OK, ret); + + // Invalid node_namespaces previously init (size is set) + node_namespaces.size = 1; + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_namespaces.size = 0; + + // Invalid node_namespaces previously init (size is zero, but internal structure size is 1) + ret = rcutils_string_array_init(&node_namespaces, 1, &allocator); + EXPECT_EQ(RCL_RET_OK, ret); + node_namespaces.size = 0; + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_namespaces.size = 1; + ret = rcutils_string_array_fini(&node_namespaces); + EXPECT_EQ(RCL_RET_OK, ret); + + // Invalid node_enclaves previously init (size is set) + node_enclaves.size = 1; + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_enclaves.size = 0; + + // Invalid node_enclave previously init (size is zero, but internal structure size is 1) + ret = rcutils_string_array_init(&node_enclaves, 1, &allocator); + EXPECT_EQ(RCL_RET_OK, ret); + node_enclaves.size = 0; + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_enclaves.size = 1; + ret = rcutils_string_array_fini(&node_enclaves); + EXPECT_EQ(RCL_RET_OK, ret); + + // Expected usage + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names_2, &node_namespaces_2, &node_enclaves); + EXPECT_EQ(RCL_RET_OK, ret); +} diff --git a/rcl/test/rcl/test_logging_rosout.cpp b/rcl/test/rcl/test_logging_rosout.cpp index 8816a55a9..ac2020ad6 100644 --- a/rcl/test/rcl/test_logging_rosout.cpp +++ b/rcl/test/rcl/test_logging_rosout.cpp @@ -25,6 +25,8 @@ #include "rcl_interfaces/msg/log.h" #include "rcutils/logging_macros.h" +#include "rcl/logging_rosout.h" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -63,6 +65,8 @@ std::ostream & operator<<( return out; } +class CLASSNAME (TestLogRosoutFixtureNotParam, RMW_IMPLEMENTATION) : public ::testing::Test {}; + class TEST_FIXTURE_P_RMW (TestLoggingRosoutFixture) : public ::testing::TestWithParam { @@ -272,3 +276,17 @@ INSTANTIATE_TEST_CASE_P_RMW( TestLoggingRosoutFixture, ::testing::ValuesIn(get_parameters()), ::testing::PrintToStringParamName()); + +/* Testing twice init logging_rosout + */ +TEST_F( + CLASSNAME(TestLogRosoutFixtureNotParam, RMW_IMPLEMENTATION), test_twice_init_logging_rosout) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_init(&allocator)); + + // Init twice returns RCL_RET_OK + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_init(&allocator)); + + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_fini()); +} diff --git a/rcl/test/rcl/test_remap.cpp b/rcl/test/rcl/test_remap.cpp index 29b41658e..1b0d2eb0e 100644 --- a/rcl/test/rcl/test_remap.cpp +++ b/rcl/test/rcl/test_remap.cpp @@ -14,11 +14,15 @@ #include +#include "rcl/arguments.h" #include "rcl/rcl.h" #include "rcl/remap.h" #include "rcl/error_handling.h" #include "./arg_macros.hpp" +#include "./arguments_impl.h" +#include "./allocator_testing_utils.h" +#include "./remap_impl.h" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -536,3 +540,37 @@ TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), url_scheme_rostopic) { EXPECT_EQ(RCL_RET_OK, ret); EXPECT_EQ(NULL, output); } + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), _rcl_remap_name_bad_arg) { + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "__node:=global_name"); + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name", "--ros-args", "-r", "__node:=local_name"); + rcl_arguments_t zero_init_global_arguments = rcl_get_zero_initialized_arguments(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t bad_allocator = get_failing_allocator(); + char * output = NULL; + + // Expected usage local_args, global not init is OK + rcl_ret_t ret = rcl_remap_node_name( + &local_arguments, &zero_init_global_arguments, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("local_name", output); + allocator.deallocate(output, allocator.state); + + // Expected usage global_args, local not null is OK + ret = rcl_remap_node_name(nullptr, &global_arguments, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("global_name", output); + allocator.deallocate(output, allocator.state); + + // Both local and global arguments, not valid + ret = rcl_remap_node_name(nullptr, nullptr, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Bad allocator + ret = rcl_remap_node_name(nullptr, &global_arguments, "NodeName", bad_allocator, &output); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); +} diff --git a/rcl/test/rcl/test_security.cpp b/rcl/test/rcl/test_security.cpp index 85aa9ba2e..9fbf28d67 100644 --- a/rcl/test/rcl/test_security.cpp +++ b/rcl/test/rcl/test_security.cpp @@ -121,6 +121,11 @@ class TestGetSecureRoot : public ::testing::Test }; TEST_F(TestGetSecureRoot, failureScenarios) { + EXPECT_EQ( + rcl_get_secure_root(nullptr, &allocator), + (char *) NULL); + rcl_reset_error(); + EXPECT_EQ( rcl_get_secure_root(TEST_ENCLAVE_ABSOLUTE, &allocator), (char *) NULL); From 8858fccd8ed7b742b4c1882fed44245386f8d172 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 16 Jul 2020 13:44:23 -0300 Subject: [PATCH 04/42] Move rcl_remap_copy to public header (#709) * Add test for remap internal functions * Add function headers * Make local function public * Move rcl_remap_copy to public header Signed-off-by: Jorge Perez --- rcl/include/rcl/remap.h | 25 ++++++++++++++++++ rcl/src/rcl/remap_impl.h | 51 ------------------------------------- rcl/test/rcl/test_remap.cpp | 43 +++++++++++++++++++++++++++++-- 3 files changed, 66 insertions(+), 53 deletions(-) diff --git a/rcl/include/rcl/remap.h b/rcl/include/rcl/remap.h index a7b10ea93..82f0b0e04 100644 --- a/rcl/include/rcl/remap.h +++ b/rcl/include/rcl/remap.h @@ -247,6 +247,31 @@ rcl_remap_node_namespace( rcl_allocator_t allocator, char ** output_namespace); +/// Copy one remap structure into another. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] rule The structure to be copied. + * Its allocator is used to copy memory into the new structure. + * \param[out] rule_out A zero-initialized rcl_remap_t structure to be copied into. + * \return `RCL_RET_OK` if the structure was copied successfully, or + * \return `RCL_RET_INVALID_ARGUMENT` if any function arguments are invalid, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_remap_copy( + const rcl_remap_t * rule, + rcl_remap_t * rule_out); + /// Reclaim resources held inside rcl_remap_t structure. /** *
diff --git a/rcl/src/rcl/remap_impl.h b/rcl/src/rcl/remap_impl.h index de8e835f8..898e58cb7 100644 --- a/rcl/src/rcl/remap_impl.h +++ b/rcl/src/rcl/remap_impl.h @@ -51,57 +51,6 @@ typedef struct rcl_remap_impl_t rcl_allocator_t allocator; } rcl_remap_impl_t; -/// Get an rcl_remap_t structure initialized with NULL. -rcl_remap_t -rcl_remap_get_zero_initialized(); - -/// Copy one remap structure into another. -/** - *
- * Attribute | Adherence - * ------------------ | ------------- - * Allocates Memory | Yes - * Thread-Safe | No - * Uses Atomics | No - * Lock-Free | Yes - * - * \param[in] rule The structure to be copied. - * Its allocator is used to copy memory into the new structure. - * \param[out] rule_out A zero-initialized rcl_remap_t structure to be copied into. - * \return `RCL_RET_OK` if the structure was copied successfully, or - * \return `RCL_RET_INVALID_ARGUMENT` if any function arguments are invalid, or - * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_remap_copy( - const rcl_remap_t * rule, - rcl_remap_t * rule_out); - -/// Reclaim resources used in an rcl_remap_t structure. -/** - *
- * Attribute | Adherence - * ------------------ | ------------- - * Allocates Memory | No - * Thread-Safe | Yes - * Uses Atomics | No - * Lock-Free | Yes - * - * \param[in] rule A rule to deallocate back to a zero initialized state. - * \return `RCL_RET_OK` if the structure was free'd, or - * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or - * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or - * \return `RCL_RET_NODE_INVALID_NAME` if the name is invalid, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. - */ -RCL_WARN_UNUSED -rcl_ret_t -rcl_remap_fini( - rcl_remap_t * rule); - #ifdef __cplusplus } #endif diff --git a/rcl/test/rcl/test_remap.cpp b/rcl/test/rcl/test_remap.cpp index 1b0d2eb0e..f2dca9dbe 100644 --- a/rcl/test/rcl/test_remap.cpp +++ b/rcl/test/rcl/test_remap.cpp @@ -19,10 +19,9 @@ #include "rcl/remap.h" #include "rcl/error_handling.h" +#include "./allocator_testing_utils.h" #include "./arg_macros.hpp" #include "./arguments_impl.h" -#include "./allocator_testing_utils.h" -#include "./remap_impl.h" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -574,3 +573,43 @@ TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), _rcl_remap_name_bad_arg) EXPECT_EQ(RCL_RET_ERROR, ret); rcl_reset_error(); } + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), internal_remap_use) { + // Easiest way to init a rcl_remap is through the arguments API + const char * argv[] = { + "process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg" + }; + int argc = sizeof(argv) / sizeof(const char *); + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + // Bad alloc + rcl_remap_t remap_dst = rcl_get_zero_initialized_remap(); + parsed_args.impl->remap_rules->impl->allocator = get_failing_allocator(); + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_remap_copy(parsed_args.impl->remap_rules, &remap_dst)); + parsed_args.impl->remap_rules->impl->allocator = alloc; + + // Expected usage + EXPECT_EQ(RCL_RET_OK, rcl_remap_copy(parsed_args.impl->remap_rules, &remap_dst)); + + // Copy twice + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_copy(parsed_args.impl->remap_rules, &remap_dst)); + rcl_reset_error(); + + // Fini + EXPECT_EQ(RCL_RET_OK, rcl_remap_fini(&remap_dst)); + + // Fini twice + EXPECT_EQ(RCL_RET_ERROR, rcl_remap_fini(&remap_dst)); + rcl_reset_error(); + + // Bad fini + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_fini(nullptr)); +} From f1d889de3a7861d7b1c26d6fdb7a8aecd7d6c44f Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 20 Jul 2020 17:28:22 -0300 Subject: [PATCH 05/42] Make public ini/fini rosout publisher (#704) * Add bad param tests ini/fini rosout publisher * Make ini/fini_publisher_for_node public Signed-off-by: Jorge Perez --- rcl/include/rcl/logging_rosout.h | 4 ++-- rcl/test/rcl/test_logging_rosout.cpp | 24 ++++++++++++++++++++++++ 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/rcl/include/rcl/logging_rosout.h b/rcl/include/rcl/logging_rosout.h index 03f6bc06d..255511680 100644 --- a/rcl/include/rcl/logging_rosout.h +++ b/rcl/include/rcl/logging_rosout.h @@ -98,7 +98,7 @@ rcl_logging_rosout_fini(); * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ -RCL_LOCAL +RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_logging_rosout_init_publisher_for_node( @@ -124,7 +124,7 @@ rcl_logging_rosout_init_publisher_for_node( * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ -RCL_LOCAL +RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_logging_rosout_fini_publisher_for_node( diff --git a/rcl/test/rcl/test_logging_rosout.cpp b/rcl/test/rcl/test_logging_rosout.cpp index ac2020ad6..e8a6d5fd0 100644 --- a/rcl/test/rcl/test_logging_rosout.cpp +++ b/rcl/test/rcl/test_logging_rosout.cpp @@ -290,3 +290,27 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_fini()); } + +/* Bad params + */ +TEST_F( + CLASSNAME( + TestLogRosoutFixtureNotParam, RMW_IMPLEMENTATION), + test_bad_params_init_fini_node_publisher) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_node_t not_init_node = rcl_get_zero_initialized_node(); + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_init(&allocator)); + + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_logging_rosout_init_publisher_for_node(nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_logging_rosout_init_publisher_for_node(¬_init_node)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_logging_rosout_fini_publisher_for_node(nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_logging_rosout_fini_publisher_for_node(¬_init_node)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_fini()); +} From 608cbb67beacdb4eb8632a80e43569f88e35fb8c Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 20 Jul 2020 17:31:53 -0300 Subject: [PATCH 06/42] Add remap needed null check (#711) * Add needed extra null check * Add test for added check * Add tests for nullptrs Signed-off-by: Jorge Perez --- rcl/src/rcl/remap.c | 1 + rcl/test/rcl/test_remap.cpp | 11 +++++++++++ 2 files changed, 12 insertions(+) diff --git a/rcl/src/rcl/remap.c b/rcl/src/rcl/remap.c index 5a84a636a..950ff9761 100644 --- a/rcl/src/rcl/remap.c +++ b/rcl/src/rcl/remap.c @@ -43,6 +43,7 @@ rcl_remap_copy( { RCL_CHECK_ARGUMENT_FOR_NULL(rule, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(rule_out, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(rule->impl, RCL_RET_INVALID_ARGUMENT); if (NULL != rule_out->impl) { RCL_SET_ERROR_MSG("rule_out must be zero initialized"); diff --git a/rcl/test/rcl/test_remap.cpp b/rcl/test/rcl/test_remap.cpp index f2dca9dbe..72016d867 100644 --- a/rcl/test/rcl/test_remap.cpp +++ b/rcl/test/rcl/test_remap.cpp @@ -596,6 +596,17 @@ TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), internal_remap_use) { EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_remap_copy(parsed_args.impl->remap_rules, &remap_dst)); parsed_args.impl->remap_rules->impl->allocator = alloc; + // Not valid null ptrs + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_copy(nullptr, &remap_dst)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_copy(parsed_args.impl->remap_rules, nullptr)); + rcl_reset_error(); + + // Not valid empty source + rcl_remap_t remap_empty = rcl_get_zero_initialized_remap(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_copy(&remap_empty, &remap_dst)); + rcl_reset_error(); + // Expected usage EXPECT_EQ(RCL_RET_OK, rcl_remap_copy(parsed_args.impl->remap_rules, &remap_dst)); From 12ead98f323edfabd3cc758fc6c4e848f0c523f7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 21 Jul 2020 13:00:52 +0000 Subject: [PATCH 07/42] Make sure to call rcl_arguments_fini at the end of the test. This just prevents a memory leak. Signed-off-by: Chris Lalancette --- rcl/test/rcl/test_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index 7a0253692..484739d90 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -798,4 +798,6 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_options_fai rcl_node_options_t default_options = rcl_node_get_default_options(); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_node_options_copy(&default_options, &prev_ini_options)); + + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&prev_ini_options.arguments)); } From b3342887c2181e4c7d6cec486d498501397c67c6 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 27 Jul 2020 15:57:45 -0300 Subject: [PATCH 08/42] Reformat rmw_impl_id_check to call a testable function (#725) * Reformat rmw_impl_id_check to call a testable function * Reformat to expose the function in the public header * Reformat style return result * Expose macro names to be tested with the function checker * Add test for failing cases of the function * Set error variable and log in the main caller * Use format string for logging * Change name of checker function * Reset rcl error to avoid overwrite Signed-off-by: Jorge Perez --- .../rcl/rmw_implementation_identifier_check.h | 35 ++++++++++ .../rcl/rmw_implementation_identifier_check.c | 53 +++++++------- rcl/test/CMakeLists.txt | 8 +++ rcl/test/rcl/test_rmw_impl_id_check_func.cpp | 70 +++++++++++++++++++ 4 files changed, 142 insertions(+), 24 deletions(-) create mode 100644 rcl/include/rcl/rmw_implementation_identifier_check.h create mode 100644 rcl/test/rcl/test_rmw_impl_id_check_func.cpp diff --git a/rcl/include/rcl/rmw_implementation_identifier_check.h b/rcl/include/rcl/rmw_implementation_identifier_check.h new file mode 100644 index 000000000..2a1ecd35a --- /dev/null +++ b/rcl/include/rcl/rmw_implementation_identifier_check.h @@ -0,0 +1,35 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__RMW_IMPLEMENTATION_IDENTIFIER_CHECK_H_ +#define RCL__RMW_IMPLEMENTATION_IDENTIFIER_CHECK_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/visibility_control.h" + +#define RMW_IMPLEMENTATION_ENV_VAR_NAME "RMW_IMPLEMENTATION" +#define RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME "RCL_ASSERT_RMW_ID_MATCHES" + +RCL_PUBLIC +rcl_ret_t rcl_rmw_implementation_identifier_check(void); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__RMW_IMPLEMENTATION_IDENTIFIER_CHECK_H_ diff --git a/rcl/src/rcl/rmw_implementation_identifier_check.c b/rcl/src/rcl/rmw_implementation_identifier_check.c index e66ee2168..9c068a67f 100644 --- a/rcl/src/rcl/rmw_implementation_identifier_check.c +++ b/rcl/src/rcl/rmw_implementation_identifier_check.c @@ -30,8 +30,7 @@ extern "C" #include "rcl/types.h" -#define RMW_IMPLEMENTATION_ENV_VAR_NAME "RMW_IMPLEMENTATION" -#define RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME "RCL_ASSERT_RMW_ID_MATCHES" +#include "rcl/rmw_implementation_identifier_check.h" // Extracted this portable method of doing a "shared library constructor" from SO: // http://stackoverflow.com/a/2390626/671658 @@ -55,7 +54,8 @@ extern "C" static void f(void) #endif -INITIALIZER(initialize) { +rcl_ret_t rcl_rmw_implementation_identifier_check(void) +{ // If the environment variable RMW_IMPLEMENTATION is set, or // the environment variable RCL_ASSERT_RMW_ID_MATCHES is set, // check that the result of `rmw_get_implementation_identifier` matches. @@ -66,18 +66,17 @@ INITIALIZER(initialize) { RMW_IMPLEMENTATION_ENV_VAR_NAME, &expected_rmw_impl_env); if (NULL != get_env_error_str) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( "Error getting env var '" RCUTILS_STRINGIFY(RMW_IMPLEMENTATION_ENV_VAR_NAME) "': %s\n", get_env_error_str); - exit(RCL_RET_ERROR); + return RCL_RET_ERROR; } if (strlen(expected_rmw_impl_env) > 0) { // Copy the environment variable so it doesn't get over-written by the next getenv call. expected_rmw_impl = rcutils_strdup(expected_rmw_impl_env, allocator); if (!expected_rmw_impl) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "allocation failed"); - exit(RCL_RET_BAD_ALLOC); + RCL_SET_ERROR_MSG("allocation failed"); + return RCL_RET_BAD_ALLOC; } } @@ -86,31 +85,29 @@ INITIALIZER(initialize) { get_env_error_str = rcutils_get_env( RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, &asserted_rmw_impl_env); if (NULL != get_env_error_str) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( "Error getting env var '" RCUTILS_STRINGIFY(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME) "': %s\n", get_env_error_str); - exit(RCL_RET_ERROR); + return RCL_RET_ERROR; } if (strlen(asserted_rmw_impl_env) > 0) { // Copy the environment variable so it doesn't get over-written by the next getenv call. asserted_rmw_impl = rcutils_strdup(asserted_rmw_impl_env, allocator); if (!asserted_rmw_impl) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "allocation failed"); - exit(RCL_RET_BAD_ALLOC); + RCL_SET_ERROR_MSG("allocation failed"); + return RCL_RET_BAD_ALLOC; } } // If both environment variables are set, and they do not match, print an error and exit. if (expected_rmw_impl && asserted_rmw_impl && strcmp(expected_rmw_impl, asserted_rmw_impl) != 0) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( "Values of RMW_IMPLEMENTATION ('%s') and RCL_ASSERT_RMW_ID_MATCHES ('%s') environment " "variables do not match, exiting with %d.", expected_rmw_impl, asserted_rmw_impl, RCL_RET_ERROR ); - exit(RCL_RET_ERROR); + return RCL_RET_ERROR; } // Collapse the expected_rmw_impl and asserted_rmw_impl variables so only expected_rmw_impl needs @@ -130,31 +127,39 @@ INITIALIZER(initialize) { // If either environment variable is set, and it does not match, print an error and exit. if (expected_rmw_impl) { const char * actual_rmw_impl_id = rmw_get_implementation_identifier(); + const rcutils_error_string_t rmw_error_msg = rcl_get_error_string(); + rcl_reset_error(); if (!actual_rmw_impl_id) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( "Error getting RMW implementation identifier / RMW implementation not installed " "(expected identifier of '%s'), with error message '%s', exiting with %d.", expected_rmw_impl, - rcl_get_error_string().str, + rmw_error_msg.str, RCL_RET_ERROR ); - rcl_reset_error(); - exit(RCL_RET_ERROR); + return RCL_RET_ERROR; } if (strcmp(actual_rmw_impl_id, expected_rmw_impl) != 0) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( "Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.", expected_rmw_impl, actual_rmw_impl_id, RCL_RET_MISMATCHED_RMW_ID ); - exit(RCL_RET_MISMATCHED_RMW_ID); + return RCL_RET_MISMATCHED_RMW_ID; } // Free the memory now that all checking has passed. allocator.deallocate((char *)expected_rmw_impl, allocator.state); } + return RCL_RET_OK; +} + +INITIALIZER(initialize) { + rcl_ret_t ret = rcl_rmw_implementation_identifier_check(); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "%s\n", rcl_get_error_string().str); + exit(ret); + } } #ifdef __cplusplus diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index bf8a2d810..a6a584b9f 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -279,6 +279,14 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) + rcl_add_custom_gtest(test_rmw_impl_id_check_func${target_suffix} + SRCS rcl/test_rmw_impl_id_check_func.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} + ) + # Launch tests rcl_add_custom_executable(service_fixture${target_suffix} diff --git a/rcl/test/rcl/test_rmw_impl_id_check_func.cpp b/rcl/test/rcl/test_rmw_impl_id_check_func.cpp new file mode 100644 index 000000000..6398a3878 --- /dev/null +++ b/rcl/test/rcl/test_rmw_impl_id_check_func.cpp @@ -0,0 +1,70 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcutils/env.h" + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/rmw_implementation_identifier_check.h" + +TEST(TestRmwCheck, test_rmw_check_id_impl) { + EXPECT_EQ(RCL_RET_OK, rcl_rmw_implementation_identifier_check()); +} + +TEST(TestRmwCheck, test_failing_configuration) { + const char * expected_rmw_impl_env = NULL; + const char * expected_rmw_id_matches = NULL; + + const char * get_env_var_name = rcutils_get_env( + RMW_IMPLEMENTATION_ENV_VAR_NAME, + &expected_rmw_impl_env); + + const char * get_env_id_matches_name = rcutils_get_env( + RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, + &expected_rmw_id_matches); + + // Fail test case, reason: RMW_IMPLEMENTATION_ENV_VAR_NAME set, not matching rmw impl + EXPECT_TRUE(rcutils_set_env(RMW_IMPLEMENTATION_ENV_VAR_NAME, "some_random_name")); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "")); + EXPECT_EQ(RCL_RET_MISMATCHED_RMW_ID, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Fail test case, reason: RMW_IMPLEMENTATION_ENV_VAR_NAME set, not matching rmw impl + EXPECT_TRUE(rcutils_set_env(RMW_IMPLEMENTATION_ENV_VAR_NAME, "")); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "some_random_name")); + EXPECT_EQ(RCL_RET_MISMATCHED_RMW_ID, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Fail test case, reason: env variables not equal + EXPECT_TRUE(rcutils_set_env(RMW_IMPLEMENTATION_ENV_VAR_NAME, "some_random_name")); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "diff_random")); + EXPECT_EQ(RCL_RET_ERROR, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Fail test case, reason: equal env variables do not match rmw impl + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "some_random_name")); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "some_random_name")); + EXPECT_EQ(RCL_RET_MISMATCHED_RMW_ID, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Restore env variables set in the test + EXPECT_TRUE(rcutils_set_env(RMW_IMPLEMENTATION_ENV_VAR_NAME, get_env_var_name)); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, get_env_id_matches_name)); +} From 286de9212170414da27244c1364c71312ac32f0d Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 10 Aug 2020 17:32:39 -0300 Subject: [PATCH 09/42] Add mock tests, publisher 95% coverage (#732) * Add mimick test for rcl_publisher_get_subscription_count * Remove const qualifiers * Add missing mock suffix * Improve test description * Add mock test for rcl_publisher_assert_liveliness * Add class to init publisher tests * Add test for mocked rmw_publish * Add mock test for rcl_publish_serialized * Mock rcutils_string_map_init to make init fail * Add mock test making rmw_publisher_get_actual_qos fail * Add class to ease mimick usage * Reformat tests to use helper class * Add mocked rcutils_string_map_init to make init fail * Add tests mocking loaned functions * Add mock fail tests for publisher_init * Add publisher fini fail mock tests * Add nullptr tests * Update mocking utilities * Reformat with macro utility * Add comments for mocked tests * Check count_size value after test * Reformat to use constexpr where possible * Add variable making clear bad param test * Add link to original file to help tracking changes Signed-off-by: Jorge Perez --- rcl/package.xml | 1 + rcl/test/CMakeLists.txt | 4 +- rcl/test/mocking_utils/patch.hpp | 355 ++++++++++++++++++++++++++++ rcl/test/rcl/test_publisher.cpp | 381 +++++++++++++++++++++++++++++-- 4 files changed, 724 insertions(+), 17 deletions(-) create mode 100644 rcl/test/mocking_utils/patch.hpp diff --git a/rcl/package.xml b/rcl/package.xml index f6c8b0a6c..c40622334 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -25,6 +25,7 @@ ament_cmake_pytest ament_lint_auto ament_lint_common + mimick_vendor rcpputils rmw rmw_implementation_cmake diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index a6a584b9f..bd04416f7 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -3,6 +3,8 @@ find_package(launch_testing_ament_cmake REQUIRED) find_package(test_msgs REQUIRED) +find_package(mimick_vendor REQUIRED) + find_package(rcpputils REQUIRED) find_package(rmw_implementation_cmake REQUIRED) @@ -206,7 +208,7 @@ function(test_target_function) ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} mimick AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) diff --git a/rcl/test/mocking_utils/patch.hpp b/rcl/test/mocking_utils/patch.hpp new file mode 100644 index 000000000..544dec27a --- /dev/null +++ b/rcl/test/mocking_utils/patch.hpp @@ -0,0 +1,355 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Original file taken from: +// https://github.com/ros2/rcutils/blob/master/test/mocking_utils/patch.hpp + +#ifndef MOCKING_UTILS__PATCH_HPP_ +#define MOCKING_UTILS__PATCH_HPP_ + +#define MOCKING_UTILS_SUPPORT_VA_LIST +#if (defined(__aarch64__) || defined(__arm__) || defined(_M_ARM) || defined(__thumb__)) +// In ARM machines, va_list does not define comparison operators +// nor the compiler allows defining them via operator overloads. +// Thus, Mimick argument matching code will not compile. +#undef MOCKING_UTILS_SUPPORT_VA_LIST +#endif + +#ifdef MOCKING_UTILS_SUPPORT_VA_LIST +#include +#endif + +#include +#include +#include +#include + +#include "mimick/mimick.h" +#include "rcutils/macros.h" + +namespace mocking_utils +{ + +/// Mimick specific traits for each mocking_utils::Patch instance. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam SignatureT Type of the symbol to be patched. +*/ +template +struct PatchTraits; + +/// Traits specialization for ReturnT(void) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT); +}; + +/// Traits specialization for ReturnT(ArgT0) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgT0 Argument type. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); +}; + +/// Generic trampoline to wrap generalized callables in plain functions. +/** + * \tparam ID Numerical identifier of this trampoline. Ought to be unique. + * \tparam SignatureT Type of the symbol this trampoline replaces. + */ +template +struct Trampoline; + +/// Trampoline specialization for free functions. +template +struct Trampoline +{ + static ReturnT base(ArgTs... args) + { + return target(std::forward(args)...); + } + + static std::function target; +}; + +template +std::function +Trampoline::target; + +/// Setup trampoline with the given @p target. +/** + * \param[in] target Callable that this trampoline will target. + * \return the plain base function of this trampoline. + * + * \tparam ID Numerical identifier of this trampoline. Ought to be unique. + * \tparam SignatureT Type of the symbol this trampoline replaces. + */ +template +auto prepare_trampoline(std::function target) +{ + Trampoline::target = target; + return Trampoline::base; +} + +/// Patch class for binary API mocking +/** + * Built on top of Mimick, to enable symbol mocking on a per dynamically + * linked binary object basis. + * + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam SignatureT Type of the symbol to be patched. + */ +template +class Patch; + +/// Patch specialization for ReturnT(ArgTs...) free functions. +/** + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTs Argument types. + */ +template +class Patch +{ +public: + using mock_type = typename PatchTraits::mock_type; + + /// Construct a patch. + /** + * \param[in] target Symbol target string, using Mimick syntax + * i.e. "symbol(@scope)?", where scope may be "self" to target the current + * binary, "lib:library_name" to target a given library, "file:path/to/library" + * to target a given file, or "sym:other_symbol" to target the first library + * that defines said symbol. + * \param[in] proxy An indirection to call the target function. + * This indirection must ensure this call goes through the function's + * trampoline, as setup by the dynamic linker. + * \return a mocking_utils::Patch instance. + */ + explicit Patch(const std::string & target, std::function proxy) + : proxy_(proxy) + { + auto MMK_MANGLE(mock_type, create) = + PatchTraits::MMK_MANGLE(mock_type, create); + mock_ = mmk_mock(target.c_str(), mock_type); + } + + // Copy construction and assignment are disabled. + Patch(const Patch &) = delete; + Patch & operator=(const Patch &) = delete; + + Patch(Patch && other) + { + mock_ = other.mock_; + other.mock_ = nullptr; + } + + Patch & operator=(Patch && other) + { + if (mock_) { + mmk_reset(mock_); + } + mock_ = other.mock_; + other.mock_ = nullptr; + } + + ~Patch() + { + if (mock_) { + mmk_reset(mock_); + } + } + + /// Inject a @p replacement for the patched function. + Patch & then_call(std::function replacement) & + { + auto type_erased_trampoline = + reinterpret_cast(prepare_trampoline(replacement)); + mmk_when(proxy_(any()...), .then_call = type_erased_trampoline); + return *this; + } + + /// Inject a @p replacement for the patched function. + Patch && then_call(std::function replacement) && + { + auto type_erased_trampoline = + reinterpret_cast(prepare_trampoline(replacement)); + mmk_when(proxy_(any()...), .then_call = type_erased_trampoline); + return std::move(*this); + } + +private: + // Helper for template parameter pack expansion using `mmk_any` + // macro as pattern. + template + T any() {return mmk_any(T);} + + mock_type mock_; + std::function proxy_; +}; + +/// Make a patch for a `target` function. +/** + * Useful for type deduction during \ref mocking_utils::Patch construction. + * + * \param[in] target Symbol target string, using Mimick syntax. + * \param[in] proxy An indirection to call the target function. + * \return a mocking_utils::Patch instance. + * + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam SignatureT Type of the function to be patched. + * + * \sa mocking_utils::Patch for further reference. + */ +template +auto make_patch(const std::string & target, std::function proxy) +{ + return Patch(target, proxy); +} + +/// Define a dummy operator `op` for a given `type`. +/** + * Useful to enable patching functions that take arguments whose types + * do not define basic comparison operators, as required by Mimick. +*/ +#define MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(type_, op) \ + template \ + typename std::enable_if::value, bool>::type \ + operator op(const T &, const T &) { \ + return false; \ + } + +/// Get the exact \ref mocking_utils::Patch type for a given `id` and `function`. +/** + * Useful to avoid ignored attribute warnings when using the \b decltype operator. + */ +#define MOCKING_UTILS_PATCH_TYPE(id, function) \ + decltype(mocking_utils::make_patch("", nullptr)) + +/// A transparent forwarding proxy to a given `function`. +/** + * Useful to ensure a call to `function` goes through its trampoline. + */ +#define MOCKING_UTILS_PATCH_PROXY(function) \ + [] (auto && ... args)->decltype(auto) { \ + return function(std::forward(args)...); \ + } + +/// Compute a Mimick symbol target string based on which `function` is to be patched +/// in which `scope`. +#define MOCKING_UTILS_PATCH_TARGET(scope, function) \ + (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope)) + +/// Patch a `function` with a used-provided `replacement` in a given `scope`. +#define patch(scope, function, replacement) \ + make_patch<__COUNTER__, decltype(function)>( \ + MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \ + ).then_call(replacement) + +/// Patch a function with a function that only returns a value +#define patch_and_return(scope, function, return_value) \ + patch(scope, function, [&](auto && ...) {return return_value;}) + +} // namespace mocking_utils + +#ifdef MOCKING_UTILS_SUPPORT_VA_LIST +// Define dummy comparison operators for C standard va_list type +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, >) +#endif + +#endif // MOCKING_UTILS__PATCH_HPP_ diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index 40cc36c12..c536f9242 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/rcl/test/rcl/test_publisher.cpp @@ -21,11 +21,15 @@ #include "test_msgs/msg/strings.h" #include "rosidl_runtime_c/string_functions.h" -#include "./failing_allocator_functions.hpp" +#include "mimick/mimick.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" +#include "rmw/validate_full_topic_name.h" +#include "rmw/validate_node_name.h" +#include "./failing_allocator_functions.hpp" #include "./publisher_impl.h" +#include "../mocking_utils/patch.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -57,7 +61,7 @@ class CLASSNAME (TestPublisherFixture, RMW_IMPLEMENTATION) : public ::testing::T } this->node_ptr = new rcl_node_t; *this->node_ptr = rcl_get_zero_initialized_node(); - const char * name = "test_publisher_node"; + constexpr char name[] = "test_publisher_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -76,6 +80,34 @@ class CLASSNAME (TestPublisherFixture, RMW_IMPLEMENTATION) : public ::testing::T } }; +class CLASSNAME (TestPublisherFixtureInit, RMW_IMPLEMENTATION) + : public CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION) +{ +public: + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic_name = "chatter"; + rcl_publisher_t publisher; + rcl_publisher_options_t publisher_options; + + void SetUp() override + { + CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION) ::SetUp(); + publisher = rcl_get_zero_initialized_publisher(); + 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; + } + + void TearDown() override + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION) ::TearDown(); + } +}; + /* Basic nominal test of a publisher. */ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nominal) { @@ -83,8 +115,8 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - const char * topic_name = "chatter"; - const char * expected_topic_name = "/chatter"; + constexpr char topic_name[] = "chatter"; + constexpr char expected_topic_name[] = "/chatter"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -109,7 +141,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin 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"; + constexpr char topic_name[] = "chatter"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -187,7 +219,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_init_ rcl_publisher_t publisher; const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - const char * topic_name = "chatter"; + constexpr char topic_name[] = "chatter"; rcl_publisher_options_t default_publisher_options = rcl_publisher_get_default_options(); // Check if null publisher is valid @@ -216,6 +248,11 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_init_ EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; rcl_reset_error(); + // Pass nullptr publisher to fini + ret = rcl_publisher_fini(nullptr, this->node_ptr); + EXPECT_EQ(RCL_RET_PUBLISHER_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; @@ -305,7 +342,7 @@ 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"; + constexpr 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); @@ -337,7 +374,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_invalid_publish 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"; + constexpr 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); @@ -371,6 +408,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_invalid_publish 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; + rmw_publisher_allocation_t * null_allocation_is_valid_arg = nullptr; // Change internal context to nullptr publisher.impl->context = nullptr; @@ -391,14 +429,25 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_invalid_publish 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)); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, null_allocation_is_valid_arg)); rcl_reset_error(); EXPECT_EQ( RCL_RET_PUBLISHER_INVALID, - rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr)); + rcl_publish_serialized_message(&publisher, &serialized_msg, null_allocation_is_valid_arg)); rcl_reset_error(); publisher.impl->context = saved_context; + // nullptr arguments + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_publish(&publisher, nullptr, null_allocation_is_valid_arg)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_publish_serialized_message(&publisher, nullptr, null_allocation_is_valid_arg)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_publisher_get_subscription_count(&publisher, nullptr)); + rcl_reset_error(); + // Change internal rmw_handle to nullptr publisher.impl->rmw_handle = nullptr; EXPECT_FALSE(rcl_publisher_is_valid_except_context(&publisher)); @@ -422,11 +471,11 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_invalid_publish 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)); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, null_allocation_is_valid_arg)); rcl_reset_error(); EXPECT_EQ( RCL_RET_PUBLISHER_INVALID, - rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr)); + rcl_publish_serialized_message(&publisher, &serialized_msg, null_allocation_is_valid_arg)); rcl_reset_error(); publisher.impl->rmw_handle = saved_rmw_handle; @@ -453,11 +502,11 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_invalid_publish 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)); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, null_allocation_is_valid_arg)); rcl_reset_error(); EXPECT_EQ( RCL_RET_PUBLISHER_INVALID, - rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr)); + rcl_publish_serialized_message(&publisher, &serialized_msg, null_allocation_is_valid_arg)); rcl_reset_error(); publisher.impl = saved_impl; @@ -483,10 +532,310 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_invalid_publish 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)); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(nullptr, &msg, null_allocation_is_valid_arg)); rcl_reset_error(); EXPECT_EQ( RCL_RET_PUBLISHER_INVALID, - rcl_publish_serialized_message(nullptr, &serialized_msg, nullptr)); + rcl_publish_serialized_message(nullptr, &serialized_msg, null_allocation_is_valid_arg)); + rcl_reset_error(); +} + +// Mocking rmw_publisher_count_matched_subscriptions to make +// rcl_publisher_get_subscription_count fail +TEST_F( + CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), + test_mock_publisher_get_subscription_count) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publisher_count_matched_subscriptions, RMW_RET_BAD_ALLOC); + + // Now normal usage of the function rcl_publisher_get_subscription_count returning + // unexpected RMW_RET_BAD_ALLOC + size_t count_size = 2u; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_publisher_get_subscription_count(&publisher, &count_size)); + EXPECT_EQ(2u, count_size); + rcl_reset_error(); +} + +// Mocking rmw_publisher_assert_liveliness to make +// rcl_publisher_assert_liveliness fail +TEST_F(CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), test_mock_assert_liveliness) { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publisher_assert_liveliness, RMW_RET_ERROR); + + // Now normal usage of the function rcl_publisher_assert_liveliness returning + // unexpected RMW_RET_ERROR + EXPECT_EQ( + RCL_RET_ERROR, rcl_publisher_assert_liveliness(&publisher)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +// Mocking rmw_publish to make rcl_publish fail +TEST_F(CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), test_mock_publish) { + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_publish, RMW_RET_ERROR); + + // Test normal usage of the function rcl_publish returning unexpected RMW_RET_ERROR + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int64_value = 42; + rcl_ret_t ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__BasicTypes__fini(&msg); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +// Mocking rmw_publish_serialized_message to make rcl_publish_serialized_message fail +TEST_F( + CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), test_mock_publish_serialized_message) +{ + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + size_t initial_size_serialized = 0u; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg, initial_size_serialized, &allocator)) << rcl_get_error_string().str; + constexpr char test_string[] = "testing"; + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ASSERT_STREQ(msg.string_value.data, test_string); + rcl_ret_t ret = rmw_serialize(&msg, ts, &serialized_msg); + ASSERT_EQ(RMW_RET_OK, ret); + + rmw_ret_t rmw_publish_serialized_return = RMW_RET_ERROR; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publish_serialized_message, rmw_publish_serialized_return); + { + // Test normal usage of the function rcl_publish_serialized_message + // returning unexpected RMW_RET_ERROR + ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + // Repeat, but now returning BAD_ALLOC + rmw_publish_serialized_return = RMW_RET_BAD_ALLOC; + ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} + +// Define dummy comparison operators for rcutils_allocator_t type for use with the Mimick Library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) + +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mock_publisher_init) { + 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); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = RCL_RET_OK; + + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_string_map_init, RCUTILS_RET_ERROR); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F( + CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mock_publisher_init_fail_qos) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publisher_get_actual_qos, RMW_RET_ERROR); + + 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); + constexpr 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); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; rcl_reset_error(); } + +// Tests for loaned msgs functions. Mocked as the rmw tier1 vendors don't support it +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mock_loaned_functions) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_t not_init_publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "chatter"; + constexpr char expected_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( + { + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + EXPECT_EQ(strcmp(rcl_publisher_get_topic_name(&publisher), expected_topic_name), 0); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int64_value = 42; + void * msg_pointer = &msg; + rmw_publisher_allocation_t * null_allocation_is_valid_arg = nullptr; + + { + // mocked, publish nominal usage + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_publish_loaned_message, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_publish_loaned_message(&publisher, &msg, nullptr)); + } + { + // bad params publish + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_loaned_message(nullptr, &msg, null_allocation_is_valid_arg)); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_loaned_message(¬_init_publisher, &msg, null_allocation_is_valid_arg)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_publish_loaned_message(&publisher, nullptr, null_allocation_is_valid_arg)); + } + { + // mocked, failure publish + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publish_loaned_message, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_publish_loaned_message(&publisher, &msg, nullptr)); + } + { + // mocked, borrow loaned nominal usage + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_borrow_loaned_message, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_borrow_loaned_message(&publisher, ts, &msg_pointer)); + } + { + // bad params borrow loaned + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_borrow_loaned_message(nullptr, ts, &msg_pointer)); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_borrow_loaned_message(¬_init_publisher, ts, &msg_pointer)); + } + { + // mocked, nominal return loaned message + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_return_loaned_message_from_publisher, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_return_loaned_message_from_publisher(&publisher, &msg)); + } + { + // bad params return loaned message + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_return_loaned_message_from_publisher(nullptr, &msg)); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_return_loaned_message_from_publisher(¬_init_publisher, &msg)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_return_loaned_message_from_publisher(&publisher, nullptr)); + } + + test_msgs__msg__BasicTypes__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +// Tests mocking ini/fini functions for specific failures +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mocks_fail_publisher_init) { + 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); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = RCL_RET_OK; + + { + // Internal rmw failure validating node name + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_validate_node_name, RMW_RET_ERROR); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal rmw failure validating node name + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal failure when fini rcutils_string_map returns error, targets substitution_map fini + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_string_map_fini, RCUTILS_RET_ERROR); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal failure when fini rcutils_string_map returns error, targets rcl_remap_topic_name + auto mock = mocking_utils::patch( + "lib:rcl", rcutils_string_map_init, [](auto...) { + static int counter = 1; + if (counter == 1) { + counter++; + return RCUTILS_RET_OK; + } else { + // This makes rcl_remap_topic_name fail + return RCUTILS_RET_ERROR; + } + }); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal rmw failure validating topic name + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_full_topic_name, RMW_RET_ERROR); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal rmw failure validating node name, returns OK but the result is set to error + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_full_topic_name, [](auto, int * result, auto) { + *result = RMW_TOPIC_INVALID_NOT_ABSOLUTE; + return RMW_RET_OK; + }); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } +} + +// Test mocked fail fini publisher +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mock_publisher_fini_fail) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr 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; + + // Internal rmw failure destroying publisher + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_destroy_publisher, RMW_RET_ERROR); + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; +} From 47327f8205a6e88a8d87c982b30f1849c87bdec8 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 11 Aug 2020 10:44:02 -0300 Subject: [PATCH 10/42] Add missing calls to rcl_convert_rmw_ret_to_rcl_ret (#738) Signed-off-by: Jorge Perez --- rcl/src/rcl/publisher.c | 6 ++++-- rcl/src/rcl/subscription.c | 20 ++++++-------------- 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 676dc0332..83f433c52 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -272,7 +272,8 @@ rcl_borrow_loaned_message( if (!rcl_publisher_is_valid(publisher)) { return RCL_RET_PUBLISHER_INVALID; // error already set } - return rmw_borrow_loaned_message(publisher->impl->rmw_handle, type_support, ros_message); + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_borrow_loaned_message(publisher->impl->rmw_handle, type_support, ros_message)); } rcl_ret_t @@ -284,7 +285,8 @@ rcl_return_loaned_message_from_publisher( return RCL_RET_PUBLISHER_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); - return rmw_return_loaned_message_from_publisher(publisher->impl->rmw_handle, loaned_message); + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_return_loaned_message_from_publisher(publisher->impl->rmw_handle, loaned_message)); } rcl_ret_t diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 9dd9ed8b0..04b076a89 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -281,10 +281,7 @@ rcl_take( subscription->impl->rmw_handle, ros_message, &taken, message_info_local, allocation); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - if (RMW_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Subscription take succeeded: %s", taken ? "true" : "false"); @@ -363,10 +360,7 @@ rcl_take_serialized_message( subscription->impl->rmw_handle, serialized_message, &taken, message_info_local, allocation); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - if (RMW_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Subscription serialized take succeeded: %s", taken ? "true" : "false"); @@ -402,10 +396,7 @@ rcl_take_loaned_message( subscription->impl->rmw_handle, loaned_message, &taken, message_info_local, allocation); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - if (RMW_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Subscription loaned take succeeded: %s", taken ? "true" : "false"); @@ -425,8 +416,9 @@ rcl_return_loaned_message_from_subscription( return RCL_RET_SUBSCRIPTION_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); - return rmw_return_loaned_message_from_subscription( - subscription->impl->rmw_handle, loaned_message); + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_return_loaned_message_from_subscription( + subscription->impl->rmw_handle, loaned_message)); } const char * From e485daeca090159300b3f548d8d08562488ba554 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 11 Aug 2020 17:06:48 -0300 Subject: [PATCH 11/42] Add deallocate calls to free strdup allocated memory (#737) * Add deallocate calls to free strdup allocated memory * Add variables to know if free is required * Reformat not use extra booleans * Address peer review comments Signed-off-by: Jorge Perez --- .../rcl/rmw_implementation_identifier_check.c | 29 +++++++++++++------ 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/rcl/src/rcl/rmw_implementation_identifier_check.c b/rcl/src/rcl/rmw_implementation_identifier_check.c index 9c068a67f..cc2414418 100644 --- a/rcl/src/rcl/rmw_implementation_identifier_check.c +++ b/rcl/src/rcl/rmw_implementation_identifier_check.c @@ -59,6 +59,7 @@ rcl_ret_t rcl_rmw_implementation_identifier_check(void) // If the environment variable RMW_IMPLEMENTATION is set, or // the environment variable RCL_ASSERT_RMW_ID_MATCHES is set, // check that the result of `rmw_get_implementation_identifier` matches. + rcl_ret_t ret = RCL_RET_OK; rcl_allocator_t allocator = rcl_get_default_allocator(); char * expected_rmw_impl = NULL; const char * expected_rmw_impl_env = NULL; @@ -89,14 +90,16 @@ rcl_ret_t rcl_rmw_implementation_identifier_check(void) "Error getting env var '" RCUTILS_STRINGIFY(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME) "': %s\n", get_env_error_str); - return RCL_RET_ERROR; + ret = RCL_RET_ERROR; + goto cleanup; } if (strlen(asserted_rmw_impl_env) > 0) { // Copy the environment variable so it doesn't get over-written by the next getenv call. asserted_rmw_impl = rcutils_strdup(asserted_rmw_impl_env, allocator); if (!asserted_rmw_impl) { RCL_SET_ERROR_MSG("allocation failed"); - return RCL_RET_BAD_ALLOC; + ret = RCL_RET_BAD_ALLOC; + goto cleanup; } } @@ -107,7 +110,8 @@ rcl_ret_t rcl_rmw_implementation_identifier_check(void) "variables do not match, exiting with %d.", expected_rmw_impl, asserted_rmw_impl, RCL_RET_ERROR ); - return RCL_RET_ERROR; + ret = RCL_RET_ERROR; + goto cleanup; } // Collapse the expected_rmw_impl and asserted_rmw_impl variables so only expected_rmw_impl needs @@ -115,12 +119,14 @@ rcl_ret_t rcl_rmw_implementation_identifier_check(void) if (expected_rmw_impl && asserted_rmw_impl) { // The strings at this point must be equal. // No need for asserted_rmw_impl anymore, free the memory. - allocator.deallocate((char *)asserted_rmw_impl, allocator.state); + allocator.deallocate(asserted_rmw_impl, allocator.state); + asserted_rmw_impl = NULL; } else { // One or none are set. // If asserted_rmw_impl has contents, move it over to expected_rmw_impl. if (asserted_rmw_impl) { expected_rmw_impl = asserted_rmw_impl; + asserted_rmw_impl = NULL; } } @@ -137,7 +143,8 @@ rcl_ret_t rcl_rmw_implementation_identifier_check(void) rmw_error_msg.str, RCL_RET_ERROR ); - return RCL_RET_ERROR; + ret = RCL_RET_ERROR; + goto cleanup; } if (strcmp(actual_rmw_impl_id, expected_rmw_impl) != 0) { RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -146,12 +153,16 @@ rcl_ret_t rcl_rmw_implementation_identifier_check(void) actual_rmw_impl_id, RCL_RET_MISMATCHED_RMW_ID ); - return RCL_RET_MISMATCHED_RMW_ID; + ret = RCL_RET_MISMATCHED_RMW_ID; + goto cleanup; } - // Free the memory now that all checking has passed. - allocator.deallocate((char *)expected_rmw_impl, allocator.state); } - return RCL_RET_OK; + ret = RCL_RET_OK; +// fallthrough +cleanup: + allocator.deallocate(expected_rmw_impl, allocator.state); + allocator.deallocate(asserted_rmw_impl, allocator.state); + return ret; } INITIALIZER(initialize) { From c595ca18eff4f41aae7d4db5b68e36fdb4d64097 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 12 Aug 2020 17:30:44 -0300 Subject: [PATCH 12/42] Complete subscription API test coverage (#734) Signed-off-by: Michel Hidalgo --- rcl/package.xml | 8 +- rcl/test/CMakeLists.txt | 3 +- rcl/test/mocking_utils/patch.hpp | 55 ++-- rcl/test/rcl/test_subscription.cpp | 438 +++++++++++++++++++++++++---- 4 files changed, 421 insertions(+), 83 deletions(-) diff --git a/rcl/package.xml b/rcl/package.xml index c40622334..4dbc85cdb 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -25,14 +25,14 @@ ament_cmake_pytest ament_lint_auto ament_lint_common - mimick_vendor - rcpputils - rmw - rmw_implementation_cmake launch launch_testing launch_testing_ament_cmake + mimick_vendor osrf_testing_tools_cpp + rcpputils + rmw + rmw_implementation_cmake test_msgs rcl_logging_packages diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index bd04416f7..411bcbfde 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -1,6 +1,7 @@ find_package(ament_cmake_gtest REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) +find_package(mimick_vendor REQUIRED) find_package(test_msgs REQUIRED) find_package(mimick_vendor REQUIRED) @@ -224,7 +225,7 @@ function(test_target_function) SRCS rcl/test_subscription.cpp rcl/wait_for_entity_helpers.cpp ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} mimick AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) if(rmw_implementation STREQUAL "rmw_fastrtps_cpp" OR diff --git a/rcl/test/mocking_utils/patch.hpp b/rcl/test/mocking_utils/patch.hpp index 544dec27a..d170c7fa3 100644 --- a/rcl/test/mocking_utils/patch.hpp +++ b/rcl/test/mocking_utils/patch.hpp @@ -219,11 +219,8 @@ class Patch * \return a mocking_utils::Patch instance. */ explicit Patch(const std::string & target, std::function proxy) - : proxy_(proxy) + : target_(target), proxy_(proxy) { - auto MMK_MANGLE(mock_type, create) = - PatchTraits::MMK_MANGLE(mock_type, create); - mock_ = mmk_mock(target.c_str(), mock_type); } // Copy construction and assignment are disabled. @@ -255,18 +252,14 @@ class Patch /// Inject a @p replacement for the patched function. Patch & then_call(std::function replacement) & { - auto type_erased_trampoline = - reinterpret_cast(prepare_trampoline(replacement)); - mmk_when(proxy_(any()...), .then_call = type_erased_trampoline); + replace_with(replacement); return *this; } /// Inject a @p replacement for the patched function. Patch && then_call(std::function replacement) && { - auto type_erased_trampoline = - reinterpret_cast(prepare_trampoline(replacement)); - mmk_when(proxy_(any()...), .then_call = type_erased_trampoline); + replace_with(replacement); return std::move(*this); } @@ -276,7 +269,21 @@ class Patch template T any() {return mmk_any(T);} - mock_type mock_; + void replace_with(std::function replacement) + { + if (mock_) { + throw std::logic_error("Cannot configure patch more than once"); + } + auto type_erased_trampoline = + reinterpret_cast(prepare_trampoline(replacement)); + auto MMK_MANGLE(mock_type, create) = + PatchTraits::MMK_MANGLE(mock_type, create); + mock_ = mmk_mock(target_.c_str(), mock_type); + mmk_when(proxy_(any()...), .then_call = type_erased_trampoline); + } + + mock_type mock_{nullptr}; + std::string target_; std::function proxy_; }; @@ -332,15 +339,29 @@ auto make_patch(const std::string & target, std::function proxy) #define MOCKING_UTILS_PATCH_TARGET(scope, function) \ (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope)) -/// Patch a `function` with a used-provided `replacement` in a given `scope`. -#define patch(scope, function, replacement) \ +/// Prepare a mocking_utils::Patch for patching a `function` in a given `scope` +/// but defer applying any changes. +#define prepare_patch(scope, function) \ make_patch<__COUNTER__, decltype(function)>( \ MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \ - ).then_call(replacement) + ) -/// Patch a function with a function that only returns a value -#define patch_and_return(scope, function, return_value) \ - patch(scope, function, [&](auto && ...) {return return_value;}) +/// Patch a `function` with a used-provided `replacement` in a given `scope`. +#define patch(scope, function, replacement) \ + prepare_patch(scope, function).then_call(replacement) + +/// Patch a `function` to always yield a given `return_code` in a given `scope`. +#define patch_and_return(scope, function, return_code) \ + patch(scope, function, [&](auto && ...) {return return_code;}) + +/// Patch a `function` to execute normally but always yield a given `return_code` +/// in a given `scope`. +#define inject_on_return(scope, function, return_code) \ + patch( \ + scope, function, ([&, base = function](auto && ... __args) { \ + static_cast(base(std::forward(__args)...)); \ + return return_code; \ + })) } // namespace mocking_utils diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 2dcbd4490..1e5b8c779 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -20,6 +20,8 @@ #include "rcl/subscription.h" #include "rcl/rcl.h" +#include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" #include "test_msgs/msg/basic_types.h" #include "test_msgs/msg/strings.h" @@ -30,6 +32,7 @@ #include "wait_for_entity_helpers.hpp" #include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -61,7 +64,7 @@ class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing } this->node_ptr = new rcl_node_t; *this->node_ptr = rcl_get_zero_initialized_node(); - const char * name = "test_subscription_node"; + constexpr char name[] = "test_subscription_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -122,8 +125,8 @@ TEST_F( const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - const char * topic = "chatter"; - const char * expected_topic = "/chatter"; + constexpr char topic[] = "chatter"; + constexpr char expected_topic[] = "/chatter"; rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); @@ -143,22 +146,33 @@ TEST_F( rcl_reset_error(); } +// Define dummy comparison operators for rcutils_allocator_t type +// to use with the Mimick mocking library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + // 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"; + constexpr 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)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_node_get_rmw_handle(&invalid_node)); + rcl_reset_error(); 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)); @@ -173,9 +187,57 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription 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); + { + rcutils_ret_t rcutils_string_map_init_returns = RCUTILS_RET_BAD_ALLOC; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_string_map_init, rcutils_string_map_init_returns); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcl_reset_error(); + + rcutils_string_map_init_returns = RCUTILS_RET_ERROR; + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + { + auto mock = + mocking_utils::inject_on_return("lib:rcl", rcutils_string_map_fini, RCUTILS_RET_ERROR); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + { + rmw_ret_t rmw_validate_full_topic_name_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_full_topic_name, + [&](auto, int * result, auto) { + *result = RMW_TOPIC_INVALID_TOO_LONG; + return rmw_validate_full_topic_name_returns; + }); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret); + rcl_reset_error(); + + rmw_validate_full_topic_name_returns = RMW_RET_ERROR; + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + { + auto mock = + mocking_utils::patch_and_return("lib:rcl", rmw_create_subscription, nullptr); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + { + auto mock = + mocking_utils::patch_and_return("lib:rcl", rmw_subscription_get_actual_qos, RMW_RET_ERROR); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -184,13 +246,20 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_EQ(RCL_RET_SUBSCRIPTION_INVALID, rcl_subscription_fini(nullptr, this->node_ptr)); + 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; + auto mock = + mocking_utils::inject_on_return("lib:rcl", rmw_destroy_subscription, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_subscription_fini(&subscription, this->node_ptr)); + rcl_reset_error(); + + // Make sure finalization completed anyways. + ASSERT_EQ(NULL, subscription.impl); } /* Basic nominal test of a subscription @@ -200,7 +269,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - const char * topic = "/chatter"; + constexpr char topic[] = "/chatter"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -273,7 +342,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription 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 = "rcl_test_subscription_nominal_string_chatter"; + constexpr char topic[] = "rcl_test_subscription_nominal_string_chatter"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -292,7 +361,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); - const char * test_string = "testing"; + constexpr char test_string[] = "testing"; { test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -325,7 +394,7 @@ TEST_F( 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 = "rcl_test_subscription_nominal_string_sequence_chatter"; + constexpr char topic[] = "rcl_test_subscription_nominal_string_sequence_chatter"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -344,7 +413,7 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); - const char * test_string = "testing"; + constexpr char test_string[] = "testing"; { test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -470,7 +539,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcutils_allocator_t allocator = rcl_get_default_allocator(); const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); - const char * topic = "/chatterSer"; + constexpr char topic[] = "/chatterSer"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -485,7 +554,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription ASSERT_EQ( RCL_RET_OK, rmw_serialized_message_init( &serialized_msg, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; - const char * test_string = "testing"; + constexpr char test_string[] = "testing"; test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); @@ -535,7 +604,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription 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 = "rcl_loan"; + constexpr char topic[] = "rcl_loan"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -565,36 +634,190 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription } ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); { - test_msgs__msg__Strings msg; - test_msgs__msg__Strings * msg_loaned; - test_msgs__msg__Strings__init(&msg); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - test_msgs__msg__Strings__fini(&msg); - }); - - // Only if rmw supports the functionality - if (rcl_subscription_can_loan_messages(&subscription)) { - ret = rcl_take_loaned_message( - &subscription, reinterpret_cast(&msg_loaned), nullptr, nullptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_EQ( - std::string(test_string), - std::string(msg_loaned->string_value.data, msg_loaned->string_value.size)); - } else { - ret = rcl_take(&subscription, &msg, nullptr, nullptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_EQ( - std::string(test_string), std::string(msg.string_value.data, msg.string_value.size)); + auto patch_take = + mocking_utils::prepare_patch("lib:rcl", rmw_take_loaned_message_with_info); + auto patch_return = + mocking_utils::prepare_patch("lib:rcl", rmw_return_loaned_message_from_subscription); + + if (!rcl_subscription_can_loan_messages(&subscription)) { + // If rcl (and ultimately rmw) does not support message loaning, + // mock it so that a unit test can still be constructed. + patch_take.then_call( + [](auto sub, void ** loaned_message, auto taken, auto msg_info, auto allocation) { + auto msg = new(std::nothrow) test_msgs__msg__Strings{}; + if (!msg) { + return RMW_RET_BAD_ALLOC; + } + test_msgs__msg__Strings__init(msg); + *loaned_message = static_cast(msg); + rmw_ret_t ret = rmw_take_with_info(sub, *loaned_message, taken, msg_info, allocation); + if (RMW_RET_OK != ret) { + delete msg; + } + return ret; + }); + patch_return.then_call( + [](auto, void * loaned_message) { + auto msg = reinterpret_cast(loaned_message); + test_msgs__msg__Strings__fini(msg); + delete msg; + + return RMW_RET_OK; + }); } + + test_msgs__msg__Strings * msg_loaned = nullptr; + ret = rcl_take_loaned_message( + &subscription, reinterpret_cast(&msg_loaned), nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ( + std::string(test_string), + std::string(msg_loaned->string_value.data, msg_loaned->string_value.size)); + ret = rcl_return_loaned_message_from_subscription(&subscription, msg_loaned); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +} + +/* Test for all failure modes in subscription take with loaned messages function. + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_bad_take_loaned_message) { + constexpr char topic[] = "rcl_loan"; + const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rmw_ret_t ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, topic, + &subscription_options); + ASSERT_EQ(RMW_RET_OK, ret) << rcl_get_error_string().str; + + test_msgs__msg__Strings * loaned_message = nullptr; + void ** type_erased_loaned_message_pointer = reinterpret_cast(&loaned_message); + rmw_message_info_t * message_info = nullptr; // is a valid argument + rmw_subscription_allocation_t * allocation = nullptr; // is a valid argument + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, rcl_take_loaned_message( + nullptr, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_loaned_message(&subscription, nullptr, message_info, allocation)); + rcl_reset_error(); + + test_msgs__msg__Strings dummy_message; + loaned_message = &dummy_message; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + loaned_message = nullptr; + + { + rmw_ret_t rmw_take_loaned_message_with_info_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_loaned_message_with_info, + [&](auto, auto, bool * taken, auto && ...) { + *taken = false; + return rmw_take_loaned_message_with_info_returns; + }); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_TAKE_FAILED, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + + rmw_take_loaned_message_with_info_returns = RMW_RET_BAD_ALLOC; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + + rmw_take_loaned_message_with_info_returns = RMW_RET_UNSUPPORTED; + EXPECT_EQ( + RCL_RET_UNSUPPORTED, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + + rmw_take_loaned_message_with_info_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_fini(&subscription, this->node_ptr)) << rcl_get_error_string().str; +} + +/* Test for all failure modes in subscription return loaned messages function. + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_bad_return_loaned_message) { + constexpr char topic[] = "rcl_loan"; + const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + test_msgs__msg__Strings dummy_message; + test_msgs__msg__Strings__init(&dummy_message); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&dummy_message); + }); + void * loaned_message = &dummy_message; + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_return_loaned_message_from_subscription(nullptr, loaned_message)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_return_loaned_message_from_subscription(&subscription, loaned_message)); + rcl_reset_error(); + + rmw_ret_t ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, topic, + &subscription_options); + ASSERT_EQ(RMW_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_return_loaned_message_from_subscription(&subscription, nullptr)); + rcl_reset_error(); + + { + rmw_ret_t rmw_return_loaned_message_from_subscription_returns = RMW_RET_OK; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_return_loaned_message_from_subscription, + rmw_return_loaned_message_from_subscription_returns); + + EXPECT_EQ( + RCL_RET_OK, rcl_return_loaned_message_from_subscription(&subscription, loaned_message)) << + rcl_get_error_string().str; + + rmw_return_loaned_message_from_subscription_returns = RMW_RET_UNSUPPORTED; + EXPECT_EQ( + RCL_RET_UNSUPPORTED, rcl_return_loaned_message_from_subscription( + &subscription, + loaned_message)); + rcl_reset_error(); + + rmw_return_loaned_message_from_subscription_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, rcl_return_loaned_message_from_subscription(&subscription, loaned_message)); + rcl_reset_error(); + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_fini(&subscription, this->node_ptr)) << rcl_get_error_string().str; } TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) { rcl_ret_t ret; const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); - const char * topic = "test_get_options"; + constexpr char topic[] = "test_get_options"; rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); @@ -629,9 +852,27 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip RCL_RET_SUBSCRIPTION_INVALID, rcl_take(&subscription_zero_init, &msg, &message_info, nullptr)); rcl_reset_error(); + rmw_ret_t rmw_take_with_info_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_with_info, + [&](auto, auto, bool * taken, auto...) { + *taken = false; + return rmw_take_with_info_returns; + }); + EXPECT_EQ( RCL_RET_SUBSCRIPTION_TAKE_FAILED, rcl_take(&subscription, &msg, &message_info, nullptr)); rcl_reset_error(); + + rmw_take_with_info_returns = RMW_RET_BAD_ALLOC; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_take(&subscription, &msg, &message_info, nullptr)); + rcl_reset_error(); + + rmw_take_with_info_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, rcl_take(&subscription, &msg, &message_info, nullptr)); + rcl_reset_error(); } /* bad take_serialized @@ -640,23 +881,53 @@ 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; + size_t initial_serialization_capacity = 0u; ASSERT_EQ( RCL_RET_OK, rmw_serialized_message_init( - &serialized_msg, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; + &serialized_msg, initial_serialization_capacity, &allocator)) << + rcl_get_error_string().str; + rmw_message_info_t * message_info = nullptr; // is a valid argument + rmw_subscription_allocation_t * allocation = nullptr; // is a valid argument EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_take_serialized_message(nullptr, &serialized_msg, nullptr, nullptr)); + rcl_take_serialized_message(nullptr, &serialized_msg, message_info, allocation)); rcl_reset_error(); EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_take_serialized_message(&subscription_zero_init, &serialized_msg, nullptr, nullptr)); + rcl_take_serialized_message( + &subscription_zero_init, &serialized_msg, + message_info, allocation)); rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_serialized_message(&subscription, nullptr, message_info, allocation)); + rcl_reset_error(); + + rmw_ret_t rmw_take_serialized_message_with_info_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_serialized_message_with_info, + [&](auto, auto, bool * taken, auto...) { + *taken = false; + return rmw_take_serialized_message_with_info_returns; + }); + EXPECT_EQ( RCL_RET_SUBSCRIPTION_TAKE_FAILED, - rcl_take_serialized_message(&subscription, &serialized_msg, nullptr, nullptr)); + rcl_take_serialized_message(&subscription, &serialized_msg, message_info, allocation)); + rcl_reset_error(); + + rmw_take_serialized_message_with_info_returns = RMW_RET_BAD_ALLOC; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, + rcl_take_serialized_message(&subscription, &serialized_msg, message_info, allocation)); + rcl_reset_error(); + + rmw_take_serialized_message_with_info_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, + rcl_take_serialized_message(&subscription, &serialized_msg, message_info, allocation)); rcl_reset_error(); } @@ -686,43 +957,92 @@ TEST_F( { EXPECT_EQ(RMW_RET_OK, rmw_message_info_sequence_fini(&message_infos)); }); + rmw_subscription_allocation_t * allocation = nullptr; // is a valid argument EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_take_sequence(nullptr, seq_size, &messages, &message_infos, nullptr)); + rcl_take_sequence(nullptr, seq_size, &messages, &message_infos, allocation)); rcl_reset_error(); EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_take_sequence(&subscription_zero_init, seq_size, &messages, &message_infos, nullptr)); + rcl_take_sequence(&subscription_zero_init, seq_size, &messages, &message_infos, allocation)); rcl_reset_error(); EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, - rcl_take_sequence(&subscription, seq_size + 1, &messages, &message_infos, nullptr)); + rcl_take_sequence(&subscription, seq_size + 1, &messages, &message_infos, allocation)); rcl_reset_error(); EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, - rcl_take_sequence(&subscription, seq_size, &messages, &message_infos_short, nullptr)); + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos_short, allocation)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_sequence(&subscription, seq_size, nullptr, &message_infos, allocation)); 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_INVALID_ARGUMENT, + rcl_take_sequence(&subscription, seq_size, &messages, nullptr, allocation)); + rcl_reset_error(); + + rmw_ret_t rmw_take_sequence_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_sequence, + [&](auto, auto, auto, auto, size_t * taken, auto) { + *taken = 0u; + return rmw_take_sequence_returns; + }); + EXPECT_EQ( RCL_RET_SUBSCRIPTION_TAKE_FAILED, - rcl_take_sequence(&subscription, seq_size, &messages, &message_infos, nullptr)); + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos, allocation)); + rcl_reset_error(); + + rmw_take_sequence_returns = RMW_RET_BAD_ALLOC; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos, allocation)); + rcl_reset_error(); + + rmw_take_sequence_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos, allocation)); rcl_reset_error(); - */ } -/* Using bad arguments subscription methods +/* Test for all failure modes in subscription get_publisher_count function. */ -TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscription_bad_argument) { - size_t pub_count = 0; +TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_bad_get_publisher_count) { + size_t publisher_count = 0; + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_publisher_count(nullptr, &publisher_count)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_publisher_count(&subscription_zero_init, &publisher_count)); + rcl_reset_error(); EXPECT_EQ( - RCL_RET_SUBSCRIPTION_INVALID, rcl_subscription_get_publisher_count(nullptr, &pub_count)); + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_get_publisher_count(&subscription, nullptr)); rcl_reset_error(); + + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_count_matched_publishers, RMW_RET_ERROR); + EXPECT_EQ( + RCL_RET_ERROR, + rcl_subscription_get_publisher_count(&subscription, &publisher_count)); + rcl_reset_error(); +} + +/* Using bad arguments subscription methods + */ +TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscription_bad_argument) { EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(nullptr)); rcl_reset_error(); EXPECT_FALSE(rcl_subscription_can_loan_messages(nullptr)); @@ -734,10 +1054,6 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip 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)); From f51f8e54161282cb29286d69c380498f12e0c9aa Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 13 Aug 2020 09:36:14 -0300 Subject: [PATCH 13/42] Fix bug error handling get_param_files (#743) * Add bomb allocator * Fix memory checked for error * Add tests for bad alloc * Fix break statement * Add static keyword to methods added Signed-off-by: Jorge Perez --- rcl/src/rcl/arguments.c | 5 +- rcl/test/rcl/allocator_testing_utils.h | 74 ++++++++++++++++++++++++++ rcl/test/rcl/test_arguments.cpp | 36 +++++++++++++ 3 files changed, 111 insertions(+), 4 deletions(-) diff --git a/rcl/src/rcl/arguments.c b/rcl/src/rcl/arguments.c index cc2b46fc5..950d08d38 100644 --- a/rcl/src/rcl/arguments.c +++ b/rcl/src/rcl/arguments.c @@ -93,10 +93,7 @@ rcl_arguments_get_param_files( if (NULL == (*parameter_files)[i]) { // deallocate allocated memory for (int r = i; r >= 0; --r) { - if (NULL == (*parameter_files[r])) { - break; - } - allocator.deallocate((*parameter_files[r]), allocator.state); + allocator.deallocate((*parameter_files)[r], allocator.state); } allocator.deallocate((*parameter_files), allocator.state); (*parameter_files) = NULL; diff --git a/rcl/test/rcl/allocator_testing_utils.h b/rcl/test/rcl/allocator_testing_utils.h index cdc707d68..f3a0d7fcb 100644 --- a/rcl/test/rcl/allocator_testing_utils.h +++ b/rcl/test/rcl/allocator_testing_utils.h @@ -87,6 +87,80 @@ set_failing_allocator_is_failing(rcutils_allocator_t & failing_allocator, bool s ((__failing_allocator_state *)failing_allocator.state)->is_failing = state; } +typedef struct time_bomb_allocator_state +{ + int count_until_failure; +} time_bomb_allocator_state; + +static void * time_bomb_malloc(size_t size, void * state) +{ + time_bomb_allocator_state * time_bomb_state = (time_bomb_allocator_state *)state; + if (time_bomb_state->count_until_failure >= 0 && + time_bomb_state->count_until_failure-- == 0) + { + return nullptr; + } + return rcutils_get_default_allocator().allocate(size, rcutils_get_default_allocator().state); +} + +static void * +time_bomb_realloc(void * pointer, size_t size, void * state) +{ + time_bomb_allocator_state * time_bomb_state = (time_bomb_allocator_state *)state; + if (time_bomb_state->count_until_failure >= 0 && + time_bomb_state->count_until_failure-- == 0) + { + return nullptr; + } + return rcutils_get_default_allocator().reallocate( + pointer, size, rcutils_get_default_allocator().state); +} + +static void +time_bomb_free(void * pointer, void * state) +{ + time_bomb_allocator_state * time_bomb_state = (time_bomb_allocator_state *)state; + if (time_bomb_state->count_until_failure >= 0 && + time_bomb_state->count_until_failure-- == 0) + { + return; + } + rcutils_get_default_allocator().deallocate(pointer, rcutils_get_default_allocator().state); +} + +static void * +time_bomb_calloc(size_t number_of_elements, size_t size_of_element, void * state) +{ + time_bomb_allocator_state * time_bomb_state = (time_bomb_allocator_state *)state; + if (time_bomb_state->count_until_failure >= 0 && + time_bomb_state->count_until_failure-- == 0) + { + return nullptr; + } + return rcutils_get_default_allocator().zero_allocate( + number_of_elements, size_of_element, rcutils_get_default_allocator().state); +} + +static inline rcutils_allocator_t +get_time_bombed_allocator(void) +{ + static time_bomb_allocator_state state; + state.count_until_failure = 1; + auto time_bombed_allocator = rcutils_get_default_allocator(); + time_bombed_allocator.allocate = time_bomb_malloc; + time_bombed_allocator.deallocate = time_bomb_free; + time_bombed_allocator.reallocate = time_bomb_realloc; + time_bombed_allocator.zero_allocate = time_bomb_calloc; + time_bombed_allocator.state = &state; + return time_bombed_allocator; +} + +static inline void +set_time_bombed_allocator_count(rcutils_allocator_t & time_bombed_allocator, int count) +{ + ((time_bomb_allocator_state *)time_bombed_allocator.state)->count_until_failure = count; +} + #ifdef __cplusplus } #endif diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp index c723eb523..81e48c212 100644 --- a/rcl/test/rcl/test_arguments.cpp +++ b/rcl/test/rcl/test_arguments.cpp @@ -1029,3 +1029,39 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_overrides ASSERT_TRUE(NULL != param_value->string_value); EXPECT_STREQ("foo", param_value->string_value); } + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_get_param_files) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str(), + "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath2.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args); + EXPECT_EQ(2, parameter_filecount); + + // Configure allocator to fail at different points of the code + rcl_allocator_t bomb_alloc = get_time_bombed_allocator(); + set_time_bombed_allocator_count(bomb_alloc, 0); + char ** parameter_files = NULL; + ret = rcl_arguments_get_param_files(&parsed_args, bomb_alloc, ¶meter_files); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + + set_time_bombed_allocator_count(bomb_alloc, 1); + ret = rcl_arguments_get_param_files(&parsed_args, bomb_alloc, ¶meter_files); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + + set_time_bombed_allocator_count(bomb_alloc, 2); + ret = rcl_arguments_get_param_files(&parsed_args, bomb_alloc, ¶meter_files); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; +} From 364e1fb1824f9c1644fdb5c007796c77f797cbe1 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 14 Aug 2020 18:49:46 -0300 Subject: [PATCH 14/42] Fix rcl package's logging API error specs and handling. (#746) Signed-off-by: Michel Hidalgo --- rcl/include/rcl/logging.h | 7 ++++--- rcl/src/rcl/logging.c | 3 ++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rcl/include/rcl/logging.h b/rcl/include/rcl/logging.h index 02f7b16a2..1014853d5 100644 --- a/rcl/include/rcl/logging.h +++ b/rcl/include/rcl/logging.h @@ -48,7 +48,7 @@ typedef rcutils_logging_output_handler_t rcl_logging_output_handler_t; * \return `RCL_RET_OK` if successful, or * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or - * \return `RCL_RET_ERR` if a general error occurs + * \return `RCL_RET_ERROR` if a general error occurs */ RCL_PUBLIC RCL_WARN_UNUSED @@ -74,8 +74,9 @@ rcl_logging_configure( * \param allocator Used to allocate memory used by the logging system * \param output_handler Output handler to be installed * \return `RCL_RET_OK` if successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or - * \return `RCL_RET_ERR` if a general error occurs + * \return `RCL_RET_ERROR` if a general error occurs */ RCL_PUBLIC RCL_WARN_UNUSED @@ -97,7 +98,7 @@ rcl_logging_configure_with_output_handler( * Lock-Free | Yes * * \return `RCL_RET_OK` if successful. - * \return `RCL_RET_ERR` if a general error occurs + * \return `RCL_RET_ERROR` if a general error occurs */ RCL_PUBLIC RCL_WARN_UNUSED diff --git a/rcl/src/rcl/logging.c b/rcl/src/rcl/logging.c index 71c778388..5ae5c7405 100644 --- a/rcl/src/rcl/logging.c +++ b/rcl/src/rcl/logging.c @@ -60,7 +60,8 @@ rcl_logging_configure_with_output_handler( rcl_logging_output_handler_t output_handler) { RCL_CHECK_ARGUMENT_FOR_NULL(global_args, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(output_handler, RCL_RET_INVALID_ARGUMENT); RCUTILS_LOGGING_AUTOINIT g_logging_allocator = *allocator; int default_level = global_args->impl->log_level; From ad8b2b71c60018e1543094f02764de2eb3ce3fac Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 18 Aug 2020 11:52:27 -0300 Subject: [PATCH 15/42] Fix allocation arguments copy (#748) * Change allocation method for copied parameter files * Add bad allocator test Signed-off-by: Jorge Perez --- rcl/src/rcl/arguments.c | 4 ++-- rcl/test/rcl/test_arguments.cpp | 34 +++++++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/arguments.c b/rcl/src/rcl/arguments.c index 950d08d38..5374ca0b0 100644 --- a/rcl/src/rcl/arguments.c +++ b/rcl/src/rcl/arguments.c @@ -862,8 +862,8 @@ rcl_arguments_copy( // Copy parameter files if (args->impl->num_param_files_args) { - args_out->impl->parameter_files = allocator.allocate( - sizeof(char *) * args->impl->num_param_files_args, allocator.state); + args_out->impl->parameter_files = allocator.zero_allocate( + args->impl->num_param_files_args, sizeof(char *), allocator.state); if (NULL == args_out->impl->parameter_files) { if (RCL_RET_OK != rcl_arguments_fini(args_out)) { RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp index 81e48c212..fba50a4c4 100644 --- a/rcl/test/rcl/test_arguments.cpp +++ b/rcl/test/rcl/test_arguments.cpp @@ -1065,3 +1065,37 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_get_p ret = rcl_arguments_get_param_files(&parsed_args, bomb_alloc, ¶meter_files); EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; } + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_allocs_copy) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str(), + "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath2.c_str(), + "-r", "/foo/bar:=/fiz/buz", "--remap", "foo:=/baz", + "-e", "/foo", "--", "foo" + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + rcl_allocator_t bomb_alloc = get_time_bombed_allocator(); + rcl_allocator_t saved_alloc = parsed_args.impl->allocator; + parsed_args.impl->allocator = bomb_alloc; + for (int i = 0; i < 8; i++) { + set_time_bombed_allocator_count(bomb_alloc, i); + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + parsed_args.impl->allocator = saved_alloc; +} From 44235ee3bbfef57fe3a2efd9ffc8ad8df4f90860 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 Aug 2020 16:01:03 -0300 Subject: [PATCH 16/42] Complete rcl enclave validation API coverage. (#751) Signed-off-by: Michel Hidalgo --- rcl/test/CMakeLists.txt | 2 +- rcl/test/rcl/test_validate_enclave_name.cpp | 59 +++++++++++++++++++++ 2 files changed, 60 insertions(+), 1 deletion(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 411bcbfde..3aac54c91 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -353,7 +353,7 @@ call_for_each_rmw_implementation(test_target) rcl_add_custom_gtest(test_validate_enclave_name SRCS rcl/test_validate_enclave_name.cpp APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} mimick ) rcl_add_custom_gtest(test_domain_id diff --git a/rcl/test/rcl/test_validate_enclave_name.cpp b/rcl/test/rcl/test_validate_enclave_name.cpp index 408bdcccb..0cc4b1cdc 100644 --- a/rcl/test/rcl/test_validate_enclave_name.cpp +++ b/rcl/test/rcl/test_validate_enclave_name.cpp @@ -17,11 +17,17 @@ #include #include +#include "rcutils/snprintf.h" + #include "rcl/rcl.h" #include "rcl/validate_enclave_name.h" #include "rcl/error_handling.h" +#include "rmw/validate_namespace.h" + +#include "../mocking_utils/patch.hpp" + TEST(TestValidateEnclaveName, test_validate) { int validation_result; size_t invalid_index; @@ -47,6 +53,59 @@ TEST(TestValidateEnclaveName, test_validate) { RCL_RET_OK, rcl_validate_enclave_name("/foo/bar", &validation_result, &invalid_index)); EXPECT_EQ(RCL_ENCLAVE_NAME_VALID, validation_result); + + { + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_namespace_with_size, + [&](auto, auto, int * result, size_t * index) { + if (index) { + *index = 0u; + } + *result = RMW_NAMESPACE_INVALID_TOO_LONG; + return RMW_RET_OK; + }); + + // When applying RMW namespace validation rules, an enclave name may be too + // long for an RMW namespace but not necessarily for an enclave name. + EXPECT_EQ( + RCL_RET_OK, + rcl_validate_enclave_name("/foo/baz", &validation_result, &invalid_index)); + EXPECT_EQ(RCL_ENCLAVE_NAME_VALID, validation_result); + } +} + +TEST(TestValidateEnclaveName, test_validate_on_internal_error) { + int validation_result; + size_t invalid_index; + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_validate_namespace_with_size, "internal error", RMW_RET_ERROR); + + EXPECT_EQ( + RCL_RET_ERROR, + rcl_validate_enclave_name("/foo", &validation_result, &invalid_index)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_namespace_with_size, + [&](auto, auto, int * result, size_t * index) { + if (index) { + *index = 0u; + } + *result = -1; + return RMW_RET_OK; + }); + + EXPECT_EQ( + RCL_RET_ERROR, + rcl_validate_enclave_name("/foo", &validation_result, &invalid_index)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } } TEST(TestValidateEnclaveName, test_validation_string) { From cabc782522b41e5c48dffb3af7b049dcacea2655 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 30 Oct 2020 11:36:02 +0100 Subject: [PATCH 17/42] Added path_to_fail to mocking_utils in rcl Signed-off-by: ahcorde --- rcl/test/mocking_utils/patch.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rcl/test/mocking_utils/patch.hpp b/rcl/test/mocking_utils/patch.hpp index d170c7fa3..eeae20156 100644 --- a/rcl/test/mocking_utils/patch.hpp +++ b/rcl/test/mocking_utils/patch.hpp @@ -354,6 +354,14 @@ auto make_patch(const std::string & target, std::function proxy) #define patch_and_return(scope, function, return_code) \ patch(scope, function, [&](auto && ...) {return return_code;}) +/// Patch a `function` to always yield a given `return_code` in a given `scope`. +#define patch_to_fail(scope, function, error_message, return_code) \ + patch( \ + scope, function, [&](auto && ...) { \ + RCUTILS_SET_ERROR_MSG(error_message); \ + return return_code; \ + }) + /// Patch a `function` to execute normally but always yield a given `return_code` /// in a given `scope`. #define inject_on_return(scope, function, return_code) \ From 12e104daed0f7919a253753a07ef57990c46c648 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 19 Aug 2020 17:40:01 -0300 Subject: [PATCH 18/42] Clean up rcl_expand_topic_name() implementation. (#757) Signed-off-by: Michel Hidalgo --- rcl/src/rcl/expand_topic_name.c | 27 ++------------------------- 1 file changed, 2 insertions(+), 25 deletions(-) diff --git a/rcl/src/rcl/expand_topic_name.c b/rcl/src/rcl/expand_topic_name.c index 24c90c55e..ab06eb19c 100644 --- a/rcl/src/rcl/expand_topic_name.c +++ b/rcl/src/rcl/expand_topic_name.c @@ -71,14 +71,7 @@ rcl_expand_topic_name( rmw_ret = rmw_validate_node_name(node_name, &validation_result, NULL); if (rmw_ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - switch (rmw_ret) { - case RMW_RET_INVALID_ARGUMENT: - return RCL_RET_INVALID_ARGUMENT; - case RMW_RET_ERROR: - // fall through on purpose - default: - return RCL_RET_ERROR; - } + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } if (validation_result != RMW_NODE_NAME_VALID) { RCL_SET_ERROR_MSG("node name is invalid"); @@ -88,14 +81,7 @@ rcl_expand_topic_name( rmw_ret = rmw_validate_namespace(node_namespace, &validation_result, NULL); if (rmw_ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - switch (rmw_ret) { - case RMW_RET_INVALID_ARGUMENT: - return RCL_RET_INVALID_ARGUMENT; - case RMW_RET_ERROR: - // fall through on purpose - default: - return RCL_RET_ERROR; - } + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } if (validation_result != RMW_NODE_NAME_VALID) { RCL_SET_ERROR_MSG("node namespace is invalid"); @@ -220,15 +206,6 @@ rcl_expand_topic_name( return RCL_RET_BAD_ALLOC; } } - // if the original input_topic_name has not yet be copied into new memory, strdup it now - if (!local_output) { - local_output = rcutils_strdup(input_topic_name, allocator); - if (!local_output) { - *output_topic_name = NULL; - RCL_SET_ERROR_MSG("failed to allocate memory for output topic"); - return RCL_RET_BAD_ALLOC; - } - } // finally store the result in the out pointer and return *output_topic_name = local_output; return RCL_RET_OK; From f4a51b408270fb9d26c4b1edee09d80d45f9f28e Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 20 Aug 2020 12:07:27 -0300 Subject: [PATCH 19/42] Add coverage tests 94% service.c (#756) * Add tests for fail init * Add fini tests * Add tests rcl_take_request_with_info * Add tests send_response * Change const strings to constexpr * Improve test descriptions * Remove test Signed-off-by: Jorge Perez --- rcl/src/rcl/service.c | 1 - rcl/test/CMakeLists.txt | 2 +- rcl/test/rcl/test_service.cpp | 206 ++++++++++++++++++++++++++++++++++ 3 files changed, 207 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index cf7de3c4e..d773f2549 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -111,7 +111,6 @@ rcl_service_init( RCL_SET_ERROR_MSG(rcutils_get_error_string().str); ret = RCL_RET_ERROR; goto cleanup; - return RCL_RET_ERROR; } if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 3aac54c91..9c4780b77 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -217,7 +217,7 @@ function(test_target_function) SRCS rcl/test_service.cpp rcl/wait_for_entity_helpers.cpp ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} mimick AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index 16d1498e9..9ec820f8c 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/rcl/test/rcl/test_service.cpp @@ -21,8 +21,11 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" +#include "rmw/validate_namespace.h" + #include "wait_for_entity_helpers.hpp" #include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -420,3 +423,206 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_fail_name EXPECT_EQ(RCL_RET_SERVICE_NAME_INVALID, ret) << rcl_get_error_string().str; rcl_reset_error(); } + +// Define dummy comparison operators for rcutils_allocator_t type for use with the Mimick Library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) + +/* Test failed service initialization using mocks + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_ini_mocked) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic[] = "topic"; + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + rcl_ret_t ret = RCL_RET_OK; + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_string_map_init, RCUTILS_RET_ERROR); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + // Mocking this function causes rcl_expand_topic_name to return RCL_RET_ERROR + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_namespace, RMW_RET_ERROR); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rcutils_string_map_fini, RCUTILS_RET_ERROR); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_full_topic_name, RMW_RET_ERROR); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_full_topic_name, + [](auto, int * result, auto) { + *result = RMW_TOPIC_INVALID_IS_EMPTY_STRING; + return RMW_RET_OK; + }); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_SERVICE_NAME_INVALID, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_create_service, nullptr); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} + +/* Test failed service finalization using mocks + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_fini_mocked) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic[] = "primitives"; + + 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, this->node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_service_t empty_service = rcl_get_zero_initialized_service(); + ret = rcl_service_fini(&empty_service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rmw_destroy_service, RMW_RET_ERROR); + ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +/* Test failed service take_request_with_info using mocks and nullptrs + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_take_request_with_info) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic[] = "primitives"; + + 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, this->node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + test_msgs__srv__BasicTypes_Request service_request; + test_msgs__srv__BasicTypes_Request__init(&service_request); + rmw_service_info_t header; + + ret = rcl_take_request_with_info(nullptr, &header, &service_request); + EXPECT_EQ(RCL_RET_SERVICE_INVALID, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_take_request_with_info(&service, nullptr, &service_request); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_take_request_with_info(&service, &header, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_take_request, RMW_RET_ERROR); + ret = rcl_take_request_with_info(&service, &header, &service_request); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_take_request, RMW_RET_BAD_ALLOC); + ret = rcl_take_request_with_info(&service, &header, &service_request); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_request, + [](auto, auto, auto, bool * taken) { + *taken = false; + return RMW_RET_OK; + }); + ret = rcl_take_request_with_info(&service, &header, &service_request); + EXPECT_EQ(RCL_RET_SERVICE_TAKE_FAILED, ret); + } +} + +/* Test failed service send_response using mocks and nullptrs + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_send_response) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic[] = "primitives"; + + 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, this->node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Init dummy response. + test_msgs__srv__BasicTypes_Response service_response; + test_msgs__srv__BasicTypes_Response__init(&service_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Response__fini(&service_response); + }); + rmw_service_info_t header; + + ret = rcl_send_response(nullptr, &header.request_id, &service_response); + EXPECT_EQ(RCL_RET_SERVICE_INVALID, ret); + + ret = rcl_send_response(&service, nullptr, &service_response); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + + ret = rcl_send_response(&service, &header.request_id, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_send_response, RMW_RET_ERROR); + ret = rcl_send_response(&service, &header.request_id, &service_response); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} From 99efabd5fdb5cac1d5f1a95b8c98b9852b9ee578 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 20 Aug 2020 12:10:34 -0300 Subject: [PATCH 20/42] Extend rcl_expand_topic_name() API test coverage. (#758) Signed-off-by: Michel Hidalgo --- rcl/test/CMakeLists.txt | 2 +- rcl/test/rcl/test_expand_topic_name.cpp | 91 +++++++++++++++++++++++++ 2 files changed, 92 insertions(+), 1 deletion(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 9c4780b77..4cbf5e460 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -377,7 +377,7 @@ rcl_add_custom_gtest(test_validate_topic_name rcl_add_custom_gtest(test_expand_topic_name SRCS rcl/test_expand_topic_name.cpp APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} mimick ) rcl_add_custom_gtest(test_security diff --git a/rcl/test/rcl/test_expand_topic_name.cpp b/rcl/test/rcl/test_expand_topic_name.cpp index fedd54041..308fbed68 100644 --- a/rcl/test/rcl/test_expand_topic_name.cpp +++ b/rcl/test/rcl/test_expand_topic_name.cpp @@ -19,11 +19,18 @@ #include #include +#include "rcutils/repl_str.h" +#include "rcutils/strdup.h" + #include "rcl/expand_topic_name.h" #include "rcl/error_handling.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + #include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" using namespace std::string_literals; @@ -137,6 +144,90 @@ TEST(test_expand_topic_name, invalid_arguments) { ASSERT_EQ(RCL_RET_OK, ret); } +// Define dummy comparison operators for rcutils_allocator_t type +// to use with the Mimick mocking library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST(test_expand_topic_name, internal_error) { + constexpr char node_name[] = "bar"; + constexpr char ns[] = "/foo"; + + rcutils_string_map_t subs = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t uret = rcutils_string_map_init(&subs, 0, rcutils_get_default_allocator()); + ASSERT_EQ(RCUTILS_RET_OK, uret) << rcutils_get_error_string().str; + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&subs); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * expanded_topic_name = nullptr; + + { + constexpr char topic_name[] = "/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_validate_node_name, "internal error", RMW_RET_ERROR); + ret = rcl_expand_topic_name( + topic_name, node_name, ns, &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + constexpr char topic_name[] = "/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_validate_namespace, "internal error", RMW_RET_ERROR); + ret = rcl_expand_topic_name( + topic_name, node_name, ns, &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + constexpr char topic_name_with_valid_substitution[] = "{node}/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_strndup, "failed to allocate", nullptr); + ret = rcl_expand_topic_name( + topic_name_with_valid_substitution, node_name, ns, + &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + constexpr char topic_name_with_unknown_substitution[] = "{unknown}/test"; + ret = rcl_expand_topic_name( + topic_name_with_unknown_substitution, node_name, ns, + &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_UNKNOWN_SUBSTITUTION, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + constexpr char topic_name[] = "{node}/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_repl_str, "failed to allocate", nullptr); + ret = rcl_expand_topic_name( + topic_name, node_name, ns, &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + constexpr char topic_name[] = "/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_strdup, "failed to allocate", nullptr); + ret = rcl_expand_topic_name( + topic_name, node_name, ns, &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} + TEST(test_expand_topic_name, various_valid_topics) { rcl_ret_t ret; rcl_allocator_t allocator = rcl_get_default_allocator(); From 30a9681877d3e572e3ee40d1b74826d24277d990 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 20 Aug 2020 18:42:11 -0300 Subject: [PATCH 21/42] Adding tests to arguments.c (#752) * Add nullptr tests get_param_files * Add bad alloc tests * Add missing tests * Add bad alloc tests rcl_arguments_copy * Remove repeated test * Remove spaces * Restore erased test * Relocate test * Refactor bomb allocator test * Add missing rcl_reset_error() checks Signed-off-by: Jorge Perez --- rcl/test/rcl/test_arguments.cpp | 155 ++++++++++++++++++++++++++++++++ 1 file changed, 155 insertions(+) diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp index fba50a4c4..7248051b3 100644 --- a/rcl/test/rcl/test_arguments.cpp +++ b/rcl/test/rcl/test_arguments.cpp @@ -309,6 +309,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_unpar const int argc = sizeof(argv) / sizeof(const char *); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_EQ(2, rcl_arguments_get_count_unparsed(&parsed_args)); @@ -317,13 +318,36 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_unpar EXPECT_EQ( RCL_RET_BAD_ALLOC, rcl_arguments_get_unparsed(&parsed_args, bad_alloc, &actual_unparsed)); rcl_reset_error(); + EXPECT_EQ( RCL_RET_BAD_ALLOC, rcl_arguments_get_unparsed_ros(&parsed_args, bad_alloc, &actual_unparsed)); rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_arguments_get_unparsed_ros(nullptr, allocator, &actual_unparsed)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); } +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_empty_unparsed) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_arguments_t empty_parsed_args = rcl_get_zero_initialized_arguments(); + int * actual_unparsed = NULL; + int * actual_unparsed_ros = NULL; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_arguments_get_unparsed(&empty_parsed_args, allocator, &actual_unparsed)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_arguments_get_unparsed_ros(&empty_parsed_args, allocator, &actual_unparsed_ros)); + rcl_reset_error(); +} + TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_params_get_counts) { rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); EXPECT_EQ(-1, rcl_arguments_get_count_unparsed(nullptr)); @@ -698,6 +722,35 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_remove_ros_ EXPECT_EQ(0, nonros_argc); } +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_remove_ros_args) { + const char * const argv[] = { + "process_name", "-d", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz", "--", + "--foo=bar", "--baz", "--ros-args", "--ros-args", "-p", "bar:=baz", "--", "--", "arg", + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int nonros_argc = 0; + const char ** nonros_argv = NULL; + rcl_allocator_t bomb_alloc = get_time_bombed_allocator(); + set_time_bombed_allocator_count(bomb_alloc, 1); + ret = rcl_remove_ros_arguments( + argv, + &parsed_args, + bomb_alloc, + &nonros_argc, + &nonros_argv); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); +} + TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args) { const char * const argv[] = { "process_name", "-d", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz", "--", @@ -854,6 +907,17 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_ } alloc.deallocate(parameter_files, alloc.state); + // Test bad alloc + rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_params_t * params_test = NULL; + rcl_allocator_t saved_alloc = parsed_args.impl->allocator; + parsed_args.impl->parameter_overrides->allocator = bad_alloc; + ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms_test); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + EXPECT_EQ(NULL, params_test); + parsed_args.impl->parameter_overrides->allocator = saved_alloc; + + // Expected usage rcl_params_t * params = NULL; ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -943,6 +1007,35 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_ EXPECT_FALSE(param_value->bool_array_value->values[2]); } +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_arguments_copy) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str(), + "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath2.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_ret_t ret; + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args); + EXPECT_EQ(2, parameter_filecount); + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(2, rcl_arguments_get_param_files_count(&copied_args)); +} + TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_param_overrides) { const char * const argv[] = {"process_name"}; const int argc = sizeof(argv) / sizeof(const char *); @@ -966,6 +1059,11 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_param_overri EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + rcl_arguments_t empty_parsed_arg = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_get_param_overrides(&empty_parsed_arg, ¶ms); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + rcl_params_t preallocated_params; params = &preallocated_params; ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms); @@ -1099,3 +1197,60 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_allocs_copy } parsed_args.impl->allocator = saved_alloc; } + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_get_param_files) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + char ** parameter_files = NULL; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, allocator, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + ret = rcl_arguments_get_param_files(nullptr, allocator, ¶meter_files); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_arguments_get_param_files(&parsed_args, allocator, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_arguments_t empty_parsed_args = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_get_param_files(&empty_parsed_args, allocator, ¶meter_files); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_parse_arg) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_allocator_t bomb_alloc = get_time_bombed_allocator(); + + for (int i = 0; i < 100; i++) { + set_time_bombed_allocator_count(bomb_alloc, i); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, bomb_alloc, &parsed_args); + if (RCL_RET_OK == ret) { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + break; + } else { + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcl_reset_error(); + } + } +} From 8ee555261fa7ead43a0805fd15044154c93030bd Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 25 Aug 2020 12:43:32 -0300 Subject: [PATCH 22/42] Zero initialize guard condition on failed init. (#760) Signed-off-by: Michel Hidalgo --- rcl/src/rcl/guard_condition.c | 1 + 1 file changed, 1 insertion(+) diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index 6f004b51c..d7e17afda 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/rcl/src/rcl/guard_condition.c @@ -87,6 +87,7 @@ __rcl_guard_condition_init_from_rmw_impl( if (!guard_condition->impl->rmw_handle) { // Deallocate impl and exit. allocator->deallocate(guard_condition->impl, allocator->state); + guard_condition->impl = NULL; RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } From 63842fd0cebeb0ac139c66be297311fd777a4f71 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 25 Aug 2020 14:08:26 -0300 Subject: [PATCH 23/42] Do not invalidate context before successful shutdown. (#761) Signed-off-by: Michel Hidalgo --- rcl/src/rcl/init.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c index 94a874cf6..e4fe376a9 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -244,15 +244,15 @@ rcl_shutdown(rcl_context_t * context) return RCL_RET_ALREADY_SHUTDOWN; } - // reset the instance id to 0 to indicate "invalid" - rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); - rmw_ret_t rmw_ret = rmw_shutdown(&(context->impl->rmw_context)); if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } + // reset the instance id to 0 to indicate "invalid" + rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); + return RCL_RET_OK; } From 0c02c8d2749094a18266e38170022b8661095a5c Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 25 Aug 2020 17:23:47 -0300 Subject: [PATCH 24/42] Yield rcl_context_fini() error codes. (#763) Signed-off-by: Michel Hidalgo --- rcl/src/rcl/context.c | 25 +++++++++++++++++-------- rcl/src/rcl/context_impl.h | 2 +- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/rcl/src/rcl/context.c b/rcl/src/rcl/context.c index b400c09ef..9c7b90ce2 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -21,6 +21,7 @@ extern "C" #include +#include "./common.h" #include "./context_impl.h" #include "rcutils/stdatomic_helper.h" @@ -56,8 +57,7 @@ rcl_context_fini(rcl_context_t * context) } RCL_CHECK_ALLOCATOR_WITH_MSG( &(context->impl->allocator), "invalid allocator", return RCL_RET_INVALID_ARGUMENT); - __cleanup_context(context); - return RCL_RET_OK; + return __cleanup_context(context); } // See `rcl_shutdown()` for invalidation of the context. @@ -92,15 +92,16 @@ rcl_context_get_rmw_context(rcl_context_t * context) return &(context->impl->rmw_context); } -void +rcl_ret_t __cleanup_context(rcl_context_t * context) { + rcl_ret_t ret = RCL_RET_OK; // 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); // clean up global_arguments if initialized if (NULL != context->global_arguments.impl) { - rcl_ret_t ret = rcl_arguments_fini(&(context->global_arguments)); + ret = rcl_arguments_fini(&(context->global_arguments)); if (RCL_RET_OK != ret) { RCUTILS_SAFE_FWRITE_TO_STDERR( "[rcl|context.c:" RCUTILS_STRINGIFY(__LINE__) @@ -118,8 +119,11 @@ __cleanup_context(rcl_context_t * context) // finalize init options if valid if (NULL != context->impl->init_options.impl) { - rcl_ret_t ret = rcl_init_options_fini(&(context->impl->init_options)); - if (RCL_RET_OK != ret) { + rcl_ret_t init_options_fini_ret = rcl_init_options_fini(&(context->impl->init_options)); + if (RCL_RET_OK != init_options_fini_ret) { + if (RCL_RET_OK == ret) { + ret = init_options_fini_ret; + } RCUTILS_SAFE_FWRITE_TO_STDERR( "[rcl|context.c:" RCUTILS_STRINGIFY(__LINE__) "] failed to finalize init options while cleaning up context, memory may be leaked: "); @@ -131,8 +135,11 @@ __cleanup_context(rcl_context_t * context) // clean up rmw_context if (NULL != context->impl->rmw_context.implementation_identifier) { - rmw_ret_t rmw_ret = rmw_context_fini(&(context->impl->rmw_context)); - if (RMW_RET_OK != rmw_ret) { + rmw_ret_t rmw_context_fini_ret = rmw_context_fini(&(context->impl->rmw_context)); + if (RMW_RET_OK != rmw_context_fini_ret) { + if (RCL_RET_OK == ret) { + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_context_fini_ret); + } RCUTILS_SAFE_FWRITE_TO_STDERR( "[rcl|context.c:" RCUTILS_STRINGIFY(__LINE__) "] failed to finalize rmw context while cleaning up context, memory may be leaked: "); @@ -157,6 +164,8 @@ __cleanup_context(rcl_context_t * context) // zero-initialize the context *context = rcl_get_zero_initialized_context(); + + return ret; } #ifdef __cplusplus diff --git a/rcl/src/rcl/context_impl.h b/rcl/src/rcl/context_impl.h index 79741e31d..08403c08f 100644 --- a/rcl/src/rcl/context_impl.h +++ b/rcl/src/rcl/context_impl.h @@ -41,7 +41,7 @@ typedef struct rcl_context_impl_t } rcl_context_impl_t; RCL_LOCAL -void +rcl_ret_t __cleanup_context(rcl_context_t * context); #ifdef __cplusplus From e79b90023c1c3d1364f0c7b35ec2ab4285465599 Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 25 Aug 2020 13:47:25 -0700 Subject: [PATCH 25/42] Add fault injection macros to rcl functions (#727) * Add fault injection macros to rcl functions Signed-off-by: Stephen Brawner * PR fixup Signed-off-by: Stephen Brawner --- rcl/CMakeLists.txt | 4 ++++ rcl/src/rcl/client.c | 5 +++++ rcl/src/rcl/graph.c | 8 ++++++++ rcl/src/rcl/publisher.c | 16 ++++++++++++++++ rcl/src/rcl/service.c | 13 +++++++++++++ rcl/src/rcl/subscription.c | 8 ++++++++ rcl/src/rcl/time.c | 4 ++++ rcl/src/rcl/timer.c | 7 +++++++ rcl/test/rcl/test_client.cpp | 27 +++++++++++++++++++++++++++ rcl/test/rcl/test_subscription.cpp | 28 ++++++++++++++++++++++++++++ 10 files changed, 120 insertions(+) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index cbc267792..934ef4ae1 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -83,6 +83,10 @@ ament_target_dependencies(${PROJECT_NAME} target_compile_definitions(${PROJECT_NAME} PRIVATE "RCL_BUILDING_DLL") rcl_set_symbol_visibility_hidden(${PROJECT_NAME} LANGUAGE "C") +if(BUILD_TESTING AND NOT RCUTILS_DISABLE_FAULT_INJECTION) + target_compile_definitions(${PROJECT_NAME} PUBLIC RCUTILS_ENABLE_FAULT_INJECTION) +endif() + install( TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 6a6a51001..875236390 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -26,6 +26,7 @@ extern "C" #include "rcl/expand_topic_name.h" #include "rcl/remap.h" #include "rcutils/logging_macros.h" +#include "rcutils/macros.h" #include "rcutils/stdatomic_helper.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -206,6 +207,10 @@ rcl_client_init( rcl_ret_t rcl_client_fini(rcl_client_t * client, rcl_node_t * node) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing client"); rcl_ret_t result = RCL_RET_OK; RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c index 9fa3be327..6712da10b 100644 --- a/rcl/src/rcl/graph.c +++ b/rcl/src/rcl/graph.c @@ -21,6 +21,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcutils/allocator.h" +#include "rcutils/macros.h" #include "rcutils/types.h" #include "rmw/error_handling.h" #include "rmw/get_node_info_and_types.h" @@ -289,6 +290,8 @@ rcl_names_and_types_init( size_t size, rcl_allocator_t * allocator) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(names_and_types, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ALLOCATOR(allocator, return RCL_RET_INVALID_ARGUMENT); rmw_ret_t rmw_ret = rmw_names_and_types_init(names_and_types, size, allocator); @@ -298,6 +301,8 @@ rcl_names_and_types_init( rcl_ret_t rcl_names_and_types_fini(rcl_names_and_types_t * topic_names_and_types) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types); return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); @@ -514,6 +519,9 @@ rcl_service_server_is_available( const rcl_client_t * client, bool * is_available) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; // error already set } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 83f433c52..59da94d94 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -27,6 +27,7 @@ extern "C" #include "rcl/expand_topic_name.h" #include "rcl/remap.h" #include "rcutils/logging_macros.h" +#include "rcutils/macros.h" #include "rmw/error_handling.h" #include "rmw/validate_full_topic_name.h" #include "tracetools/tracetools.h" @@ -50,6 +51,13 @@ rcl_publisher_init( const rcl_publisher_options_t * options ) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_TOPIC_NAME_INVALID); + rcl_ret_t fail_ret = RCL_RET_ERROR; // Check options and allocator first, so allocator can be used with errors. @@ -225,6 +233,11 @@ rcl_publisher_init( rcl_ret_t rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + rcl_ret_t result = RCL_RET_OK; RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_PUBLISHER_INVALID); if (!rcl_node_is_valid_except_context(node)) { @@ -295,6 +308,9 @@ rcl_publish( const void * ros_message, rmw_publisher_allocation_t * allocation) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + if (!rcl_publisher_is_valid(publisher)) { return RCL_RET_PUBLISHER_INVALID; // error already set } diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index d773f2549..196e4aab3 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -26,6 +26,7 @@ extern "C" #include "rcl/expand_topic_name.h" #include "rcl/remap.h" #include "rcutils/logging_macros.h" +#include "rcutils/macros.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" @@ -52,6 +53,13 @@ rcl_service_init( const char * service_name, const rcl_service_options_t * options) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SERVICE_NAME_INVALID); + rcl_ret_t fail_ret = RCL_RET_ERROR; // Check options and allocator first, so the allocator can be used in errors. @@ -209,6 +217,11 @@ rcl_service_init( rcl_ret_t rcl_service_fini(rcl_service_t * service, rcl_node_t * node) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SERVICE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing service"); RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_SERVICE_INVALID); if (!rcl_node_is_valid_except_context(node)) { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 04b076a89..f3628b828 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -220,6 +220,11 @@ rcl_subscription_init( rcl_ret_t rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing subscription"); rcl_ret_t result = RCL_RET_OK; RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_SUBSCRIPTION_INVALID); @@ -466,6 +471,9 @@ rcl_subscription_get_publisher_count( const rcl_subscription_t * subscription, size_t * publisher_count) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + if (!rcl_subscription_is_valid(subscription)) { return RCL_RET_SUBSCRIPTION_INVALID; } diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index 21ed84812..852cb8f18 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -20,6 +20,7 @@ #include "./common.h" #include "rcl/allocator.h" #include "rcl/error_handling.h" +#include "rcutils/macros.h" #include "rcutils/stdatomic_helper.h" #include "rcutils/time.h" @@ -251,6 +252,9 @@ rcl_difference_times( rcl_ret_t rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(time_point_value, RCL_RET_INVALID_ARGUMENT); if (clock->type && clock->get_now) { diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 37c3014ea..c4f9880ab 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -345,6 +345,8 @@ rcl_timer_get_period(const rcl_timer_t * timer, int64_t * period) rcl_ret_t rcl_timer_exchange_period(const rcl_timer_t * timer, int64_t new_period, int64_t * old_period) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT); *old_period = rcutils_atomic_exchange_uint64_t(&timer->impl->period, new_period); @@ -375,6 +377,9 @@ rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_ rcl_ret_t rcl_timer_cancel(rcl_timer_t * timer) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); rcutils_atomic_store(&timer->impl->canceled, true); @@ -394,6 +399,8 @@ rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled) rcl_ret_t rcl_timer_reset(rcl_timer_t * timer) { + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); rcl_time_point_value_t now; diff --git a/rcl/test/rcl/test_client.cpp b/rcl/test/rcl/test_client.cpp index 87fa532f3..72bfa3ce9 100644 --- a/rcl/test/rcl/test_client.cpp +++ b/rcl/test/rcl/test_client.cpp @@ -17,6 +17,7 @@ #include "rcl/client.h" #include "rcl/rcl.h" +#include "rcutils/testing/fault_injection.h" #include "test_msgs/srv/basic_types.h" @@ -277,3 +278,29 @@ TEST_F(TestClientFixture, test_client_bad_arguments) { &client, &client_request, &sequence_number)) << rcl_get_error_string().str; EXPECT_EQ(24, sequence_number); } + +TEST_F(TestClientFixture, test_client_init_fini_maybe_fail) +{ + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic_name[] = "chatter"; + rcl_client_options_t default_client_options = rcl_client_get_default_options(); + + RCUTILS_FAULT_INJECTION_TEST( + { + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_ret_t ret = rcl_client_init( + &client, this->node_ptr, ts, topic_name, &default_client_options); + if (RCL_RET_OK == ret) { + EXPECT_TRUE(rcl_client_is_valid(&client)); + ret = rcl_client_fini(&client, this->node_ptr); + if (RCL_RET_OK != ret) { + // If fault injection caused fini to fail, we should try it again. + EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, this->node_ptr)); + } + } else { + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + }); +} diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 1e5b8c779..471a3110a 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -23,6 +23,7 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" +#include "rcutils/testing/fault_injection.h" #include "test_msgs/msg/basic_types.h" #include "test_msgs/msg/strings.h" #include "rosidl_runtime_c/string_functions.h" @@ -1065,3 +1066,30 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip EXPECT_EQ(NULL, rcl_subscription_get_options(&subscription_zero_init)); rcl_reset_error(); } + +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_init_fini_maybe_fail) +{ + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "chatter"; + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + + RCUTILS_FAULT_INJECTION_TEST( + { + rcl_ret_t ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, topic, &subscription_options); + + if (RCL_RET_OK == ret) { + EXPECT_TRUE(rcl_subscription_is_valid(&subscription)); + ret = rcl_subscription_fini(&subscription, this->node_ptr); + if (RCL_RET_OK != ret) { + // If fault injection caused fini to fail, we should try it again. + EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, this->node_ptr)); + } + } else { + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + }); +} From 1f9f6279dcdd93f3f830da19792c792b28c3a46e Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 25 Aug 2020 18:19:12 -0300 Subject: [PATCH 26/42] Cleanup rcl_get_secure_root() implementation. (#762) * Do not assume allocation will succeed. * Do not leak allocated memory on failure. Signed-off-by: Michel Hidalgo --- rcl/src/rcl/security.c | 101 ++++++++++++++++++++++------------------- 1 file changed, 54 insertions(+), 47 deletions(-) diff --git a/rcl/src/rcl/security.c b/rcl/src/rcl/security.c index e895fb0f7..bc688398d 100644 --- a/rcl/src/rcl/security.c +++ b/rcl/src/rcl/security.c @@ -128,52 +128,58 @@ char * exact_match_lookup( return secure_root; } +static const char * +dupenv(const char * name, const rcl_allocator_t * allocator, char ** value) +{ + const char * buffer = NULL; + const char * error = rcutils_get_env(name, &buffer); + if (NULL != error) { + return error; + } + *value = NULL; + if (0 != strcmp("", buffer)) { + *value = rcutils_strdup(buffer, *allocator); + if (NULL == *value) { + return "string duplication failed"; + } + } + return NULL; +} + char * rcl_get_secure_root( const char * name, const rcl_allocator_t * allocator) { - bool ros_secure_enclave_override = true; + RCL_CHECK_ARGUMENT_FOR_NULL(name, NULL); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "allocator is invalid", return NULL); - // find out if either of the configuration environment variables are set - const char * env_buf = NULL; - if (NULL == name) { - return NULL; - } - const char * get_env_error_str = NULL; - // check if enclave override environment variable is empty - get_env_error_str = rcutils_get_env(ROS_SECURITY_ENCLAVE_OVERRIDE, &env_buf); - if (NULL != get_env_error_str) { - RCUTILS_LOG_ERROR("rcutils_get_env failed: %s\n", get_env_error_str); - return NULL; - } - if (!env_buf) { - return NULL; - } - if (0 == strcmp("", env_buf)) { - ros_secure_enclave_override = false; - } - char * ros_secure_enclave_override_env = rcutils_strdup(env_buf, *allocator); + char * secure_root = NULL; + char * ros_secure_keystore_env = NULL; + char * ros_secure_enclave_override_env = NULL; - // check if keystore environment variable is empty - get_env_error_str = rcutils_get_env(ROS_SECURITY_KEYSTORE_VAR_NAME, &env_buf); - if (NULL != get_env_error_str) { - RCUTILS_LOG_ERROR("rcutils_get_env failed: %s\n", get_env_error_str); - allocator->deallocate(ros_secure_enclave_override_env, allocator->state); - return NULL; - } - if (!env_buf) { - allocator->deallocate(ros_secure_enclave_override_env, allocator->state); + // check keystore environment variable + const char * error = + dupenv(ROS_SECURITY_KEYSTORE_VAR_NAME, allocator, &ros_secure_keystore_env); + if (NULL != error) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "failed to get %s: %s", ROS_SECURITY_KEYSTORE_VAR_NAME, error); return NULL; } - if (0 == strcmp("", env_buf)) { - allocator->deallocate(ros_secure_enclave_override_env, allocator->state); + + if (NULL == ros_secure_keystore_env) { return NULL; // environment variable was empty } - char * ros_secure_keystore_env = rcutils_strdup(env_buf, *allocator); + + // check enclave override environment variable + error = dupenv(ROS_SECURITY_ENCLAVE_OVERRIDE, allocator, &ros_secure_enclave_override_env); + if (NULL != error) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "failed to get %s: %s", ROS_SECURITY_ENCLAVE_OVERRIDE, error); + goto leave_rcl_get_secure_root; + } // given usable environment variables, overwrite with next lookup - char * secure_root = NULL; - if (ros_secure_enclave_override) { + if (NULL != ros_secure_enclave_override_env) { secure_root = exact_match_lookup( ros_secure_enclave_override_env, ros_secure_keystore_env, @@ -185,21 +191,22 @@ char * rcl_get_secure_root( allocator); } - if (NULL == secure_root || !rcutils_is_directory(secure_root)) { - // Check secure_root is not NULL before checking directory - if (NULL == secure_root) { - RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( - "SECURITY ERROR: unable to find a folder matching the name '%s' in '%s'. ", - name, ros_secure_keystore_env); - } else { - RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( - "SECURITY ERROR: directory '%s' does not exist.", secure_root); - } - allocator->deallocate(ros_secure_enclave_override_env, allocator->state); - allocator->deallocate(ros_secure_keystore_env, allocator->state); + if (NULL == secure_root) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "SECURITY ERROR: unable to find a folder matching the name '%s' in '%s'. ", + name, ros_secure_keystore_env); + goto leave_rcl_get_secure_root; + } + + if (!rcutils_is_directory(secure_root)) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "SECURITY ERROR: directory '%s' does not exist.", secure_root); allocator->deallocate(secure_root, allocator->state); - return NULL; + secure_root = NULL; } + +leave_rcl_get_secure_root: + allocator->deallocate(ros_secure_enclave_override_env, allocator->state); allocator->deallocate(ros_secure_keystore_env, allocator->state); return secure_root; } From bfff4c17afa527e4adc094249cd8600924ea1068 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 28 Aug 2020 09:54:48 -0300 Subject: [PATCH 27/42] Check rcutils_strdup() outcome immediately. (#768) Within rcl_node_init() implementation. Signed-off-by: Michel Hidalgo --- rcl/src/rcl/init.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c index e4fe376a9..0025bdda8 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -175,6 +175,13 @@ rcl_init( context->impl->init_options.impl->rmw_init_options.enclave = rcutils_strdup( "/", context->impl->allocator); } + + if (!context->impl->init_options.impl->rmw_init_options.enclave) { + RCL_SET_ERROR_MSG("failed to set context name"); + fail_ret = RCL_RET_BAD_ALLOC; + goto fail; + } + int validation_result; size_t invalid_index; ret = rcl_validate_enclave_name( @@ -195,12 +202,6 @@ rcl_init( goto fail; } - if (!context->impl->init_options.impl->rmw_init_options.enclave) { - RCL_SET_ERROR_MSG("failed to set context name"); - fail_ret = RCL_RET_BAD_ALLOC; - goto fail; - } - rmw_security_options_t * security_options = &context->impl->init_options.impl->rmw_init_options.security_options; ret = rcl_get_security_options_from_environment( From adfb2e9ccc22c0fb6fd2757d8e1bb0be1bbd69f1 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 28 Aug 2020 16:16:33 -0300 Subject: [PATCH 28/42] Bump test coverage. (#764) * init/shutdown API * context fini API * node init/fini API * guard condition init/fini API * security APIs Signed-off-by: Michel Hidalgo --- rcl/src/rcl/arguments.c | 3 + rcl/src/rcl/domain_id.c | 7 +- rcl/src/rcl/expand_topic_name.c | 7 ++ rcl/src/rcl/init_options.c | 11 ++ rcl/src/rcl/localhost.c | 6 +- rcl/src/rcl/node_options.c | 7 +- rcl/src/rcl/remap.c | 14 +++ rcl/src/rcl/validate_enclave_name.c | 16 ++- rcl/test/CMakeLists.txt | 11 +- rcl/test/rcl/test_context.cpp | 12 ++- rcl/test/rcl/test_guard_condition.cpp | 24 +++++ rcl/test/rcl/test_init.cpp | 74 +++++++++++++ rcl/test/rcl/test_node.cpp | 140 ++++++++++++++++++++---- rcl/test/rcl/test_security.cpp | 150 ++++++++++++++++++++++++++ 14 files changed, 440 insertions(+), 42 deletions(-) diff --git a/rcl/src/rcl/arguments.c b/rcl/src/rcl/arguments.c index 5374ca0b0..e76637022 100644 --- a/rcl/src/rcl/arguments.c +++ b/rcl/src/rcl/arguments.c @@ -783,6 +783,9 @@ rcl_arguments_copy( const rcl_arguments_t * args, rcl_arguments_t * args_out) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(args->impl, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(args_out, RCL_RET_INVALID_ARGUMENT); diff --git a/rcl/src/rcl/domain_id.c b/rcl/src/rcl/domain_id.c index 07f586995..6435e6618 100644 --- a/rcl/src/rcl/domain_id.c +++ b/rcl/src/rcl/domain_id.c @@ -27,12 +27,13 @@ const char * const RCL_DOMAIN_ID_ENV_VAR = "ROS_DOMAIN_ID"; rcl_ret_t rcl_get_default_domain_id(size_t * domain_id) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + const char * ros_domain_id = NULL; const char * get_env_error_str = NULL; - if (!domain_id) { - return RCL_RET_INVALID_ARGUMENT; - } + RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT); get_env_error_str = rcutils_get_env(RCL_DOMAIN_ID_ENV_VAR, &ros_domain_id); if (NULL != get_env_error_str) { diff --git a/rcl/src/rcl/expand_topic_name.c b/rcl/src/rcl/expand_topic_name.c index ab06eb19c..313949c67 100644 --- a/rcl/src/rcl/expand_topic_name.c +++ b/rcl/src/rcl/expand_topic_name.c @@ -49,6 +49,13 @@ rcl_expand_topic_name( rcl_allocator_t allocator, char ** output_topic_name) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_TOPIC_NAME_INVALID); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID_NAME); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID_NAMESPACE); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_UNKNOWN_SUBSTITUTION); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + // check arguments that could be null RCL_CHECK_ARGUMENT_FOR_NULL(input_topic_name, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); diff --git a/rcl/src/rcl/init_options.c b/rcl/src/rcl/init_options.c index 713337d60..ebf5c7b77 100644 --- a/rcl/src/rcl/init_options.c +++ b/rcl/src/rcl/init_options.c @@ -21,6 +21,7 @@ extern "C" #include "./common.h" #include "./init_options_impl.h" +#include "rcutils/macros.h" #include "rcl/error_handling.h" #include "rmw/error_handling.h" #include "rcutils/logging_macros.h" @@ -36,6 +37,11 @@ rcl_get_zero_initialized_init_options(void) rcl_ret_t rcl_init_options_init(rcl_init_options_t * init_options, rcl_allocator_t allocator) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ARGUMENT_FOR_NULL(init_options, RCL_RET_INVALID_ARGUMENT); if (NULL != init_options->impl) { RCL_SET_ERROR_MSG("given init_options (rcl_init_options_t) is already initialized"); @@ -61,6 +67,11 @@ rcl_init_options_init(rcl_init_options_t * init_options, rcl_allocator_t allocat rcl_ret_t rcl_init_options_copy(const rcl_init_options_t * src, rcl_init_options_t * dst) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ARGUMENT_FOR_NULL(src, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(src->impl, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(dst, RCL_RET_INVALID_ARGUMENT); diff --git a/rcl/src/rcl/localhost.c b/rcl/src/rcl/localhost.c index 6cc0e18aa..7730b56cf 100644 --- a/rcl/src/rcl/localhost.c +++ b/rcl/src/rcl/localhost.c @@ -30,9 +30,9 @@ rcl_get_localhost_only(rmw_localhost_only_t * localhost_only) const char * ros_local_host_env_val = NULL; const char * get_env_error_str = NULL; - if (!localhost_only) { - return RCL_RET_INVALID_ARGUMENT; - } + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ARGUMENT_FOR_NULL(localhost_only, RCL_RET_INVALID_ARGUMENT); get_env_error_str = rcutils_get_env(RCL_LOCALHOST_ENV_VAR, &ros_local_host_env_val); if (NULL != get_env_error_str) { diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index 77bedeff0..cb6d47f21 100644 --- a/rcl/src/rcl/node_options.c +++ b/rcl/src/rcl/node_options.c @@ -17,6 +17,8 @@ extern "C" { #endif +#include "rcutils/macros.h" + #include "rcl/node_options.h" #include "rcl/arguments.h" @@ -44,6 +46,8 @@ rcl_node_options_copy( const rcl_node_options_t * options, rcl_node_options_t * options_out) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(options_out, RCL_RET_INVALID_ARGUMENT); if (options_out == options) { @@ -59,8 +63,7 @@ rcl_node_options_copy( options_out->use_global_arguments = options->use_global_arguments; options_out->enable_rosout = options->enable_rosout; if (NULL != options->arguments.impl) { - rcl_ret_t ret = rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); - return ret; + return rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); } return RCL_RET_OK; } diff --git a/rcl/src/rcl/remap.c b/rcl/src/rcl/remap.c index 950ff9761..d8af63f87 100644 --- a/rcl/src/rcl/remap.c +++ b/rcl/src/rcl/remap.c @@ -19,6 +19,7 @@ #include "rcl/error_handling.h" #include "rcl/expand_topic_name.h" #include "rcutils/allocator.h" +#include "rcutils/macros.h" #include "rcutils/strdup.h" #include "rcutils/types/string_map.h" @@ -41,6 +42,9 @@ rcl_remap_copy( const rcl_remap_t * rule, rcl_remap_t * rule_out) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCL_CHECK_ARGUMENT_FOR_NULL(rule, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(rule_out, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(rule->impl, RCL_RET_INVALID_ARGUMENT); @@ -289,6 +293,11 @@ rcl_remap_node_name( rcl_allocator_t allocator, char ** output_name) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID_NAME); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT); return _rcl_remap_name( local_arguments, global_arguments, RCL_NODENAME_REMAP, NULL, node_name, NULL, NULL, @@ -303,6 +312,11 @@ rcl_remap_node_namespace( rcl_allocator_t allocator, char ** output_namespace) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID_NAMESPACE); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT); return _rcl_remap_name( local_arguments, global_arguments, RCL_NAMESPACE_REMAP, NULL, node_name, NULL, NULL, diff --git a/rcl/src/rcl/validate_enclave_name.c b/rcl/src/rcl/validate_enclave_name.c index 0db544d3d..8cd421cba 100644 --- a/rcl/src/rcl/validate_enclave_name.c +++ b/rcl/src/rcl/validate_enclave_name.c @@ -18,6 +18,7 @@ #include #include +#include #include #include "rmw/validate_namespace.h" @@ -32,9 +33,7 @@ rcl_validate_enclave_name( int * validation_result, size_t * invalid_index) { - if (!enclave) { - return RCL_RET_INVALID_ARGUMENT; - } + RCL_CHECK_ARGUMENT_FOR_NULL(enclave, RCL_RET_INVALID_ARGUMENT); return rcl_validate_enclave_name_with_size( enclave, strlen(enclave), validation_result, invalid_index); } @@ -46,12 +45,11 @@ rcl_validate_enclave_name_with_size( int * validation_result, size_t * invalid_index) { - if (!enclave) { - return RCL_RET_INVALID_ARGUMENT; - } - if (!validation_result) { - return RCL_RET_INVALID_ARGUMENT; - } + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(enclave, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(validation_result, RCL_RET_INVALID_ARGUMENT); int tmp_validation_result; size_t tmp_invalid_index; diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 4cbf5e460..6d8ef17d4 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -74,7 +74,7 @@ function(test_target_function) SRCS rcl/test_context.cpp ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools + LIBRARIES ${PROJECT_NAME} mimick osrf_testing_tools_cpp::memory_tools AMENT_DEPENDENCIES ${rmw_implementation} ) @@ -157,7 +157,7 @@ function(test_target_function) SRCS rcl/test_init.cpp ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools + LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools mimick AMENT_DEPENDENCIES ${rmw_implementation} ) @@ -165,8 +165,9 @@ function(test_target_function) SRCS rcl/test_node.cpp ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools + LIBRARIES ${PROJECT_NAME} mimick osrf_testing_tools_cpp::memory_tools AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" + TIMEOUT 240 # Large timeout to wait for fault injection tests ) rcl_add_custom_gtest(test_arguments${target_suffix} @@ -200,7 +201,7 @@ function(test_target_function) SRCS rcl/test_guard_condition.cpp ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools + LIBRARIES ${PROJECT_NAME} mimick osrf_testing_tools_cpp::memory_tools AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" ) @@ -383,7 +384,7 @@ rcl_add_custom_gtest(test_expand_topic_name rcl_add_custom_gtest(test_security SRCS rcl/test_security.cpp APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} mimick AMENT_DEPENDENCIES "osrf_testing_tools_cpp" ) diff --git a/rcl/test/rcl/test_context.cpp b/rcl/test/rcl/test_context.cpp index 22cc6ff8b..05a104d5d 100644 --- a/rcl/test/rcl/test_context.cpp +++ b/rcl/test/rcl/test_context.cpp @@ -20,6 +20,10 @@ #include "rcl/error_handling.h" #include "rcl/init.h" +#include "rmw/rmw.h" + +#include "../mocking_utils/patch.hpp" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -148,6 +152,10 @@ TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), bad_fini) { ret = rcl_shutdown(&context); EXPECT_EQ(ret, RCL_RET_OK); - ret = rcl_context_fini(&context); - EXPECT_EQ(ret, RCL_RET_OK); + { + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rmw_context_fini, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_context_fini(&context)); + rcl_reset_error(); + } } diff --git a/rcl/test/rcl/test_guard_condition.cpp b/rcl/test/rcl/test_guard_condition.cpp index b34e68d7f..48d4ede4d 100644 --- a/rcl/test/rcl/test_guard_condition.cpp +++ b/rcl/test/rcl/test_guard_condition.cpp @@ -24,6 +24,10 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" +#include "rmw/rmw.h" + +#include "../mocking_utils/patch.hpp" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -188,6 +192,14 @@ TEST_F( ASSERT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC"; ASSERT_TRUE(rcl_error_is_set()); rcl_reset_error(); + // Try init but force an internal error. + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_create_guard_condition, "internal error", nullptr); + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } // Try fini with invalid arguments. ret = rcl_guard_condition_fini(nullptr); @@ -202,6 +214,18 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret); ret = rcl_guard_condition_fini(&guard_condition); EXPECT_EQ(RCL_RET_OK, ret); + // Try normal init and fini, but force an internal error on first try. + { + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rmw_destroy_guard_condition, RMW_RET_ERROR); + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_OK, ret); // Try repeated init and fini calls. ret = rcl_guard_condition_init(&guard_condition, &context, default_options); EXPECT_EQ(RCL_RET_OK, ret); diff --git a/rcl/test/rcl/test_init.cpp b/rcl/test/rcl/test_init.cpp index da96012dc..6ecce73d4 100644 --- a/rcl/test/rcl/test_init.cpp +++ b/rcl/test/rcl/test_init.cpp @@ -23,8 +23,12 @@ #include "rcutils/env.h" #include "rcutils/format_string.h" #include "rcutils/snprintf.h" +#include "rcutils/testing/fault_injection.h" + +#include "rmw/rmw.h" #include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" #include "../src/rcl/init_options_impl.h" #ifdef RMW_IMPLEMENTATION @@ -297,6 +301,76 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_shutdown context = rcl_get_zero_initialized_context(); } +/* Tests rcl_init() deals with internal errors correctly. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_internal_error) { + 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; + }); + FakeTestArgv test_args; + rcl_context_t context = rcl_get_zero_initialized_context(); + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_init, "internal error", RMW_RET_ERROR); + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + EXPECT_FALSE(rcl_context_is_valid(&context)); + } + + RCUTILS_FAULT_INJECTION_TEST( + { + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + + int64_t count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + + if (RCL_RET_OK == ret) { + ASSERT_TRUE(rcl_context_is_valid(&context)); + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; + } else { + ASSERT_FALSE(rcl_context_is_valid(&context)); + rcl_reset_error(); + } + + rcutils_fault_injection_set_count(count); + }); +} + +/* Tests rcl_shutdown() deals with internal errors correctly. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_shutdown_internal_error) { + 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(); + + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; + }); + EXPECT_TRUE(rcl_context_is_valid(&context)); + + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_shutdown, "internal error", RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_shutdown(&context)); + rcl_reset_error(); +} + /* Tests the rcl_get_instance_id() function. */ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_get_instance_id) { diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index 484739d90..f6b5bcabc 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -21,11 +21,18 @@ #include "rcl/rcl.h" #include "rcl/node.h" #include "rmw/rmw.h" // For rmw_get_implementation_identifier. +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.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 "rcutils/testing/fault_injection.h" #include "rcl/error_handling.h" +#include "rcl/logging.h" +#include "rcl/logging_rosout.h" + +#include "../mocking_utils/patch.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -385,24 +392,6 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_life_cycle) EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); ASSERT_TRUE(rcl_error_is_set()); rcl_reset_error(); - // Try with invalid allocator. - rcl_node_options_t options_with_invalid_allocator = rcl_node_get_default_options(); - options_with_invalid_allocator.allocator.allocate = nullptr; - options_with_invalid_allocator.allocator.deallocate = nullptr; - options_with_invalid_allocator.allocator.reallocate = nullptr; - ret = rcl_node_init(&node, name, namespace_, &context, &options_with_invalid_allocator); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; - ASSERT_TRUE(rcl_error_is_set()); - rcl_reset_error(); - // Try with failing allocator. - rcl_node_options_t options_with_failing_allocator = rcl_node_get_default_options(); - options_with_failing_allocator.allocator.allocate = failing_malloc; - options_with_failing_allocator.allocator.reallocate = failing_realloc; - ret = rcl_node_init(&node, name, namespace_, &context, &options_with_failing_allocator); - EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC"; - ASSERT_TRUE(rcl_error_is_set()); - rcl_reset_error(); - // Try fini with invalid arguments. ret = rcl_node_fini(nullptr); EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << "Expected RCL_RET_NODE_INVALID"; @@ -442,6 +431,121 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_life_cycle) } } +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_init_with_internal_errors) { + rcl_ret_t ret; + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_node_t node = rcl_get_zero_initialized_node(); + const char * name = "test_rcl_node_init_with_internal_errors"; + const char * namespace_ = "ns"; // force non-absolute namespace handling + rcl_node_options_t options = rcl_node_get_default_options(); + options.enable_rosout = true; // enable logging to cover more ground + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_init_options_init(&init_options, 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; + }); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; + }); + // Initialize logging and rosout. + ret = rcl_logging_configure(&context.global_arguments, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + }); + ret = rcl_logging_rosout_init(&allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_fini()) << rcl_get_error_string().str; + }); + // Try with invalid allocator. + rcl_node_options_t options_with_invalid_allocator = rcl_node_get_default_options(); + options_with_invalid_allocator.allocator.allocate = nullptr; + options_with_invalid_allocator.allocator.deallocate = nullptr; + options_with_invalid_allocator.allocator.reallocate = nullptr; + ret = rcl_node_init(&node, name, namespace_, &context, &options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try with failing allocator. + rcl_node_options_t options_with_failing_allocator = rcl_node_get_default_options(); + options_with_failing_allocator.allocator.allocate = failing_malloc; + options_with_failing_allocator.allocator.reallocate = failing_realloc; + ret = rcl_node_init(&node, name, namespace_, &context, &options_with_failing_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try init but force internal errors. + { + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_create_node, nullptr); + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_node_get_graph_guard_condition, nullptr); + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_node_name, RMW_RET_ERROR); + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_namespace, RMW_RET_ERROR); + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + // Try normal init but force an internal error on fini. + { + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_OK, ret); + auto mock = mocking_utils::inject_on_return("lib:rcl", rmw_destroy_node, RMW_RET_ERROR); + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + // Battle test node init. + RCUTILS_FAULT_INJECTION_TEST( + { + ret = rcl_node_init(&node, name, namespace_, &context, &options); + + int64_t count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + + if (RCL_RET_OK == ret) { + ASSERT_TRUE(rcl_node_is_valid(&node)); + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node)) << rcl_get_error_string().str; + } else { + ASSERT_FALSE(rcl_node_is_valid(&node)); + rcl_reset_error(); + } + + rcutils_fault_injection_set_count(count); + }); +} + /* Tests the node name restrictions enforcement. */ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_name_restrictions) { diff --git a/rcl/test/rcl/test_security.cpp b/rcl/test/rcl/test_security.cpp index 9fbf28d67..cea6b00e4 100644 --- a/rcl/test/rcl/test_security.cpp +++ b/rcl/test/rcl/test_security.cpp @@ -16,17 +16,22 @@ #include #include +#include #include #include "rcl/security.h" #include "rcl/error_handling.h" #include "rcutils/filesystem.h" +#include "rcutils/get_env.h" #include "rmw/error_handling.h" +#include "rmw/rmw.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" #define TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME "/test_security_directory" #define TEST_ENCLAVE "dummy_enclave" @@ -253,3 +258,148 @@ TEST_F(TestGetSecureRoot, test_get_security_options) { PATH_SEPARATOR "enclaves" PATH_SEPARATOR TEST_ENCLAVE, options.security_root_path); } + +TEST_F(TestGetSecureRoot, test_rcl_security_enabled) { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_security_enabled(nullptr)); + rcl_reset_error(); + + { + bool use_security; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_get_env, "internal error"); + EXPECT_EQ(RCL_RET_ERROR, rcl_security_enabled(&use_security)); + rcl_reset_error(); + } + + { + bool use_security = false; + putenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME "=true"); + EXPECT_EQ(RCL_RET_OK, rcl_security_enabled(&use_security)); + EXPECT_TRUE(use_security); + unsetenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME); + } + + { + bool use_security = true; + putenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME "=false"); + EXPECT_EQ(RCL_RET_OK, rcl_security_enabled(&use_security)); + EXPECT_FALSE(use_security); + unsetenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME); + } + + { + bool use_security = true; + putenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME "=foo"); + EXPECT_EQ(RCL_RET_OK, rcl_security_enabled(&use_security)); + EXPECT_FALSE(use_security); + unsetenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME); + } + + { + bool use_security = true; + EXPECT_EQ(RCL_RET_OK, rcl_security_enabled(&use_security)); + EXPECT_FALSE(use_security); + } +} + +TEST_F(TestGetSecureRoot, test_rcl_get_enforcement_policy) { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_get_enforcement_policy(nullptr)); + rcl_reset_error(); + + { + rmw_security_enforcement_policy_t policy; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_get_env, "internal error"); + EXPECT_EQ(RCL_RET_ERROR, rcl_get_enforcement_policy(&policy)); + rcl_reset_error(); + } + + { + rmw_security_enforcement_policy_t policy = RMW_SECURITY_ENFORCEMENT_PERMISSIVE; + putenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME "=Enforce"); + EXPECT_EQ(RCL_RET_OK, rcl_get_enforcement_policy(&policy)); + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_ENFORCE, policy); + unsetenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME); + } + + { + rmw_security_enforcement_policy_t policy = RMW_SECURITY_ENFORCEMENT_ENFORCE; + EXPECT_EQ(RCL_RET_OK, rcl_get_enforcement_policy(&policy)); + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_PERMISSIVE, policy); + } + + { + rmw_security_enforcement_policy_t policy = RMW_SECURITY_ENFORCEMENT_ENFORCE; + putenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME "=foo"); + EXPECT_EQ(RCL_RET_OK, rcl_get_enforcement_policy(&policy)); + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_PERMISSIVE, policy); + unsetenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME); + } + + { + rmw_security_enforcement_policy_t policy = RMW_SECURITY_ENFORCEMENT_ENFORCE; + putenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME "=ENFORCE"); + EXPECT_EQ(RCL_RET_OK, rcl_get_enforcement_policy(&policy)); + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_PERMISSIVE, policy); + unsetenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME); + } +} + +TEST_F(TestGetSecureRoot, test_rcl_get_secure_root_with_bad_arguments) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + EXPECT_EQ(nullptr, rcl_get_secure_root(nullptr, &allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ(nullptr, rcl_get_secure_root("test", nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_allocator_t invalid_allocator = rcutils_get_zero_initialized_allocator(); + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &invalid_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +TEST_F(TestGetSecureRoot, test_rcl_get_secure_root_with_internal_errors) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t failing_allocator = get_time_bombed_allocator(); + + std::map env; + auto mock = mocking_utils::patch( + "lib:rcl", rcutils_get_env, + [&](const char * name, const char ** value) -> const char * { + if (env.count(name) == 0) { + return "internal error"; + } + *value = env[name].c_str(); + return nullptr; + }); + + // fail to get ROS_SECURITY_KEYSTORE_VAR_NAME from environment + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + env[ROS_SECURITY_KEYSTORE_VAR_NAME] = + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME; + + // fail to copy ROS_SECURITY_KEYSTORE_VAR_NAME value + set_time_bombed_allocator_count(failing_allocator, 0); + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &failing_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // fail to get ROS_SECURITY_ENCLAVE_OVERRIDE from environment + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + env[ROS_SECURITY_ENCLAVE_OVERRIDE] = TEST_ENCLAVE_ABSOLUTE; + + // fail to copy ROS_SECURITY_ENCLAVE_OVERRIDE value + set_time_bombed_allocator_count(failing_allocator, 1); + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &failing_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} From e2f6526de2abd8600ae728832d1ea75a398da40c Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 31 Aug 2020 13:11:43 -0300 Subject: [PATCH 29/42] Fix wait allocation cleanup (#770) * Add fix wait.c init/fini allocation on error * Add test for the fix * Remove unit tests Signed-off-by: Jorge Perez --- rcl/src/rcl/wait.c | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 79556df9f..a23c77f7b 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -86,13 +86,13 @@ rcl_wait_set_is_valid(const rcl_wait_set_t * wait_set) } static void -__wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) +__wait_set_clean_up(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_wait_set_resize(wait_set, 0, 0, 0, 0, 0, 0); (void)ret; // NO LINT assert(RCL_RET_OK == ret); // Defensive, shouldn't fail with size 0. if (wait_set->impl) { - allocator.deallocate(wait_set->impl, allocator.state); + wait_set->impl->allocator.deallocate(wait_set->impl, wait_set->impl->allocator.state); wait_set->impl = NULL; } } @@ -146,6 +146,10 @@ rcl_wait_set_init( wait_set->impl->rmw_services.service_count = 0; wait_set->impl->rmw_events.events = NULL; wait_set->impl->rmw_events.event_count = 0; + // Set context. + wait_set->impl->context = context; + // Set allocator. + wait_set->impl->allocator = allocator; size_t num_conditions = (2 * number_of_subscriptions) + @@ -159,10 +163,6 @@ rcl_wait_set_init( goto fail; } - // Set context. - wait_set->impl->context = context; - // Set allocator. - wait_set->impl->allocator = allocator; // Initialize subscription space. rcl_ret_t ret = rcl_wait_set_resize( wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers, @@ -179,7 +179,7 @@ rcl_wait_set_init( fail_ret = RCL_RET_WAIT_SET_INVALID; } } - __wait_set_clean_up(wait_set, allocator); + __wait_set_clean_up(wait_set); return fail_ret; } @@ -195,7 +195,7 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_WAIT_SET_INVALID; } - __wait_set_clean_up(wait_set, wait_set->impl->allocator); + __wait_set_clean_up(wait_set); } return result; } @@ -300,6 +300,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al wait_set->impl->RMWStorage, sizeof(void *) * Type ## s_size, allocator.state); \ if (!wait_set->impl->RMWStorage) { \ allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ + wait_set->Type ## s = NULL; \ wait_set->size_of_ ## Type ## s = 0; \ RCL_SET_ERROR_MSG("allocating memory failed"); \ return RCL_RET_BAD_ALLOC; \ From a72ae0a3c0963d172fd3a3946c15ab35bfd31128 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 1 Sep 2020 12:23:49 -0300 Subject: [PATCH 30/42] Add coverage tests wait module (#769) * Add coverage tests wait module * Add fault injection test * Add zero_init allocator test to init * Remove bomb allocator test * Add better handling fault injection * Add comment to fault injection tests * Move test to another section Signed-off-by: Jorge Perez --- rcl/test/CMakeLists.txt | 2 +- rcl/test/rcl/test_wait.cpp | 92 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 93 insertions(+), 1 deletion(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 6d8ef17d4..b835f4564 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -263,7 +263,7 @@ function(test_target_function) SRCS rcl/test_wait.cpp ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} mimick AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" ) diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 82244406f..895b9d593 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -30,6 +30,7 @@ #include "rcutils/logging_macros.h" #include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -646,6 +647,28 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_valid_argumen rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, ¬_init_context, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_NOT_INIT, ret) << rcl_get_error_string().str; rcl_reset_error(); + + // nullptr failures + ret = + rcl_wait_set_init(nullptr, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_allocator_t zero_init_allocator = + static_cast(rcutils_get_zero_initialized_allocator()); + ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, zero_init_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -684,3 +707,72 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_get_allocator ret = rcl_wait_set_fini(&wait_set); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } + +// Test wait set init failure cases using mocks +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_failed_init) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + // Mock rmw implementation to fail init + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_create_wait_set, nullptr); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_WAIT_SET_INVALID, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +// Test wait set fini failure cases using mocks +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_failed_fini) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret); + { + // Mock rmw implementation to fail fini + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rmw_destroy_wait_set, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_WAIT_SET_INVALID, rcl_wait_set_fini(&wait_set)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} + +// Test proper error handling with a fault injection test +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_test_maybe_init_fail) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = RCL_RET_OK; + rcl_allocator_t alloc = rcl_get_default_allocator(); + + RCUTILS_FAULT_INJECTION_TEST( + { + // Test init in the case where guard_conditions_size + timers_size = 0 + // (used for guard condition size) + ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 1, 1, 0, context_ptr, alloc); + if (RCL_RET_OK == ret) { + ret = rcl_wait_set_fini(&wait_set); + if (ret != RCL_RET_OK) { + rcl_reset_error(); + } + } else { + EXPECT_TRUE(RCL_RET_WAIT_SET_INVALID == ret || RCL_RET_BAD_ALLOC == ret); + rcl_reset_error(); + } + }); + + RCUTILS_FAULT_INJECTION_TEST( + { + // Test init wait_set using at least one of each of the possible elements that receives + // (subs, guard conditions, timers, clients, services, events) + ret = rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 1, context_ptr, alloc); + if (RCL_RET_OK == ret) { + ret = rcl_wait_set_fini(&wait_set); + if (ret != RCL_RET_OK) { + rcl_reset_error(); + } + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); + } else { + EXPECT_TRUE(RCL_RET_WAIT_SET_INVALID == ret || RCL_RET_BAD_ALLOC == ret); + rcl_reset_error(); + } + }); +} From 7ee8e118ed44a53864fd39dc6944f0021f97b7ce Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 1 Sep 2020 14:32:09 -0300 Subject: [PATCH 31/42] Fix rcl arguments' API memory leaks and bugs. (#778) Signed-off-by: Michel Hidalgo --- rcl/src/rcl/arguments.c | 75 ++++++++++++++++++++++++----------------- 1 file changed, 44 insertions(+), 31 deletions(-) diff --git a/rcl/src/rcl/arguments.c b/rcl/src/rcl/arguments.c index e76637022..ce7bf7520 100644 --- a/rcl/src/rcl/arguments.c +++ b/rcl/src/rcl/arguments.c @@ -576,17 +576,20 @@ rcl_parse_arguments( } // Shrink remap_rules array to match number of successfully parsed rules - if (args_impl->num_remap_rules > 0) { - args_impl->remap_rules = rcutils_reallocf( - args_impl->remap_rules, sizeof(rcl_remap_t) * args_impl->num_remap_rules, &allocator); - if (NULL == args_impl->remap_rules) { - ret = RCL_RET_BAD_ALLOC; - goto fail; - } - } else { + if (0 == args_impl->num_remap_rules) { // No remap rules allocator.deallocate(args_impl->remap_rules, allocator.state); args_impl->remap_rules = NULL; + } else if (args_impl->num_remap_rules < argc) { + rcl_remap_t * new_remap_rules = allocator.reallocate( + args_impl->remap_rules, + sizeof(rcl_remap_t) * args_impl->num_remap_rules, + &allocator); + if (NULL == new_remap_rules) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + args_impl->remap_rules = new_remap_rules; } // Shrink Parameter files @@ -594,12 +597,15 @@ rcl_parse_arguments( allocator.deallocate(args_impl->parameter_files, allocator.state); args_impl->parameter_files = NULL; } else if (args_impl->num_param_files_args < argc) { - args_impl->parameter_files = rcutils_reallocf( - args_impl->parameter_files, sizeof(char *) * args_impl->num_param_files_args, &allocator); - if (NULL == args_impl->parameter_files) { + char ** new_parameter_files = allocator.reallocate( + args_impl->parameter_files, + sizeof(char *) * args_impl->num_param_files_args, + &allocator); + if (NULL == new_parameter_files) { ret = RCL_RET_BAD_ALLOC; goto fail; } + args_impl->parameter_files = new_parameter_files; } // Drop parameter overrides if none was found. @@ -843,7 +849,6 @@ rcl_arguments_copy( } return RCL_RET_BAD_ALLOC; } - args_out->impl->num_remap_rules = args->impl->num_remap_rules; for (int i = 0; i < args->impl->num_remap_rules; ++i) { args_out->impl->remap_rules[i] = rcl_get_zero_initialized_remap(); ret = rcl_remap_copy( @@ -854,6 +859,7 @@ rcl_arguments_copy( } return ret; } + ++(args_out->impl->num_remap_rules); } } @@ -873,7 +879,6 @@ rcl_arguments_copy( } return RCL_RET_BAD_ALLOC; } - args_out->impl->num_param_files_args = args->impl->num_param_files_args; for (int i = 0; i < args->impl->num_param_files_args; ++i) { args_out->impl->parameter_files[i] = rcutils_strdup(args->impl->parameter_files[i], allocator); @@ -883,6 +888,7 @@ rcl_arguments_copy( } return RCL_RET_BAD_ALLOC; } + ++(args_out->impl->num_param_files_args); } } char * enclave_copy = rcutils_strdup(args->impl->enclave, allocator); @@ -1619,9 +1625,8 @@ _rcl_parse_remap_rule( RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(output_rule, RCL_RET_INVALID_ARGUMENT); - rcl_ret_t ret; - - output_rule->impl = allocator.allocate(sizeof(rcl_remap_impl_t), allocator.state); + output_rule->impl = + allocator.allocate(sizeof(rcl_remap_impl_t), allocator.state); if (NULL == output_rule->impl) { return RCL_RET_BAD_ALLOC; } @@ -1632,25 +1637,31 @@ _rcl_parse_remap_rule( output_rule->impl->replacement = NULL; rcl_lexer_lookahead2_t lex_lookahead = rcl_get_zero_initialized_lexer_lookahead2(); + rcl_ret_t ret = rcl_lexer_lookahead2_init(&lex_lookahead, arg, allocator); - ret = rcl_lexer_lookahead2_init(&lex_lookahead, arg, allocator); - if (RCL_RET_OK != ret) { - return ret; - } - - ret = _rcl_parse_remap_begin_remap_rule(&lex_lookahead, output_rule); + if (RCL_RET_OK == ret) { + ret = _rcl_parse_remap_begin_remap_rule(&lex_lookahead, output_rule); - if (RCL_RET_OK != ret) { - // cleanup stuff, but return the original error code - if (RCL_RET_OK != rcl_remap_fini(output_rule)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini remap rule after error occurred"); - } - if (RCL_RET_OK != rcl_lexer_lookahead2_fini(&lex_lookahead)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini lookahead2 after error occurred"); + rcl_ret_t fini_ret = rcl_lexer_lookahead2_fini(&lex_lookahead); + if (RCL_RET_OK != ret) { + if (RCL_RET_OK != fini_ret) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to fini lookahead2 after error occurred"); + } + } else { + if (RCL_RET_OK == fini_ret) { + return RCL_RET_OK; + } + ret = fini_ret; } - } else { - ret = rcl_lexer_lookahead2_fini(&lex_lookahead); } + + // cleanup output rule but keep first error return code + if (RCL_RET_OK != rcl_remap_fini(output_rule)) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to fini remap rule after error occurred"); + } + return ret; } @@ -1747,6 +1758,8 @@ _rcl_parse_param_file( return RCL_RET_BAD_ALLOC; } if (!rcl_parse_yaml_file(*param_file, params)) { + allocator.deallocate(*param_file, allocator.state); + *param_file = NULL; // Error message already set. return RCL_RET_ERROR; } From ece525132fcaa08cdd1e99fe12de4c72fb7db921 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 1 Sep 2020 16:15:02 -0300 Subject: [PATCH 32/42] Bump rcl arguments' API test coverage. (#777) Signed-off-by: Michel Hidalgo --- rcl/src/rcl/lexer.c | 3 + rcl/src/rcl/lexer_lookahead.c | 12 +++ rcl/test/rcl/test_arguments.cpp | 177 ++++++++++++++++++++++---------- 3 files changed, 135 insertions(+), 57 deletions(-) diff --git a/rcl/src/rcl/lexer.c b/rcl/src/rcl/lexer.c index 0442048c6..6307d52ac 100644 --- a/rcl/src/rcl/lexer.c +++ b/rcl/src/rcl/lexer.c @@ -600,6 +600,9 @@ rcl_lexer_analyze( rcl_lexeme_t * lexeme, size_t * length) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ARGUMENT_FOR_NULL(text, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(lexeme, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(length, RCL_RET_INVALID_ARGUMENT); diff --git a/rcl/src/rcl/lexer_lookahead.c b/rcl/src/rcl/lexer_lookahead.c index 6ce787d40..a5b258957 100644 --- a/rcl/src/rcl/lexer_lookahead.c +++ b/rcl/src/rcl/lexer_lookahead.c @@ -48,6 +48,9 @@ rcl_lexer_lookahead2_init( const char * text, rcl_allocator_t allocator) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(buffer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(text, RCL_RET_INVALID_ARGUMENT); @@ -93,6 +96,8 @@ rcl_lexer_lookahead2_peek( rcl_lexer_lookahead2_t * buffer, rcl_lexeme_t * next_type) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(buffer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( buffer->impl, "buffer not initialized", return RCL_RET_INVALID_ARGUMENT); @@ -126,6 +131,8 @@ rcl_lexer_lookahead2_peek2( rcl_lexeme_t * next_type1, rcl_lexeme_t * next_type2) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + rcl_ret_t ret; // Peek 1 ahead first (reusing its error checking for buffer and next_type1) ret = rcl_lexer_lookahead2_peek(buffer, next_type1); @@ -161,6 +168,9 @@ rcl_lexer_lookahead2_accept( const char ** lexeme_text, size_t * lexeme_text_length) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ARGUMENT_FOR_NULL(buffer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( buffer->impl, "buffer not initialized", return RCL_RET_INVALID_ARGUMENT); @@ -209,6 +219,8 @@ rcl_lexer_lookahead2_expect( const char ** lexeme_text, size_t * lexeme_text_length) { + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_WRONG_LEXEME); + rcl_ret_t ret; rcl_lexeme_t lexeme; diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp index 7248051b3..9e7bcecf4 100644 --- a/rcl/test/rcl/test_arguments.cpp +++ b/rcl/test/rcl/test_arguments.cpp @@ -26,6 +26,8 @@ #include "rcl_yaml_param_parser/parser.h" +#include "rcutils/testing/fault_injection.h" + #include "./allocator_testing_utils.h" #include "./arguments_impl.h" @@ -227,21 +229,18 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_inval EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--remap"})); - + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", ":"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "1"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "~"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", ":="})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "foo:="})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", ":=bar"})); - - EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p"})); - EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--params-file"})); - - EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":="})); - EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "foo:="})); - EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":=bar"})); - EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__ns:="})); - + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "::="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "1:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "~:="})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__node:="})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__node:=/foo/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__ns:="})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__ns:=foo"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", ":__node:=nodename"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "~:__node:=nodename"})); @@ -254,11 +253,31 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_inval EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "rostopic://:=rosservice"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "rostopic::=rosservice"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "foo:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":=bar"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "1"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "~"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "::="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "1:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "~:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "__node:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "__node:=/foo/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "__ns:=foo"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":__node:=nodename"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "~:__node:=nodename"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "}foo:=/bar"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--param", "}foo:=/bar"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "f oo:=/bar"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--param", "f oo:=/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-e"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--enclave"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--params-file"})); EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--log-config-file"})); @@ -1032,8 +1051,9 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_arguments rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); ret = rcl_arguments_copy(&parsed_args, &copied_args); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_EQ(2, rcl_arguments_get_param_files_count(&copied_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&copied_args)); } TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_param_overrides) { @@ -1164,40 +1184,6 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_get_p EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; } -TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_allocs_copy) { - const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); - const std::string parameters_filepath2 = (test_path / "test_parameters.2.yaml").string(); - const char * const argv[] = { - "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str(), - "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath2.c_str(), - "-r", "/foo/bar:=/fiz/buz", "--remap", "foo:=/baz", - "-e", "/foo", "--", "foo" - }; - const int argc = sizeof(argv) / sizeof(const char *); - - rcl_allocator_t alloc = rcl_get_default_allocator(); - rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); - - rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); - }); - - rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); - rcl_allocator_t bomb_alloc = get_time_bombed_allocator(); - rcl_allocator_t saved_alloc = parsed_args.impl->allocator; - parsed_args.impl->allocator = bomb_alloc; - for (int i = 0; i < 8; i++) { - set_time_bombed_allocator_count(bomb_alloc, i); - ret = rcl_arguments_copy(&parsed_args, &copied_args); - EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; - rcl_reset_error(); - } - parsed_args.impl->allocator = saved_alloc; -} - TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_get_param_files) { const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); const char * const argv[] = { @@ -1232,25 +1218,102 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_get_param_ rcl_reset_error(); } -TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_parse_arg) { - const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_parse_with_internal_errors) { + const std::string parameters_filepath1 = + (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = + (test_path / "test_parameters.2.yaml").string(); const char * const argv[] = { - "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str() + "process_name", RCL_ROS_ARGS_FLAG, + RCL_PARAM_FILE_FLAG, parameters_filepath1.c_str(), + RCL_REMAP_FLAG, "that_node:foo:=baz", + RCL_REMAP_FLAG, "foo:=bar", + RCL_PARAM_FILE_FLAG, parameters_filepath2.c_str(), + RCL_REMAP_FLAG, "__name:=my_node", + RCL_REMAP_FLAG, "__ns:=/my_ns", + RCL_PARAM_FLAG, "testing:=true", + RCL_PARAM_FLAG, "this_node:constant:=42", + RCL_ENCLAVE_FLAG, "fizz", + RCL_ENCLAVE_FLAG, "buzz", // override + RCL_LOG_LEVEL_FLAG, "rcl:=debug", + RCL_EXTERNAL_LOG_CONFIG_FLAG, "flip.txt", + RCL_EXTERNAL_LOG_CONFIG_FLAG, "flop.txt", // override + "--enable-" RCL_LOG_STDOUT_FLAG_SUFFIX, + "--enable-" RCL_LOG_ROSOUT_FLAG_SUFFIX, + "--disable-" RCL_LOG_EXT_LIB_FLAG_SUFFIX, + "--not-a-real-ros-flag", "not-a-real-ros-arg", + RCL_ROS_ARGS_EXPLICIT_END_TOKEN, + "--some-non-ros-flag", "some-non-ros-flag" }; - const int argc = sizeof(argv) / sizeof(const char *); + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + RCUTILS_FAULT_INJECTION_TEST( + { + rcl_ret_t ret = rcl_parse_arguments(argc, argv, allocator, &parsed_args); + if (RCL_RET_OK == ret) { + int64_t count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + ret = rcl_arguments_fini(&parsed_args); + rcutils_fault_injection_set_count(count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } else { + rcl_reset_error(); + } + }); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_with_internal_errors) { + const std::string parameters_filepath1 = + (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = + (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", RCL_ROS_ARGS_FLAG, + RCL_PARAM_FILE_FLAG, parameters_filepath1.c_str(), + RCL_REMAP_FLAG, "that_node:foo:=baz", + RCL_REMAP_FLAG, "foo:=bar", + RCL_PARAM_FILE_FLAG, parameters_filepath2.c_str(), + RCL_REMAP_FLAG, "__name:=my_node", + RCL_REMAP_FLAG, "__ns:=/my_ns", + RCL_PARAM_FLAG, "testing:=true", + RCL_PARAM_FLAG, "this_node:constant:=42", + RCL_ENCLAVE_FLAG, "fizz", + RCL_ENCLAVE_FLAG, "buzz", // override + RCL_LOG_LEVEL_FLAG, "rcl:=debug", + RCL_EXTERNAL_LOG_CONFIG_FLAG, "flip.txt", + RCL_EXTERNAL_LOG_CONFIG_FLAG, "flop.txt", // override + "--enable-" RCL_LOG_STDOUT_FLAG_SUFFIX, + "--enable-" RCL_LOG_ROSOUT_FLAG_SUFFIX, + "--disable-" RCL_LOG_EXT_LIB_FLAG_SUFFIX, + "--not-a-real-ros-flag", "not-a-real-ros-arg", + RCL_ROS_ARGS_EXPLICIT_END_TOKEN, + "--some-non-ros-flag", "some-non-ros-flag" + }; + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); - rcl_allocator_t bomb_alloc = get_time_bombed_allocator(); - for (int i = 0; i < 100; i++) { - set_time_bombed_allocator_count(bomb_alloc, i); - rcl_ret_t ret = rcl_parse_arguments(argc, argv, bomb_alloc, &parsed_args); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + RCUTILS_FAULT_INJECTION_TEST( + { + rcl_ret_t ret = rcl_arguments_copy(&parsed_args, &copied_args); if (RCL_RET_OK == ret) { - EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); - break; + int64_t count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + ret = rcl_arguments_fini(&copied_args); + rcutils_fault_injection_set_count(count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } else { - EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); rcl_reset_error(); } - } + }); } From 14d17846aa99cb594bce6aab314de81e0ce48878 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 30 Oct 2020 12:43:24 +0100 Subject: [PATCH 33/42] Fixed log level in backported foxy tests Signed-off-by: ahcorde --- rcl/test/rcl/test_arguments.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp index 9e7bcecf4..6dca6d26b 100644 --- a/rcl/test/rcl/test_arguments.cpp +++ b/rcl/test/rcl/test_arguments.cpp @@ -1235,7 +1235,6 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_parse_with_inte RCL_PARAM_FLAG, "this_node:constant:=42", RCL_ENCLAVE_FLAG, "fizz", RCL_ENCLAVE_FLAG, "buzz", // override - RCL_LOG_LEVEL_FLAG, "rcl:=debug", RCL_EXTERNAL_LOG_CONFIG_FLAG, "flip.txt", RCL_EXTERNAL_LOG_CONFIG_FLAG, "flop.txt", // override "--enable-" RCL_LOG_STDOUT_FLAG_SUFFIX, @@ -1280,7 +1279,6 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_with_inter RCL_PARAM_FLAG, "this_node:constant:=42", RCL_ENCLAVE_FLAG, "fizz", RCL_ENCLAVE_FLAG, "buzz", // override - RCL_LOG_LEVEL_FLAG, "rcl:=debug", RCL_EXTERNAL_LOG_CONFIG_FLAG, "flip.txt", RCL_EXTERNAL_LOG_CONFIG_FLAG, "flop.txt", // override "--enable-" RCL_LOG_STDOUT_FLAG_SUFFIX, From 8b9d9cc380decf35952fcd03fe0c72cdc2db743d Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 8 Sep 2020 22:39:39 +0800 Subject: [PATCH 34/42] calling fini functions to avoid memory leak (#791) * Fix test memory leaks 1. calling rcutils_string_map_fini to avoid memory leak 2. Fix memory leak that not to call rcutils_string_array_fini for enclaves 3. Fix that not to rcutils_string_array_fini for node_names_2 and node_namespaces_2 4. Fix that not to rcl_log_levels_fini for copied_log_levels 5. Fix that not to call rmw_security_options_fini for options 6. Call test_msgs__srv__BasicTypes_Request__fini for service_request in the end Signed-off-by: Chen Lihui --- rcl/test/rcl/test_expand_topic_name.cpp | 3 +++ rcl/test/rcl/test_get_node_names.cpp | 3 +++ rcl/test/rcl/test_graph.cpp | 4 ++++ rcl/test/rcl/test_security.cpp | 1 + rcl/test/rcl/test_service.cpp | 4 ++++ 5 files changed, 15 insertions(+) diff --git a/rcl/test/rcl/test_expand_topic_name.cpp b/rcl/test/rcl/test_expand_topic_name.cpp index 308fbed68..8ec900a93 100644 --- a/rcl/test/rcl/test_expand_topic_name.cpp +++ b/rcl/test/rcl/test_expand_topic_name.cpp @@ -226,6 +226,9 @@ TEST(test_expand_topic_name, internal_error) { EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); } + + ret = rcutils_string_map_fini(&subs); + ASSERT_EQ(RCL_RET_OK, ret); } TEST(test_expand_topic_name, various_valid_topics) { diff --git a/rcl/test/rcl/test_get_node_names.cpp b/rcl/test/rcl/test_get_node_names.cpp index 734dc1d4a..80a0da47c 100644 --- a/rcl/test/rcl/test_get_node_names.cpp +++ b/rcl/test/rcl/test_get_node_names.cpp @@ -279,6 +279,9 @@ TEST_F( ret = rcutils_string_array_fini(&node_namespaces); ASSERT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&enclaves); + ASSERT_EQ(RCUTILS_RET_OK, ret); + ret = rcl_node_fini(&node1); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 6d8d83a2f..ad95b5133 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -1435,6 +1435,10 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_bad_get_node_names) EXPECT_EQ(RCUTILS_RET_OK, ret); ret = rcutils_string_array_fini(&node_namespaces); EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_names_2); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_namespaces_2); + EXPECT_EQ(RCUTILS_RET_OK, ret); ret = rcutils_string_array_fini(&node_enclaves); EXPECT_EQ(RCUTILS_RET_OK, ret); }); diff --git a/rcl/test/rcl/test_security.cpp b/rcl/test/rcl/test_security.cpp index cea6b00e4..688e37cdd 100644 --- a/rcl/test/rcl/test_security.cpp +++ b/rcl/test/rcl/test_security.cpp @@ -257,6 +257,7 @@ TEST_F(TestGetSecureRoot, test_get_security_options) { TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME PATH_SEPARATOR "enclaves" PATH_SEPARATOR TEST_ENCLAVE, options.security_root_path); + EXPECT_EQ(RMW_RET_OK, rmw_security_options_fini(&options, &allocator)); } TEST_F(TestGetSecureRoot, test_rcl_security_enabled) { diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index 9ec820f8c..b4f6df67c 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/rcl/test/rcl/test_service.cpp @@ -537,6 +537,10 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_take_request test_msgs__srv__BasicTypes_Request service_request; test_msgs__srv__BasicTypes_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Request__fini(&service_request); + }); rmw_service_info_t header; ret = rcl_take_request_with_info(nullptr, &header, &service_request); From 99c96eaf426031b22337e94cd0ecabfe6f34339b Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 9 Sep 2020 04:17:08 +0800 Subject: [PATCH 35/42] Fix that not to deallocate event impl in some failure case (#790) * Fix that not to deallocate event impl in some failure case Signed-off-by: Chen Lihui Co-authored-by: Chris Lalancette --- rcl/src/rcl/event.c | 64 ++++++++++++++++++++++++++++----------------- 1 file changed, 40 insertions(+), 24 deletions(-) diff --git a/rcl/src/rcl/event.c b/rcl/src/rcl/event.c index 81ccf7c50..c441019f3 100644 --- a/rcl/src/rcl/event.c +++ b/rcl/src/rcl/event.c @@ -52,22 +52,11 @@ rcl_publisher_event_init( const rcl_publisher_t * publisher, const rcl_publisher_event_type_t event_type) { - rcl_ret_t ret = RCL_RET_OK; RCL_CHECK_ARGUMENT_FOR_NULL(event, RCL_RET_EVENT_INVALID); // Check publisher and allocator first, so allocator can be used with errors. RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); rcl_allocator_t * allocator = &publisher->impl->options.allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); - - // Allocate space for the implementation struct. - event->impl = (rcl_event_impl_t *) allocator->allocate( - sizeof(rcl_event_impl_t), allocator->state); - RCL_CHECK_FOR_NULL_WITH_MSG( - event->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; return ret); - - event->impl->rmw_handle = rmw_get_zero_initialized_event(); - event->impl->allocator = *allocator; - rmw_event_type_t rmw_event_type = RMW_EVENT_INVALID; switch (event_type) { case RCL_PUBLISHER_OFFERED_DEADLINE_MISSED: @@ -83,10 +72,29 @@ rcl_publisher_event_init( RCL_SET_ERROR_MSG("Event type for publisher not supported"); return RCL_RET_INVALID_ARGUMENT; } - return rmw_publisher_event_init( + + // Allocate space for the implementation struct. + event->impl = (rcl_event_impl_t *) allocator->allocate( + sizeof(rcl_event_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + event->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + + event->impl->rmw_handle = rmw_get_zero_initialized_event(); + event->impl->allocator = *allocator; + + rmw_ret_t ret = rmw_publisher_event_init( &event->impl->rmw_handle, publisher->impl->rmw_handle, rmw_event_type); + if (ret != RMW_RET_OK) { + goto fail; + } + + return RCL_RET_OK; +fail: + allocator->deallocate(event->impl, allocator->state); + event->impl = NULL; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } rcl_ret_t @@ -95,22 +103,11 @@ rcl_subscription_event_init( const rcl_subscription_t * subscription, const rcl_subscription_event_type_t event_type) { - rcl_ret_t ret = RCL_RET_OK; RCL_CHECK_ARGUMENT_FOR_NULL(event, RCL_RET_EVENT_INVALID); // Check subscription and allocator first, so allocator can be used with errors. RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); rcl_allocator_t * allocator = &subscription->impl->options.allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); - - // Allocate space for the implementation struct. - event->impl = (rcl_event_impl_t *) allocator->allocate( - sizeof(rcl_event_impl_t), allocator->state); - RCL_CHECK_FOR_NULL_WITH_MSG( - event->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; return ret); - - event->impl->rmw_handle = rmw_get_zero_initialized_event(); - event->impl->allocator = *allocator; - rmw_event_type_t rmw_event_type = RMW_EVENT_INVALID; switch (event_type) { case RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED: @@ -126,10 +123,29 @@ rcl_subscription_event_init( RCL_SET_ERROR_MSG("Event type for subscription not supported"); return RCL_RET_INVALID_ARGUMENT; } - return rmw_subscription_event_init( + + // Allocate space for the implementation struct. + event->impl = (rcl_event_impl_t *) allocator->allocate( + sizeof(rcl_event_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + event->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + + event->impl->rmw_handle = rmw_get_zero_initialized_event(); + event->impl->allocator = *allocator; + + rmw_ret_t ret = rmw_subscription_event_init( &event->impl->rmw_handle, subscription->impl->rmw_handle, rmw_event_type); + if (ret != RMW_RET_OK) { + goto fail; + } + + return RCL_RET_OK; +fail: + allocator->deallocate(event->impl, allocator->state); + event->impl = NULL; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } rcl_ret_t From f3a7318dda412e5cf170b9803051401b21a6ccd9 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 15 Sep 2020 22:28:10 +0800 Subject: [PATCH 36/42] Fix memory leak because of mock test (#800) * Fix memory leak because of mock test Signed-off-by: Chen Lihui Co-authored-by: Chris Lalancette --- rcl/test/rcl/test_init.cpp | 70 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/rcl/test/rcl/test_init.cpp b/rcl/test/rcl/test_init.cpp index 6ecce73d4..2abf6f748 100644 --- a/rcl/test/rcl/test_init.cpp +++ b/rcl/test/rcl/test_init.cpp @@ -458,3 +458,73 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_options_acce 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)); } + +// Define dummy comparison operators for rcutils_allocator_t type for use with the Mimick Library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) + +// Tests rcl_init_options_init() mocked to fail +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_mocked_rcl_init_options_ini) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_init_options_init, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_init_options_init(&init_options, rcl_get_default_allocator())); + rcl_reset_error(); +} + +// Tests rcl_init_options_fini() mocked to fail +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_mocked_rcl_init_options_fini) { + 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; + auto mock = mocking_utils::inject_on_return("lib:rcl", rmw_init_options_fini, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_init_options_fini(&init_options)); + rcl_reset_error(); + auto mock_ok = mocking_utils::inject_on_return("lib:rcl", rmw_init_options_fini, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)); +} + +// Mock rcl_init_options_copy to fail +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_copy_mocked_fail_fini) { + 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_init_options_t init_options_dst = rcl_get_zero_initialized_init_options(); + auto mock = mocking_utils::inject_on_return("lib:rcl", rmw_init_options_fini, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_init_options_copy(&init_options, &init_options_dst)); + rcl_reset_error(); + auto mock_ok = mocking_utils::inject_on_return("lib:rcl", rmw_init_options_fini, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options_dst)); +} + +// Mock rcl_init_options_copy to fail +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_options_copy_fail_rmw_copy) { + 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_init_options_t init_options_dst = rcl_get_zero_initialized_init_options(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + // dst is in a invalid state after failed copy + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_init_options_fini(&init_options_dst)) << rcl_get_error_string().str; + rcl_reset_error(); + auto mock_ok = mocking_utils::patch_and_return("lib:rcl", rmw_init_options_fini, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options_dst)) << rcl_get_error_string().str; + }); + + // rmw_init_options_copy error is logged + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_init_options_copy, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_init_options_copy(&init_options, &init_options_dst)); + rcl_reset_error(); +} From f9cea34ac130f25e845ad80b1a7e41e49420e7ed Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 19 Oct 2020 10:33:58 -0400 Subject: [PATCH 37/42] Make sure to check the return value of rcl APIs. (#838) This is just a further check to ensure the test is correct, and also gets rid of a slew of dead store warnings from clang static analysis. Signed-off-by: Chris Lalancette --- rcl/test/rcl/client_fixture.cpp | 5 +++++ rcl/test/rcl/service_fixture.cpp | 5 +++++ rcl/test/rcl/test_count_matched.cpp | 1 + rcl/test/rcl/test_graph.cpp | 1 + rcl/test/rcl/test_rmw_impl_id_check_exe.cpp | 3 +++ rcl/test/rcl/test_subscription.cpp | 6 ++++++ rcl/test/test_namespace.cpp | 2 ++ rcl_action/test/rcl_action/test_action_client.cpp | 1 - .../test/rcl_action/test_action_communication.cpp | 5 +++++ .../test/rcl_action/test_action_interaction.cpp | 13 +++++++++++++ rcl_action/test/rcl_action/test_graph.cpp | 1 + rcl_action/test/rcl_action/test_wait.cpp | 6 ++++++ rcl_lifecycle/test/test_rcl_lifecycle.cpp | 1 + rcl_lifecycle/test/test_transition_map.cpp | 3 +-- 14 files changed, 50 insertions(+), 3 deletions(-) diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp index c6d035a9c..c20caafc4 100644 --- a/rcl/test/rcl/client_fixture.cpp +++ b/rcl/test/rcl/client_fixture.cpp @@ -54,6 +54,11 @@ int main(int argc, char ** argv) } }); ret = rcl_init_options_fini(&init_options); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in options fini: %s", rcl_get_error_string().str); + return -1; + } rcl_node_t node = rcl_get_zero_initialized_node(); const char * name = "client_fixture_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); diff --git a/rcl/test/rcl/service_fixture.cpp b/rcl/test/rcl/service_fixture.cpp index 0dc7fdcd8..c3fdaea05 100644 --- a/rcl/test/rcl/service_fixture.cpp +++ b/rcl/test/rcl/service_fixture.cpp @@ -59,6 +59,11 @@ int main(int argc, char ** argv) } }); ret = rcl_init_options_fini(&init_options); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in options fini: %s", rcl_get_error_string().str); + return -1; + } rcl_node_t node = rcl_get_zero_initialized_node(); const char * name = "service_fixture_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp index 3b236d0eb..b0baaa8ba 100644 --- a/rcl/test/rcl/test_count_matched.cpp +++ b/rcl/test/rcl/test_count_matched.cpp @@ -126,6 +126,7 @@ class CLASSNAME (TestCountFixture, RMW_IMPLEMENTATION) : public ::testing::Test *this->wait_set_ptr = rcl_get_zero_initialized_wait_set(); ret = rcl_wait_set_init( this->wait_set_ptr, 0, 1, 0, 0, 0, 0, this->context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } void TearDown() diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index ad95b5133..66af443d5 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -888,6 +888,7 @@ class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEME this->remote_context_ptr = new rcl_context_t; *this->remote_context_ptr = rcl_get_zero_initialized_context(); ret = rcl_init(0, nullptr, &init_options, this->remote_context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_node_init( remote_node_ptr, remote_node_name, "", this->remote_context_ptr, diff --git a/rcl/test/rcl/test_rmw_impl_id_check_exe.cpp b/rcl/test/rcl/test_rmw_impl_id_check_exe.cpp index 03bb29230..688b3e64f 100644 --- a/rcl/test/rcl/test_rmw_impl_id_check_exe.cpp +++ b/rcl/test/rcl/test_rmw_impl_id_check_exe.cpp @@ -27,6 +27,9 @@ int main(int, char **) return ret; } ret = rcl_init_options_fini(&init_options); + if (ret != RCL_RET_OK) { + return ret; + } ret = rcl_shutdown(&context); if (ret != RCL_RET_OK) { return ret; diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 471a3110a..af35023a9 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -420,7 +420,9 @@ TEST_F( test_msgs__msg__Strings__init(&msg); ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__Strings__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -488,9 +490,13 @@ TEST_F( test_msgs__msg__Strings__init(&msg); ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__Strings__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; diff --git a/rcl/test/test_namespace.cpp b/rcl/test/test_namespace.cpp index ba0927742..3252e3e83 100644 --- a/rcl/test/test_namespace.cpp +++ b/rcl/test/test_namespace.cpp @@ -106,6 +106,7 @@ TEST_F(TestNamespaceFixture, test_client_server) { bool is_available = false; for (auto i = 0; i < timeout; ++i) { ret = rcl_service_server_is_available(this->node_ptr, &unmatched_client, &is_available); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; if (is_available) { // this should not happen break; @@ -128,6 +129,7 @@ TEST_F(TestNamespaceFixture, test_client_server) { is_available = false; for (auto i = 0; i < timeout; ++i) { ret = rcl_service_server_is_available(this->node_ptr, &matched_client, &is_available); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; if (is_available) { break; } diff --git a/rcl_action/test/rcl_action/test_action_client.cpp b/rcl_action/test/rcl_action/test_action_client.cpp index 8fed2e955..b091809ce 100644 --- a/rcl_action/test/rcl_action/test_action_client.cpp +++ b/rcl_action/test/rcl_action/test_action_client.cpp @@ -165,7 +165,6 @@ TEST_F(TestActionClientBaseFixture, test_action_client_init_fini) { EXPECT_EQ(ret, RCL_RET_BAD_ALLOC) << rcl_get_error_string().str; rcl_reset_error(); - ret = RCL_RET_OK; int i = 0; do { time_bomb_state.malloc_count_until_failure = i; diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp index 4207847ad..2da45d97e 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -55,6 +55,7 @@ class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing ret = rcl_node_init(&this->node, "test_action_communication_node", "", &context, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT( test_msgs, Fibonacci); const char * action_name = "test_action_commmunication_name"; @@ -227,6 +228,7 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -346,6 +348,7 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -463,6 +466,7 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -535,6 +539,7 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); diff --git a/rcl_action/test/rcl_action/test_action_interaction.cpp b/rcl_action/test/rcl_action/test_action_interaction.cpp index d16367a82..c9f023b92 100644 --- a/rcl_action/test/rcl_action/test_action_interaction.cpp +++ b/rcl_action/test/rcl_action/test_action_interaction.cpp @@ -67,6 +67,7 @@ class CLASSNAME (TestActionClientServerInteraction, RMW_IMPLEMENTATION) : public ret = rcl_node_init(&this->node, "test_action_communication_node", "", &context, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT( test_msgs, Fibonacci); const char * action_name = "test_action_commmunication_name"; @@ -216,6 +217,7 @@ TEST_F(CLASSNAME(TestActionClientServerInteraction, RMW_IMPLEMENTATION), test_in EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -261,6 +263,7 @@ TEST_F(CLASSNAME(TestActionClientServerInteraction, RMW_IMPLEMENTATION), test_in EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -319,6 +322,7 @@ TEST_F(CLASSNAME(TestActionClientServerInteraction, RMW_IMPLEMENTATION), test_in EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -363,6 +367,7 @@ TEST_F(CLASSNAME(TestActionClientServerInteraction, RMW_IMPLEMENTATION), test_in this->outgoing_feedback.feedback.sequence.size)); ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -413,6 +418,7 @@ TEST_F(CLASSNAME(TestActionClientServerInteraction, RMW_IMPLEMENTATION), test_in EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -484,6 +490,7 @@ TEST_F( EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -529,6 +536,7 @@ TEST_F( EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -587,6 +595,7 @@ TEST_F( EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -631,6 +640,7 @@ TEST_F( this->outgoing_feedback.feedback.sequence.size)); ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -687,6 +697,7 @@ TEST_F( EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -737,6 +748,7 @@ TEST_F( EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); @@ -786,6 +798,7 @@ TEST_F( EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_clear(&this->wait_set); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_client( &this->wait_set, &this->action_client, NULL, NULL); diff --git a/rcl_action/test/rcl_action/test_graph.cpp b/rcl_action/test/rcl_action/test_graph.cpp index d155661c5..bb8be4465 100644 --- a/rcl_action/test/rcl_action/test_graph.cpp +++ b/rcl_action/test/rcl_action/test_graph.cpp @@ -296,6 +296,7 @@ class TestActionGraphMultiNodeFixture : public CLASSNAME(TestActionGraphFixture, this->remote_context = rcl_get_zero_initialized_context(); ret = rcl_init(0, nullptr, &init_options, &this->remote_context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_node_init( &this->remote_node, this->remote_node_name, "", &this->remote_context, &node_options); diff --git a/rcl_action/test/rcl_action/test_wait.cpp b/rcl_action/test/rcl_action/test_wait.cpp index 45aaa3959..9fd1c69e7 100644 --- a/rcl_action/test/rcl_action/test_wait.cpp +++ b/rcl_action/test/rcl_action/test_wait.cpp @@ -294,6 +294,7 @@ TEST_F(TestActionServerWait, test_wait_set_add_action_server) { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; ret = rcl_wait_set_init( &wait_set, 0, 0, 0, 0, 0, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str; @@ -305,6 +306,7 @@ TEST_F(TestActionServerWait, test_wait_set_add_action_server) { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; ret = rcl_wait_set_init( &wait_set, 0, 0, 0, 0, 1, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str; @@ -316,6 +318,7 @@ TEST_F(TestActionServerWait, test_wait_set_add_action_server) { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; ret = rcl_wait_set_init( &wait_set, 0, 0, 0, 0, 2, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str; @@ -327,6 +330,7 @@ TEST_F(TestActionServerWait, test_wait_set_add_action_server) { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; ret = rcl_wait_set_init( &wait_set, 0, 0, 0, 0, 3, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str; @@ -338,6 +342,7 @@ TEST_F(TestActionServerWait, test_wait_set_add_action_server) { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; ret = rcl_wait_set_init( &wait_set, 0, 0, 1, 0, 3, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -347,6 +352,7 @@ TEST_F(TestActionServerWait, test_wait_set_add_action_server) { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; ret = rcl_wait_set_init( &wait_set, 0, 0, 1, 0, 3, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, nullptr); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str; diff --git a/rcl_lifecycle/test/test_rcl_lifecycle.cpp b/rcl_lifecycle/test/test_rcl_lifecycle.cpp index ad1c5a346..493506a7e 100644 --- a/rcl_lifecycle/test/test_rcl_lifecycle.cpp +++ b/rcl_lifecycle/test/test_rcl_lifecycle.cpp @@ -69,6 +69,7 @@ TEST(TestRclLifecycle, lifecycle_state) { rcutils_reset_error(); ret = rcl_lifecycle_state_init(&state, expected_id, &expected_label[0], &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_EQ(expected_id, state.id); EXPECT_STREQ(&expected_label[0], state.label); diff --git a/rcl_lifecycle/test/test_transition_map.cpp b/rcl_lifecycle/test/test_transition_map.cpp index 990460367..50d99c3a5 100644 --- a/rcl_lifecycle/test/test_transition_map.cpp +++ b/rcl_lifecycle/test/test_transition_map.cpp @@ -73,6 +73,7 @@ TEST_F(TestTransitionMap, initialized) { rcl_lifecycle_state_t state1 = {"my_state_1", 1, NULL, 0}; ret = rcl_lifecycle_register_state(&transition_map, state1, &allocator); + ASSERT_EQ(RCL_RET_OK, ret); rcl_lifecycle_state_t unregistered = {"my_state_2", 2, NULL, 0}; @@ -96,7 +97,6 @@ TEST_F(TestTransitionMap, initialized) { rcl_lifecycle_transition_t transition01 = {"from0to1", 0, start_state, goal_state}; - original_size = transition_map.transitions_size; ret = rcl_lifecycle_register_transition( &transition_map, transition01, &allocator); EXPECT_EQ(RCL_RET_OK, ret); @@ -104,7 +104,6 @@ TEST_F(TestTransitionMap, initialized) { rcl_lifecycle_transition_t transition10 = {"from1to0", 1, goal_state, start_state}; - original_size = transition_map.transitions_size; ret = rcl_lifecycle_register_transition( &transition_map, transition10, &allocator); EXPECT_EQ(RCL_RET_OK, ret); From 8d47d7c60ee6605cdde72b5e3c516c5be6ebf6d7 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 27 Oct 2020 18:50:26 -0700 Subject: [PATCH 38/42] Return OK when finalizing zero-initialized contexts (#842) Resolves #814 This is consistent behavior with finalizing other types of zero-initialized objects. Signed-off-by: Jacob Perron --- rcl/include/rcl/context.h | 3 ++- rcl/src/rcl/context.c | 6 ++++-- rcl/test/rcl/test_context.cpp | 4 ++++ 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/rcl/include/rcl/context.h b/rcl/include/rcl/context.h index 36dd33d58..89995a9f9 100644 --- a/rcl/include/rcl/context.h +++ b/rcl/include/rcl/context.h @@ -154,8 +154,9 @@ rcl_get_zero_initialized_context(void); /** * The context to be finalized must have been previously initialized with * `rcl_init()`, and then later invalidated with `rcl_shutdown()`. + * A zero-initialized context that has not been initialized can be finalized. * If context is `NULL`, then `RCL_RET_INVALID_ARGUMENT` is returned. - * If context is zero-initialized, then `RCL_RET_INVALID_ARGUMENT` is returned. + * If context is zero-initialized, then `RCL_RET_OK` is returned. * If context is initialized and valid (`rcl_shutdown()` was not called on it), * then `RCL_RET_INVALID_ARGUMENT` is returned. * diff --git a/rcl/src/rcl/context.c b/rcl/src/rcl/context.c index 9c7b90ce2..3eb40b239 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -49,8 +49,10 @@ rcl_ret_t rcl_context_fini(rcl_context_t * context) { RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG( - context->impl, "context is zero-initialized", return RCL_RET_INVALID_ARGUMENT); + if (!context->impl) { + // Context is zero-initialized + return RCL_RET_OK; + } if (rcl_context_is_valid(context)) { RCL_SET_ERROR_MSG("rcl_shutdown() not called on the given context"); return RCL_RET_INVALID_ARGUMENT; diff --git a/rcl/test/rcl/test_context.cpp b/rcl/test/rcl/test_context.cpp index 05a104d5d..bfef946e0 100644 --- a/rcl/test/rcl/test_context.cpp +++ b/rcl/test/rcl/test_context.cpp @@ -142,6 +142,10 @@ TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), bad_fini) { }); rcl_context_t context = rcl_get_zero_initialized_context(); + + ret = rcl_context_fini(&context); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_init(0, nullptr, &init_options, &context); EXPECT_EQ(RCL_RET_OK, ret); From 6222bee441c21ca9670d93741fde070901afd9c4 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 27 Oct 2020 18:49:48 -0700 Subject: [PATCH 39/42] Zero initialize events an size_of_events members of rcl_wait_set_t (#841) Signed-off-by: Jacob Perron --- rcl/src/rcl/wait.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index a23c77f7b..6c973685b 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -74,6 +74,8 @@ rcl_get_zero_initialized_wait_set() .size_of_services = 0, .timers = NULL, .size_of_timers = 0, + .events = NULL, + .size_of_events = 0, .impl = NULL, }; return null_wait_set; From caf3a16b7c114929d52a44c1727c7859229ced7c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 29 Oct 2020 08:15:52 -0400 Subject: [PATCH 40/42] Make sure to always check return values. (#840) * Make sure to always check return values. Pointed out by clang static analysis, we should always check these return values. Signed-off-by: Chris Lalancette --- rcl_action/src/rcl_action/graph.c | 18 +++++++++++++++++- rcl_lifecycle/src/rcl_lifecycle.c | 4 ++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/rcl_action/src/rcl_action/graph.c b/rcl_action/src/rcl_action/graph.c index ce8474234..88c2e87d7 100644 --- a/rcl_action/src/rcl_action/graph.c +++ b/rcl_action/src/rcl_action/graph.c @@ -122,7 +122,10 @@ _filter_action_names( // Cleanup if there is an error if (RCL_RET_OK != ret) { rcl_ret_t fini_ret = rcl_names_and_types_fini(action_names_and_types); - (void)fini_ret; // Error already set + if (RCL_RET_OK != fini_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Freeing names and types failed while handling a previous error. Leaking memory!\n"); + } } return ret; @@ -154,6 +157,10 @@ rcl_action_get_client_names_and_types_by_node( rcl_ret_t nat_fini_ret = rcl_names_and_types_fini(&topic_names_and_types); if (RCL_RET_OK != nat_fini_ret) { ret = rcl_names_and_types_fini(action_names_and_types); + if (RCL_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Freeing names and types failed while handling a previous error. Leaking memory!\n"); + } return nat_fini_ret; } return ret; @@ -185,6 +192,11 @@ rcl_action_get_server_names_and_types_by_node( rcl_ret_t nat_fini_ret = rcl_names_and_types_fini(&topic_names_and_types); if (RCL_RET_OK != nat_fini_ret) { ret = rcl_names_and_types_fini(action_names_and_types); + if (RCL_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Freeing names and types failed while handling a previous error. Leaking memory!\n"); + } + return nat_fini_ret; } return ret; @@ -211,6 +223,10 @@ rcl_action_get_names_and_types( rcl_ret_t nat_fini_ret = rcl_names_and_types_fini(&topic_names_and_types); if (RCL_RET_OK != nat_fini_ret) { ret = rcl_names_and_types_fini(action_names_and_types); + if (RCL_RET_OK != ret) { + RCUTILS_SET_ERROR_MSG( + "Freeing names and types failed while handling a previous error. Leaking memory!\n"); + } return nat_fini_ret; } return ret; diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index ee3d7feff..56dc6b40e 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -234,6 +234,10 @@ rcl_lifecycle_state_machine_init( // init default state machine might have allocated memory, // so we have to call fini ret = rcl_lifecycle_state_machine_fini(state_machine, node_handle, allocator); + if (ret != RCL_RET_OK) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Freeing state machine failed while handling a previous error. Leaking memory!\n"); + } return RCL_RET_ERROR; } } From 488d54e06a2d3171b4e096c0768d6c2e724a8775 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 5 Oct 2020 13:39:39 -0400 Subject: [PATCH 41/42] Add a semicolon to RCUTILS_LOGGING_AUTOINIT. (#816) This makes it look like a C statement. Signed-off-by: Chris Lalancette --- rcl/src/rcl/logging.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/logging.c b/rcl/src/rcl/logging.c index 5ae5c7405..381a85239 100644 --- a/rcl/src/rcl/logging.c +++ b/rcl/src/rcl/logging.c @@ -62,8 +62,8 @@ rcl_logging_configure_with_output_handler( RCL_CHECK_ARGUMENT_FOR_NULL(global_args, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(output_handler, RCL_RET_INVALID_ARGUMENT); - RCUTILS_LOGGING_AUTOINIT - g_logging_allocator = *allocator; + RCUTILS_LOGGING_AUTOINIT; + g_logging_allocator = *allocator; int default_level = global_args->impl->log_level; const char * config_file = global_args->impl->external_log_config_file; g_rcl_logging_stdout_enabled = !global_args->impl->log_stdout_disabled; From 6ac4453bc4e4332b1aa3017dcd8e80e5b9c06007 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 12 Aug 2020 13:08:08 -0700 Subject: [PATCH 42/42] increase timeouts in test_services fixtures for Connext (#745) Signed-off-by: Dirk Thomas --- rcl/test/rcl/client_fixture.cpp | 4 ++-- rcl/test/rcl/service_fixture.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp index c20caafc4..22142d817 100644 --- a/rcl/test/rcl/client_fixture.cpp +++ b/rcl/test/rcl/client_fixture.cpp @@ -99,7 +99,7 @@ int main(int argc, char ** argv) }); // Wait until server is available - if (!wait_for_server_to_be_available(&node, &client, 10, 100)) { + if (!wait_for_server_to_be_available(&node, &client, 30, 100)) { RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Server never became available"); return -1; } @@ -134,7 +134,7 @@ int main(int argc, char ** argv) memset(&client_response, 0, sizeof(test_msgs__srv__BasicTypes_Response)); test_msgs__srv__BasicTypes_Response__init(&client_response); - if (!wait_for_client_to_be_ready(&client, &context, 10, 100)) { + if (!wait_for_client_to_be_ready(&client, &context, 30, 100)) { RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Client never became ready"); return -1; } diff --git a/rcl/test/rcl/service_fixture.cpp b/rcl/test/rcl/service_fixture.cpp index c3fdaea05..611cd1fca 100644 --- a/rcl/test/rcl/service_fixture.cpp +++ b/rcl/test/rcl/service_fixture.cpp @@ -116,7 +116,7 @@ int main(int argc, char ** argv) // Block until a client request comes in. - if (!wait_for_service_to_be_ready(&service, &context, 10, 100)) { + if (!wait_for_service_to_be_ready(&service, &context, 30, 100)) { RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Service never became ready"); return -1; }