Clean up error handling in many rcl{_action,_lifecycle} codepaths

rcl/src/rcl/arguments.c

@@ -1213,10 +1213,10 @@ _rcl_parse_resource_match_token(rcl_lexer_lookahead2_t * lex_lookahead)
     ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL);
   } else if (RCL_LEXEME_WILD_ONE == lexeme) {
     RCL_SET_ERROR_MSG("Wildcard '*' is not implemented");
-    return RCL_RET_ERROR;
+    ret = RCL_RET_ERROR;
   } else if (RCL_LEXEME_WILD_MULTI == lexeme) {
     RCL_SET_ERROR_MSG("Wildcard '**' is not implemented");
-    return RCL_RET_ERROR;
+    ret = RCL_RET_ERROR;
   } else {
     RCL_SET_ERROR_MSG("Expecting token or wildcard");
     ret = RCL_RET_WRONG_LEXEME;
@@ -1276,6 +1276,9 @@ _rcl_parse_resource_match(
     ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_FORWARD_SLASH, NULL, NULL);
     if (RCL_RET_WRONG_LEXEME == ret) {
       return RCL_RET_INVALID_REMAP_RULE;
+    } else if (RCL_RET_OK != ret) {
+      // Another failure
+      return ret;
     }
     ret = _rcl_parse_resource_match_token(lex_lookahead);
     if (RCL_RET_OK != ret) {

rcl/src/rcl/expand_topic_name.c

@@ -104,6 +104,7 @@ rcl_expand_topic_name(
     *output_topic_name = rcutils_strdup(input_topic_name, allocator);
     if (!*output_topic_name) {
       *output_topic_name = NULL;
+      rcl_reset_error();
       RCL_SET_ERROR_MSG("failed to allocate memory for output topic");
       return RCL_RET_BAD_ALLOC;
     }
@@ -176,6 +177,7 @@ rcl_expand_topic_name(
         rcutils_strndup(next_opening_brace, substitution_substr_len, allocator);
       if (!next_substitution) {
         *output_topic_name = NULL;
+        rcl_reset_error();
         RCL_SET_ERROR_MSG("failed to allocate memory for substitution");
         allocator.deallocate(local_output, allocator.state);
         return RCL_RET_BAD_ALLOC;
@@ -186,6 +188,7 @@ rcl_expand_topic_name(
       allocator.deallocate(original_local_output, allocator.state);  // free no matter what
       if (!local_output) {
         *output_topic_name = NULL;
+        rcl_reset_error();
         RCL_SET_ERROR_MSG("failed to allocate memory for expanded topic");
         return RCL_RET_BAD_ALLOC;
       }

rcl/src/rcl/init.c

@@ -235,7 +235,7 @@ rcl_init(
     &validation_result,
     &invalid_index);
   if (RCL_RET_OK != ret) {
-    RCL_SET_ERROR_MSG("rcl_validate_enclave_name() failed");
+    // rcl_validate_enclave_name already set the error
     fail_ret = ret;
     goto fail;
   }

rcl/src/rcl/logging_rosout.c

@@ -213,7 +213,7 @@ rcl_ret_t rcl_logging_rosout_init_publisher_for_node(rcl_node_t * node)
   RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_NODE_INVALID);
   logger_name = rcl_node_get_logger_name(node);
   if (NULL == logger_name) {
-    RCL_SET_ERROR_MSG("Logger name was null.");
+    // rcl_node_get_logger_name already set the error
     return RCL_RET_ERROR;
   }
   if (rcutils_hash_map_key_exists(&__logger_map, &logger_name)) {

rcl/src/rcl/node_type_cache.c

@@ -59,6 +59,7 @@ rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node)
     &node->context->impl->allocator);
 
   if (RCUTILS_RET_OK != ret) {
+    rcl_reset_error();
     RCL_SET_ERROR_MSG("Failed to initialize type cache hash map");
     return RCL_RET_ERROR;
   }
@@ -187,6 +188,8 @@ rcl_ret_t rcl_node_type_cache_register_type(
       &node->impl->registered_types_by_type_hash,
       type_hash, &type_info_with_registrations))
   {
+    // Reset the error since rcutils_hash_map_set already set it
+    rcutils_reset_error();
     RCL_SET_ERROR_MSG("Failed to update type info");
     type_description_interfaces__msg__TypeDescription__destroy(
       type_info_with_registrations.type_info.type_description);

rcl/src/rcl/publisher.c

@@ -421,7 +421,11 @@ rcl_publisher_is_valid(const rcl_publisher_t * publisher)
     return false;  // error already set
   }
   if (!rcl_context_is_valid(publisher->impl->context)) {
-    RCL_SET_ERROR_MSG("publisher's context is invalid");
+    if (!rcl_error_is_set()) {
+      // rcl_context_is_valid can return false both in the error case, and when the context
+      // hasn't been initialized.  It will only set the error message in the first case.
+      RCL_SET_ERROR_MSG("publisher's context is invalid");
+    }
     return false;
   }
   RCL_CHECK_FOR_NULL_WITH_MSG(

rcl/src/rcl/subscription.c

@@ -100,8 +100,11 @@ rcl_subscription_init(
     1, sizeof(rcl_subscription_impl_t), allocator->state);
   RCL_CHECK_FOR_NULL_WITH_MSG(
     subscription->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup);
+
+  // options
+  subscription->impl->options = *options;
+
   // Fill out the implemenation struct.
-  // rmw_handle
   // TODO(wjwwood): pass allocator once supported in rmw api.
   subscription->impl->rmw_handle = rmw_create_subscription(
     rcl_node_get_rmw_handle(node),
@@ -123,8 +126,6 @@ rcl_subscription_init(
   }
   subscription->impl->actual_qos.avoid_ros_namespace_conventions =
     options->qos.avoid_ros_namespace_conventions;
-  // options
-  subscription->impl->options = *options;
 
   if (RCL_RET_OK != rcl_node_type_cache_register_type(
       node, type_support->get_type_hash_func(type_support),

rcl/src/rcl/timer.c

@@ -198,9 +198,11 @@ rcl_timer_init2(
       // Should be impossible
       RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini guard condition after bad alloc");
     }
-    if (RCL_RET_OK != rcl_clock_remove_jump_callback(clock, _rcl_timer_time_jump, timer)) {
-      // Should be impossible
-      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to remove callback after bad alloc");
+    if (RCL_ROS_TIME == impl.clock->type) {
+      if (RCL_RET_OK != rcl_clock_remove_jump_callback(clock, _rcl_timer_time_jump, timer)) {
+        // Should be impossible
+        RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to remove callback after bad alloc");
+      }
     }
 
     RCL_SET_ERROR_MSG("allocating memory failed");

rcl/src/rcl/validate_enclave_name.c

@@ -56,6 +56,7 @@ rcl_validate_enclave_name_with_size(
   rmw_ret_t ret = rmw_validate_namespace_with_size(
     enclave, enclave_length, &tmp_validation_result, &tmp_invalid_index);
   if (ret != RMW_RET_OK) {
+    // rmw_validate_namespace_with_size already set the error
     return rcl_convert_rmw_ret_to_rcl_ret(ret);
   }
 

rcl/src/rcl/wait.c

@@ -74,18 +74,6 @@ rcl_wait_set_is_valid(const rcl_wait_set_t * wait_set)
   return wait_set && wait_set->impl;
 }
 
-static void
-__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) {
-    wait_set->impl->allocator.deallocate(wait_set->impl, wait_set->impl->allocator.state);
-    wait_set->impl = NULL;
-  }
-}
-
 rcl_ret_t
 rcl_wait_set_init(
   rcl_wait_set_t * wait_set,
@@ -103,7 +91,7 @@ rcl_wait_set_init(
     "'%zu' subscriptions, '%zu' guard conditions, '%zu' timers, '%zu' clients, '%zu' services",
     number_of_subscriptions, number_of_guard_conditions, number_of_timers, number_of_clients,
     number_of_services);
-  rcl_ret_t fail_ret = RCL_RET_ERROR;
+  rcl_ret_t rcl_ret = RCL_RET_ERROR;
 
   RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
@@ -149,27 +137,30 @@ rcl_wait_set_init(
 
   wait_set->impl->rmw_wait_set = rmw_create_wait_set(&(context->impl->rmw_context), num_conditions);
   if (!wait_set->impl->rmw_wait_set) {
+    rcl_ret = RCL_RET_BAD_ALLOC;
     goto fail;
   }
 
   // Initialize subscription space.
-  rcl_ret_t ret = rcl_wait_set_resize(
+  rcl_ret = rcl_wait_set_resize(
     wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers,
     number_of_clients, number_of_services, number_of_events);
-  if (RCL_RET_OK != ret) {
-    fail_ret = ret;
+  if (RCL_RET_OK != rcl_ret) {
     goto fail;
   }
   return RCL_RET_OK;
+
 fail:
-  if (rcl_wait_set_is_valid(wait_set)) {
-    rmw_ret_t ret = rmw_destroy_wait_set(wait_set->impl->rmw_wait_set);
-    if (ret != RMW_RET_OK) {
-      fail_ret = RCL_RET_WAIT_SET_INVALID;
+  if (wait_set->impl->rmw_wait_set != NULL) {
+    rmw_ret_t rmw_ret = rmw_destroy_wait_set(wait_set->impl->rmw_wait_set);
+    if (rmw_ret != RMW_RET_OK) {
+      rcl_ret = RCL_RET_WAIT_SET_INVALID;
     }
   }
-  __wait_set_clean_up(wait_set);
-  return fail_ret;
+  allocator.deallocate(wait_set->impl, wait_set->impl->allocator.state);
+  wait_set->impl = NULL;
+
+  return rcl_ret;
 }
 
 rcl_ret_t
@@ -184,7 +175,16 @@ 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);
+
+    rcl_ret_t resize_result = rcl_wait_set_resize(wait_set, 0, 0, 0, 0, 0, 0);
+    if (result == RCL_RET_OK) {
+      // Only return the error here if we had no earlier errors.
+      result = resize_result;
+    }
+    if (wait_set->impl) {
+      wait_set->impl->allocator.deallocate(wait_set->impl, wait_set->impl->allocator.state);
+      wait_set->impl = NULL;
+    }
   }
   return result;
 }

rcl/test/rcl/test_network_flow_endpoints.cpp

@@ -247,6 +247,7 @@ TEST_F(TestPublisherNetworkFlowEndpoints, test_publisher_get_network_flow_endpoi
   ret_1 = rcl_publisher_get_network_flow_endpoints(
     &this->publisher_1, &allocator, &network_flow_endpoint_array_1);
   EXPECT_TRUE(ret_1 == RCL_RET_OK || ret_1 == RCL_RET_UNSUPPORTED);
+  rcl_reset_error();
 
   // Get network flow endpoints of publisher with unique network flow endpoints
   rcl_network_flow_endpoint_array_t network_flow_endpoint_array_2 =
@@ -259,6 +260,7 @@ TEST_F(TestPublisherNetworkFlowEndpoints, test_publisher_get_network_flow_endpoi
     ret_2 = rcl_publisher_get_network_flow_endpoints(
       &this->publisher_2, &allocator, &network_flow_endpoint_array_2);
     EXPECT_TRUE(ret_2 == RCL_RET_OK || ret_2 == RCL_RET_UNSUPPORTED);
+    rcl_reset_error();
   } else {
     ret_2 = RCL_RET_ERROR;
   }
@@ -348,6 +350,7 @@ TEST_F(TestSubscriptionNetworkFlowEndpoints, test_subscription_get_network_flow_
   ret_1 = rcl_subscription_get_network_flow_endpoints(
     &this->subscription_1, &allocator, &network_flow_endpoint_array_1);
   EXPECT_TRUE(ret_1 == RCL_RET_OK || ret_1 == RCL_RET_UNSUPPORTED);
+  rcl_reset_error();
 
   // Get network flow endpoints of subscription with unique network flow endpoints
   rcl_network_flow_endpoint_array_t network_flow_endpoint_array_2 =
@@ -358,6 +361,7 @@ TEST_F(TestSubscriptionNetworkFlowEndpoints, test_subscription_get_network_flow_
     ret_2 = rcl_subscription_get_network_flow_endpoints(
       &this->subscription_2, &allocator, &network_flow_endpoint_array_2);
     EXPECT_TRUE(ret_2 == RCL_RET_OK || ret_2 == RCL_RET_UNSUPPORTED);
+    rcl_reset_error();
   } else {
     ret_2 = RCL_RET_ERROR;
   }

rcl/test/rcl/test_wait.cpp

@@ -850,8 +850,7 @@ TEST_F(WaitSetTestFixture, wait_set_failed_init) {
     "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());
+  EXPECT_EQ(RCL_RET_BAD_ALLOC, ret);
   rcl_reset_error();
 }
 

rcl_action/src/rcl_action/action_client.c

@@ -114,11 +114,6 @@ _rcl_action_client_fini_impl(
   if (RCL_RET_OK != ret) { \
     rcl_reset_error(); \
     RCL_SET_ERROR_MSG("failed to get " #Type " service name"); \
-    if (RCL_RET_BAD_ALLOC == ret) { \
-      ret = RCL_RET_BAD_ALLOC; \
-    } else { \
-      ret = RCL_RET_ERROR; \
-    } \
     goto fail; \
   } \
   rcl_client_options_t Type ## _service_client_options = { \
@@ -133,12 +128,8 @@ _rcl_action_client_fini_impl(
     &Type ## _service_client_options); \
   allocator.deallocate(Type ## _service_name, allocator.state); \
   if (RCL_RET_OK != ret) { \
-    if (RCL_RET_BAD_ALLOC == ret) { \
-      ret = RCL_RET_BAD_ALLOC; \
-    } else if (RCL_RET_SERVICE_NAME_INVALID == ret) { \
+    if (RCL_RET_SERVICE_NAME_INVALID == ret) { \
       ret = RCL_RET_ACTION_NAME_INVALID; \
-    } else { \
-      ret = RCL_RET_ERROR; \
     } \
     goto fail; \
   }
@@ -150,11 +141,6 @@ _rcl_action_client_fini_impl(
   if (RCL_RET_OK != ret) { \
     rcl_reset_error(); \
     RCL_SET_ERROR_MSG("failed to get " #Type " topic name"); \
-    if (RCL_RET_BAD_ALLOC == ret) { \
-      ret = RCL_RET_BAD_ALLOC; \
-    } else { \
-      ret = RCL_RET_ERROR; \
-    } \
     goto fail; \
   } \
   rcl_subscription_options_t Type ## _topic_subscription_options = \
@@ -170,12 +156,8 @@ _rcl_action_client_fini_impl(
     &Type ## _topic_subscription_options); \
   allocator.deallocate(Type ## _topic_name, allocator.state); \
   if (RCL_RET_OK != ret) { \
-    if (RCL_RET_BAD_ALLOC == ret) { \
-      ret = RCL_RET_BAD_ALLOC; \
-    } else if (RCL_RET_TOPIC_NAME_INVALID == ret) { \
+    if (RCL_RET_TOPIC_NAME_INVALID == ret) { \
       ret = RCL_RET_ACTION_NAME_INVALID; \
-    } else { \
-      ret = RCL_RET_ERROR; \
     } \
     goto fail; \
   }
@@ -231,11 +213,11 @@ rcl_action_client_init(
   SUBSCRIPTION_INIT(feedback);
   SUBSCRIPTION_INIT(status);
 
-  if (RCL_RET_OK != rcl_node_type_cache_register_type(
+  ret = rcl_node_type_cache_register_type(
       node, type_support->get_type_hash_func(type_support),
       type_support->get_type_description_func(type_support),
-      type_support->get_type_description_sources_func(type_support)))
-  {
+      type_support->get_type_description_sources_func(type_support));
+  if (RCL_RET_OK != ret) {
     rcutils_reset_error();
     RCL_SET_ERROR_MSG("Failed to register type for action");
     goto fail;