diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 6aea8f6..676dc03 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -197,6 +197,15 @@ rcl_publisher_init( goto cleanup; fail: if (publisher->impl) { + if (publisher->impl->rmw_handle) { + rmw_ret_t rmw_fail_ret = rmw_destroy_publisher( + rcl_node_get_rmw_handle(node), publisher->impl->rmw_handle); + if (RMW_RET_OK != rmw_fail_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + } + allocator->deallocate(publisher->impl, allocator->state); publisher->impl = NULL; } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 2e3b37d..6a4af49 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -193,6 +193,15 @@ rcl_subscription_init( goto cleanup; fail: if (subscription->impl) { + if (subscription->impl->rmw_handle) { + rmw_ret_t rmw_fail_ret = rmw_destroy_subscription( + rcl_node_get_rmw_handle(node), subscription->impl->rmw_handle); + if (RMW_RET_OK != rmw_fail_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + } + allocator->deallocate(subscription->impl, allocator->state); } ret = fail_ret;