Add fault injection macros to rcl functions (#727)

* Add fault injection macros to rcl functions

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* PR fixup

Signed-off-by: Stephen Brawner <brawner@gmail.com>

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

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 @@ cleanup:
 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);

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
   }

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 @@ cleanup:
 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
   }

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 @@ cleanup:
 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)) {

rcl/src/rcl/subscription.c

@@ -220,6 +220,11 @@ cleanup:
 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;
   }

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) {

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;

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();
+    }
+  });
+}

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();
+    }
+  });
+}