some small API changes and initial wait impl

rcl/CMakeLists.txt

@@ -25,6 +25,7 @@ set(${PROJECT_NAME}_sources
   src/rcl/rcl.c
   src/rcl/subscription.c
   src/rcl/wait.c
+  src/rcl/time.c
 )
 add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources})
 ament_target_dependencies(${PROJECT_NAME}

rcl/include/rcl/guard_condition.h

@@ -141,7 +141,7 @@ rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition);
  * \return rmw guard_condition handle if successful, otherwise NULL
  */
 rmw_guard_condition_t *
-rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition);
+rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition);
 
 #if __cplusplus
 }

rcl/include/rcl/node.h

@@ -29,12 +29,6 @@ extern "C"
 struct rcl_node_impl_t;
 
 /// Handle for a ROS node.
-/* This handle can be in one a few stats, which are referred to below:
- * - uninitialized: memory allocated, but not initialized to anything
- * - invalid: memory allocated but not usable
- *   - either zero initialized or shutdown (rcl_fini() or rcl_node_fini())
- * - valid: memory allocated and usable
- */
 typedef struct rcl_node_t {
   /// Private implementation pointer.
   struct rcl_node_impl_t * impl;

rcl/include/rcl/subscription.h

@@ -239,7 +239,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription);
  * \return options struct if successful, otherwise NULL
  */
 const rcl_subscription_options_t *
-rcl_subscription_get_options(rcl_subscription_t * subscription);
+rcl_subscription_get_options(const rcl_subscription_t * subscription);
 
 /// Return the rmw subscription handle.
 /* The handle returned is a pointer to the internally held rmw handle.
@@ -257,7 +257,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription);
  * \return rmw subscription handle if successful, otherwise NULL
  */
 rmw_subscription_t *
-rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription);
+rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription);
 
 #if __cplusplus
 }

rcl/include/rcl/time.h

@@ -22,8 +22,22 @@ extern "C"
 
 #include "rmw/rmw.h"
 
+#define RCL_S_TO_NS(seconds) seconds * 1000000000
+#define RCL_MS_TO_NS(milliseconds) milliseconds * 1000000
+#define RCL_US_TO_NS(microseconds) microseconds * 1000
+
+#define RCL_NS_TO_S(nanoseconds) nanoseconds / 1000000000
+#define RCL_NS_TO_MS(nanoseconds) nanoseconds / 1000000
+#define RCL_NS_TO_US(nanoseconds) nanoseconds / 1000
+
 typedef rmw_time_t rcl_time_t;
 
+rcl_time_t
+rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds);
+
+rcl_time_t
+rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds);
+
 #if __cplusplus
 }
 #endif

rcl/include/rcl/wait.h

@@ -25,7 +25,6 @@ extern "C"
 
 #include "rcl/subscription.h"
 #include "rcl/guard_condition.h"
-#include "rcl/time.h"
 #include "rcl/types.h"
 
 struct rcl_wait_set_impl_t;
@@ -35,11 +34,9 @@ typedef struct rcl_wait_set_t {
   /// Storage for subscription pointers.
   const rcl_subscription_t ** subscriptions;
   size_t size_of_subscriptions;
-  size_t __current_subscription_offset;
   /// Storage for guard condition pointers.
   const rcl_guard_condition_t ** guard_conditions;
   size_t size_of_guard_conditions;
-  size_t __current_guard_condition_offset;
   /// Allocator for storage.
   rcl_allocator_t allocator;
   /// If set to true, actions like add_subscription will fail until cleared.
@@ -240,8 +237,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
  *     // ... error handling
  *     ret = rcl_wait_set_add_guard_condition(&wait_set, &gc1);
  *     // ... error handling
- *     rcl_time_t timeout = {1, 0};  // 1 second and 0 nanoseconds.
+ *     ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1000));  // 1000ms == 1s, passed as ns
- *     ret = rcl_wait(&wait_set, &timeout);
  *     if (ret == RCL_RET_TIMEOUT) {
  *       continue;
  *     }
@@ -271,14 +267,14 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
  * Passing an uninitialized (zero initialized) wait set struct will fail.
  * Passing a wait set struct with uninitialized memory is undefined behavior.
  *
- * If the timeout pointer is NULL then this function will block indefinitely
+ * The unit of timeout is nanoseconds.
- * until something in the wait set is valid or it is interrupted.
+ * If the timeout is negative then this function will block indefinitely until
- * If the timeout contains 0 for seconds and nanoseconds this function will be
+ * something in the wait set is valid or it is interrupted.
- * non-blocking; checking what's ready but not waiting if nothing is ready yet.
+ * If the timeout is 0 then this function will be non-blocking; checking what's
- * If the timeout contains a non-zero time then this function will return after
+ * ready now, but not waiting if nothing is ready yet.
- * that period of time has elapsed if something in the wait set does not become
+ * If the timeout is greater than 0 then this function will return after
- * ready before then.
+ * that period of time has elapsed or the wait set becomes ready, which ever
- * The timeout's value is not changed.
+ * comes first.
  * Passing a timeout struct with uninitialized memory is undefined behavior.
  *
  * \TODO(wjwwood) this function should probably be thread-safe with itself but
@@ -288,16 +284,16 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
  * the given wait sets are not the same and non-overlapping in contents.
  *
  * \param[inout] wait_set the set of things to be waited on and to be pruned if not ready
- * \param[in] timeout the time duration to wait something in the wait set to be ready
+ * \param[in] timeout the duration to wait for the wait set to be ready, in nanoseconds
  * \return RCL_RET_OK something in the wait set became ready, or
- *         RCL_RET_TIMEOUT timeout expired before something was ready, or
- *         RCL_RET_NOT_INIT wait set is zero initialized, or
- *         RCL_RET_WAIT_SET_EMPTY wait set contains no items, or
  *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ *         RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or
+ *         RCL_RET_WAIT_SET_EMPTY if the wait set contains no items, or
+ *         RCL_RET_TIMEOUT if the timeout expired before something was ready, or
  *         RCL_RET_ERROR an unspecified error occur.
  */
 rcl_ret_t
-rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout);
+rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout);
 
 #if __cplusplus
 }

rcl/src/rcl/guard_condition.c

@@ -137,7 +137,7 @@ rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition)
 }
 
 rmw_guard_condition_t *
-rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition)
+rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL);
   RCL_CHECK_FOR_NULL_WITH_MSG(

rcl/src/rcl/subscription.c

@@ -157,7 +157,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription)
 }
 
 const rcl_subscription_options_t *
-rcl_subscription_get_options(rcl_subscription_t * subscription)
+rcl_subscription_get_options(const rcl_subscription_t * subscription)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
   RCL_CHECK_FOR_NULL_WITH_MSG(
@@ -166,7 +166,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription)
 }
 
 rmw_subscription_t *
-rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription)
+rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
   RCL_CHECK_FOR_NULL_WITH_MSG(

rcl/src/rcl/time.c

@@ -0,0 +1,42 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#if __cplusplus
+extern "C"
+{
+#endif
+
+#include "rcl/time.h"
+
+rcl_time_t
+rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds)
+{
+  rcl_time_t result;
+  result.sec = RCL_NS_TO_S(nanoseconds);
+  result.nsec = nanoseconds % 1000000000;
+  return result;
+}
+
+rcl_time_t
+rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds)
+{
+  rcl_time_t result;
+  result.sec = RCL_NS_TO_S(nanoseconds);
+  result.nsec = nanoseconds % 1000000000;
+  return result;
+}
+
+#if __cplusplus
+}
+#endif

rcl/src/rcl/wait.c

@@ -19,15 +19,21 @@ extern "C"
 
 #include "rcl/wait.h"
 
+#include <assert.h>
+#include <stdatomic.h>
 #include <stdbool.h>
 #include <string.h>
 
 #include "rcl/error_handling.h"
+#include "rcl/time.h"
 #include "rmw/rmw.h"
 #include "./common.h"
 
 typedef struct rcl_wait_set_impl_t {
-  void * place_holder;  // To prevent size differences between C and C++.
+  size_t subscription_index;
+  rmw_subscriptions_t rmw_subscriptions;
+  size_t guard_condition_index;
+  rmw_guard_conditions_t rmw_guard_conditions;
 } rcl_wait_set_impl_t;
 
 rcl_wait_set_t
@@ -43,6 +49,21 @@ __wait_set_is_valid(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_allocator_t allocator)
+{
+  if (wait_set->subscriptions) {
+    rcl_wait_set_resize_subscriptions(wait_set, 0);
+  }
+  if (wait_set->guard_conditions) {
+    rcl_wait_set_resize_guard_conditions(wait_set, 0);
+  }
+  if (wait_set->impl) {
+    allocator.deallocate(wait_set->impl, allocator.state);
+    wait_set->impl = NULL;
+  }
+}
+
 rcl_ret_t
 rcl_wait_set_init(
   rcl_wait_set_t * wait_set,
@@ -68,26 +89,42 @@ rcl_wait_set_init(
     allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_FOR_NULL_WITH_MSG(
     allocator.reallocate, "reallocate not set", return RCL_RET_INVALID_ARGUMENT);
-  // Right now, the implementation struct is unused, but checked for NULL, so set it.
+  // Allocate space for the implementation struct.
-  wait_set->impl = (rcl_wait_set_impl_t *)0xDEADBEEF;
+  wait_set->impl = (rcl_wait_set_impl_t *)allocator.allocate(
+    sizeof(rcl_wait_set_impl_t), allocator.state);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+  wait_set->impl->rmw_subscriptions.subscribers = NULL;
+  wait_set->impl->rmw_subscriptions.subscriber_count = 0;
+  wait_set->impl->rmw_guard_conditions.guard_conditions = NULL;
+  wait_set->impl->rmw_guard_conditions.guard_condition_count = 0;
   // Initialize subscription space.
-  rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions);
+  rcl_ret_t ret;
-  rcl_wait_set_clear_subscriptions(wait_set);
+  if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) {
+    fail_ret = ret;
+    goto fail;
+  }
+  if ((ret = rcl_wait_set_clear_subscriptions(wait_set)) != RCL_RET_OK) {
+    fail_ret = ret;
+    goto fail;
+  }
   // Initialize guard condition space.
-  rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions);
+  ret = rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions);
-  rcl_wait_set_clear_guard_conditions(wait_set);
+  if (ret != RCL_RET_OK) {
+    fail_ret = ret;
+    goto fail;
+  }
+  if ((ret = rcl_wait_set_clear_guard_conditions(wait_set)) != RCL_RET_OK) {
+    fail_ret = ret;
+    goto fail;
+  }
   // Set allocator.
   wait_set->allocator = allocator;
   // Initialize pruned.
   wait_set->pruned = false;
   return RCL_RET_OK;
 fail:
-  if (wait_set->subscriptions) {
+  __wait_set_clean_up(wait_set, allocator);
-    allocator.deallocate(wait_set->subscriptions, allocator.state);
-  }
-  if (wait_set->guard_conditions) {
-    allocator.deallocate(wait_set->guard_conditions, allocator.state);
-  }
   return fail_ret;
 }
 
@@ -97,61 +134,83 @@ 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)) {
-    if (wait_set->subscriptions) {
+    __wait_set_clean_up(wait_set, wait_set->allocator);
-      wait_set->allocator.deallocate(wait_set->subscriptions, wait_set->allocator.state);
-    }
-    if (wait_set->guard_conditions) {
-      wait_set->allocator.deallocate(wait_set->guard_conditions, wait_set->allocator.state);
-    }
-    if (wait_set->impl) {
-      wait_set->impl = NULL;
-    }
   }
   return result;
 }
 
-#define SET_ADD(Type) \
+#define SET_ADD(Type, RMWStorage) \
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
   RCL_CHECK_ARGUMENT_FOR_NULL(Type, RCL_RET_INVALID_ARGUMENT); \
   if (!__wait_set_is_valid(wait_set)) { \
     RCL_SET_ERROR_MSG("wait set is invalid"); \
     return RCL_RET_WAIT_SET_INVALID; \
   } \
-  if (!(wait_set->__current_##Type##_offset < wait_set->size_of_##Type##s)) { \
+  if (!(wait_set->impl->Type##_index < wait_set->size_of_##Type##s)) { \
     RCL_SET_ERROR_MSG(#Type "s set is full"); \
     return RCL_RET_WAIT_SET_FULL; \
   } \
-  wait_set->Type##s[wait_set->__current_##Type##_offset++] = Type; \
+  size_t current_index = wait_set->impl->Type##_index++; \
+  wait_set->Type##s[current_index] = Type; \
+  /* Also place into rmw storage. */ \
+  rmw_##Type##_t * rmw_handle = rcl_##Type##_get_rmw_##Type##_handle(Type); \
+  RCL_CHECK_FOR_NULL_WITH_MSG( \
+    rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR); \
+  wait_set->impl->RMWStorage[current_index] = rmw_handle->data; \
   return RCL_RET_OK;
 
-#define SET_CLEAR(Type) \
+#define SET_CLEAR(Type, RMWStorage, RMWCount) \
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
   if (!__wait_set_is_valid(wait_set)) { \
     RCL_SET_ERROR_MSG("wait set is invalid"); \
     return RCL_RET_WAIT_SET_INVALID; \
   } \
   memset(wait_set->Type##s, 0, sizeof(rcl_##Type##_t *) * wait_set->size_of_##Type##s);  \
-  wait_set->__current_##Type##_offset = 0; \
+  wait_set->impl->Type##_index = 0; \
+  /* Also clear the rmw storage. */ \
+  memset( \
+    wait_set->impl->RMWStorage, \
+    0, \
+    sizeof(rmw_subscription_t *) * wait_set->impl->RMWCount); \
   return RCL_RET_OK;
 
-#define SET_RESIZE(Type) \
+#define SET_RESIZE(Type, RMWStorage, RMWCount) \
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
+  RCL_CHECK_FOR_NULL_WITH_MSG( \
+    wait_set->impl, "wait set is invalid", return RCL_RET_WAIT_SET_INVALID); \
   if (size == wait_set->size_of_##Type##s) { \
     return RCL_RET_OK; \
   } \
   if (size == 0) { \
     if (wait_set->Type##s) { \
       wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \
+      wait_set->Type##s = NULL; \
+    } \
+    /* Also deallocate the rmw storage. */ \
+    if (wait_set->impl->RMWStorage) { \
+      wait_set->allocator.deallocate(wait_set->impl->RMWStorage, wait_set->allocator.state); \
+      wait_set->impl->RMWStorage = NULL; \
     } \
-    wait_set->Type##s = NULL; \
-    return RCL_RET_OK; \
   } \
-  wait_set->size_of_##Type##s = 0; \
+  else { \
-  wait_set->Type##s = (const rcl_##Type##_t **)wait_set->allocator.reallocate( \
+    wait_set->size_of_##Type##s = 0; \
-    wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \
+    wait_set->Type##s = (const rcl_##Type##_t **)wait_set->allocator.reallocate( \
-  RCL_CHECK_FOR_NULL_WITH_MSG( \
+      wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \
-    wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \
+    RCL_CHECK_FOR_NULL_WITH_MSG( \
-  wait_set->size_of_##Type##s = size; \
+      wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \
+    wait_set->size_of_##Type##s = size; \
+    /* Also resize the rmw storage. */ \
+    wait_set->impl->RMWCount = 0; \
+    wait_set->impl->RMWStorage = (void **)wait_set->allocator.reallocate( \
+      wait_set->impl->RMWStorage, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \
+    if (!wait_set->impl->RMWStorage) { \
+      wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \
+      wait_set->size_of_##Type##s = 0; \
+      RCL_SET_ERROR_MSG("allocating memory failed"); \
+      return RCL_RET_BAD_ALLOC; \
+    } \
+    wait_set->impl->RMWCount = size; \
+  } \
   return RCL_RET_OK;
 
 rcl_ret_t
@@ -159,19 +218,25 @@ rcl_wait_set_add_subscription(
   rcl_wait_set_t * wait_set,
   const rcl_subscription_t * subscription)
 {
-  SET_ADD(subscription)
+  SET_ADD(subscription, rmw_subscriptions.subscribers)
 }
 
 rcl_ret_t
 rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set)
 {
-  SET_CLEAR(subscription)
+  SET_CLEAR(
+    subscription,
+    rmw_subscriptions.subscribers,
+    rmw_subscriptions.subscriber_count)
 }
 
 rcl_ret_t
 rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size)
 {
-  SET_RESIZE(subscription)
+  SET_RESIZE(
+    subscription,
+    rmw_subscriptions.subscribers,
+    rmw_subscriptions.subscriber_count)
 }
 
 rcl_ret_t
@@ -179,24 +244,78 @@ rcl_wait_set_add_guard_condition(
   rcl_wait_set_t * wait_set,
   const rcl_guard_condition_t * guard_condition)
 {
-  SET_ADD(guard_condition)
+  SET_ADD(guard_condition, rmw_guard_conditions.guard_conditions)
 }
 
 rcl_ret_t
 rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set)
 {
-  SET_CLEAR(guard_condition)
+  SET_CLEAR(
+    guard_condition,
+    rmw_guard_conditions.guard_conditions,
+    rmw_guard_conditions.guard_condition_count)
 }
 
 rcl_ret_t
 rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size)
 {
-  SET_RESIZE(guard_condition)
+  SET_RESIZE(
+    guard_condition,
+    rmw_guard_conditions.guard_conditions,
+    rmw_guard_conditions.guard_condition_count)
 }
 
 rcl_ret_t
-rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout)
+rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
 {
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timeout, RCL_RET_INVALID_ARGUMENT);
+  if (!__wait_set_is_valid(wait_set)) {
+    RCL_SET_ERROR_MSG("wait set is invalid");
+    return RCL_RET_WAIT_SET_INVALID;
+  }
+  if (wait_set->size_of_subscriptions == 0 && wait_set->size_of_guard_conditions == 0) {
+    RCL_SET_ERROR_MSG("wait set is empty");
+    return RCL_RET_WAIT_SET_EMPTY;
+  }
+  // Create dummy sets for currently unsupported wait-ables.
+  static rmw_services_t dummy_services = {0, NULL};
+  static rmw_clients_t dummy_clients = {0, NULL};
+  rmw_time_t rmw_timeout = rcl_time_t_from_int64_t_nanoseconds(timeout);
+  // Wait.
+  rmw_ret_t ret = rmw_wait(
+    &wait_set->impl->rmw_subscriptions,
+    &wait_set->impl->rmw_guard_conditions,
+    &dummy_services,
+    &dummy_clients,
+    &rmw_timeout
+  );
+  // Check for timeout.
+  if (ret == RMW_RET_TIMEOUT) {
+    // Assume none were set (because timeout was reached first), and clear all.
+    rcl_wait_set_clear_subscriptions(wait_set);
+    rcl_wait_set_clear_guard_conditions(wait_set);
+    return RCL_RET_TIMEOUT;
+  }
+  // Check for error.
+  if (ret != RMW_RET_OK) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    return RCL_RET_ERROR;
+  }
+  // Set corresponding rcl subscription handles NULL.
+  for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
+    assert(i < wait_set->impl->rmw_subscriptions.subscriber_count);  // Defensive.
+    if (!wait_set->impl->rmw_subscriptions.subscribers[i]) {
+      wait_set->subscriptions[i] = NULL;
+    }
+  }
+  // Set corresponding rcl guard_condition handles NULL.
+  for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) {
+    assert(i < wait_set->impl->rmw_guard_conditions.guard_condition_count);  // Defensive.
+    if (!wait_set->impl->rmw_guard_conditions.guard_conditions[i]) {
+      wait_set->guard_conditions[i] = NULL;
+    }
+  }
   return RCL_RET_OK;
 }