diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 184a9d765..e9e0b4c50 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -86,6 +86,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 eb2cc90bd..e9fd33fcf 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. @@ -216,6 +224,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)) { @@ -286,6 +299,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 cf7de3c4e..5c95f36c9 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. @@ -210,6 +218,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 d4e307529..d6c4150bf 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -211,6 +211,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); @@ -457,6 +462,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(); + } + }); +}