From d6ea437dce2e7a4bbc3775542cabc681b5bda9e4 Mon Sep 17 00:00:00 2001
From: Jackie Kay <jacquelinekay1@gmail.com>
Date: Fri, 18 Dec 2015 11:50:20 -0800
Subject: [PATCH] Incorporate rmw_waitset

---
 rcl/src/rcl/wait.c | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c
index 12ff30b..f01a92b 100644
--- a/rcl/src/rcl/wait.c
+++ b/rcl/src/rcl/wait.c
@@ -115,7 +115,10 @@ rcl_wait_set_init(
   wait_set->impl->rmw_guard_conditions.guard_conditions = NULL;
   wait_set->impl->rmw_guard_conditions.guard_condition_count = 0;
   wait_set->impl->rmw_waitset = rmw_create_waitset(
-    NULL, 2*number_of_subscriptions + number_of_guard_conditions);
+    NULL, 2 * number_of_subscriptions + number_of_guard_conditions);
+  if (!wait_set->impl->rmw_waitset) {
+    goto fail;
+  }
 
   // Initialize subscription space.
   rcl_ret_t ret;
@@ -151,6 +154,12 @@ rcl_wait_set_init(
   wait_set->impl->allocator = allocator;
   return RCL_RET_OK;
 fail:
+  if (__wait_set_is_valid(wait_set)) {
+    rmw_ret_t ret = rmw_destroy_waitset(wait_set->impl->rmw_waitset);
+    if (ret != RMW_RET_OK) {
+      fail_ret = RCL_RET_WAIT_SET_INVALID;
+    }
+  }
   __wait_set_clean_up(wait_set, allocator);
   return fail_ret;
 }
@@ -160,7 +169,13 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set)
 {
   rcl_ret_t result = RCL_RET_OK;
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
+
   if (__wait_set_is_valid(wait_set)) {
+    rmw_ret_t ret = rmw_destroy_waitset(wait_set->impl->rmw_waitset);
+    if (ret != RMW_RET_OK) {
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      result = RCL_RET_WAIT_SET_INVALID;
+    }
     __wait_set_clean_up(wait_set, wait_set->impl->allocator);
   }
   return result;