Fix memory leak in rcl_subscription_init()/rcl_publisher_init() (#794)

* Fix memory leak in rcl_subscription_init()/rcl_publisher_init()

In rcl_subscription_init(), while rmw_subscription_get_actual_qos()
return failure, created rmw subscription handle isn't freed.
In rcl_publisher_init(), while rmw_publisher_get_actual_qos()
return failure, created rmw publisher handle isn't freed.

Signed-off-by: Barry Xu <barry.xu@sony.com>

* Remove codes on the cascading errors.

Signed-off-by: Barry Xu <barry.xu@sony.com>

* Change code style

Signed-off-by: Barry Xu <barry.xu@sony.com>

* Output error message to stderr

Signed-off-by: Barry Xu <barry.xu@sony.com>

rcl/src/rcl/publisher.c

@@ -205,6 +205,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_WITH_FORMAT_STRING(
+          "%s, at %s:%d\n", rmw_get_error_string().str, __FILE__, __LINE__);
+      }
+    }
+
     allocator->deallocate(publisher->impl, allocator->state);
     publisher->impl = NULL;
   }

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_WITH_FORMAT_STRING(
+          "%s, at %s:%d\n", rmw_get_error_string().str, __FILE__, __LINE__);
+      }
+    }
+
     allocator->deallocate(subscription->impl, allocator->state);
     subscription->impl = NULL;
   }