everything but rcl_wait

rcl/include/rcl/allocator.h

@@ -35,6 +35,7 @@ typedef struct rcl_allocator_t {
    * This should behave as realloc is described, as opposed to reallocf, i.e.
    * the memory given by pointer will not be free'd automatically if realloc
    * fails.
+   * This function must be able to take an input pointer of NULL and succeed.
    */
   void * (*reallocate)(void * pointer, size_t size, void * state);
   /// Implementation defined state storage.

rcl/include/rcl/types.h

@@ -36,7 +36,8 @@ typedef rmw_ret_t rcl_ret_t;
 // rcl service server specific ret codes in 6XX
 // rcl guard condition specific ret codes in 7XX
 // rcl wait and wait set specific ret codes in 8XX
-#define RCL_RET_WAIT_SET_EMPTY 800
+#define RCL_RET_WAIT_SET_INVALID 800
-#define RCL_RET_WAIT_SET_FULL 801
+#define RCL_RET_WAIT_SET_EMPTY 801
+#define RCL_RET_WAIT_SET_FULL 802
 
 #endif  // RCL__TYPES_H_

rcl/include/rcl/wait.h

@@ -33,11 +33,11 @@ struct rcl_wait_set_impl_t;
 /// Container for subscription's, guard condition's, etc to be waited on.
 typedef struct rcl_wait_set_t {
   /// Storage for subscription pointers.
-  rcl_subscription_t ** subscriptions;
+  const rcl_subscription_t ** subscriptions;
   size_t size_of_subscriptions;
   size_t __current_subscription_offset;
   /// Storage for guard condition pointers.
-  rcl_guard_condition_t ** guard_conditions;
+  const rcl_guard_condition_t ** guard_conditions;
   size_t size_of_guard_conditions;
   size_t __current_guard_condition_offset;
   /// Allocator for storage.
@@ -55,7 +55,7 @@ rcl_get_zero_initialized_wait_set();
 /// Initialize a rcl wait set with space for items to be waited on.
 /* This function allocates space for the subscriptions and other wait-able
  * entities that can be stored in the wait set.
- * It also sets the allocator to the given one and initializes the pruned
+ * It also sets the allocator to the given allocator and initializes the pruned
  * member to be false.
  *
  * The wait_set struct should be allocated and initialized to NULL.
@@ -73,7 +73,7 @@ rcl_get_zero_initialized_wait_set();
  *
  *   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
  *   rcl_ret_t ret = rcl_wait_set_init(&wait_set, 42, 42, rcl_get_default_allocator());
- *   // ... error handling, and then after using call matching fini:
+ *   // ... error handling, then use it, then call the matching fini:
  *   ret = rcl_wait_set_fini(&wait_set);
  *   // ... error handling
  *
@@ -82,12 +82,12 @@ rcl_get_zero_initialized_wait_set();
  * allocator is shared with other parts of the system.
  *
  * \param[inout] wait_set the wait set struct to be initialized
- * \param[in] number_of_subscriptions size of the subscriptions set
+ * \param[in] number_of_subscriptions non-zero size of the subscriptions set
- * \param[in] number_of_guard_conditions size of the guard conditions set
+ * \param[in] number_of_guard_conditions non-zero size of the guard conditions set
  * \param[in] allocator the allocator to use when allocating space in the sets
  * \return RCL_RET_OK if the wait set is initialized successfully, or
  *         RCL_RET_ALREADY_INIT if the wait set is not zero initialized, or
- *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
  *         RCL_RET_BAD_ALLOC if allocating memory failed, or
  *         RCL_RET_ERROR if an unspecified error occurs.
  */
@@ -113,13 +113,13 @@ rcl_wait_set_init(
  *
  * \param[inout] wait_set the wait set struct to be finalized.
  * \return RCL_RET_OK if the finalization was successful, or
- *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
  *         RCL_RET_ERROR if an unspecified error occurs.
  */
 rcl_ret_t
 rcl_wait_set_fini(rcl_wait_set_t * wait_set);
 
-/// Stores a pointer to the given subscription in next empty spot in the set.
+/// Stores a pointer to the given subscription in the next empty spot in the set.
 /* This function does not guarantee that the subscription is not already in the
  * wait set.
  *
@@ -128,9 +128,9 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set);
  * \param[inout] wait_set struct in which the subscription is to be stored
  * \param[in] subscription the subscription to be added to the wait set
  * \return RCL_RET_OK if added successfully, 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_FULL if the subscription set is full, or
- *         RCL_RET_NOT_INIT if the wait set is zero initialized, or
- *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
  *         RCL_RET_ERROR if an unspecified error occurs.
  */
 rcl_ret_t
@@ -148,8 +148,8 @@ rcl_wait_set_add_subscription(
  *
  * \param[inout] wait_set struct to have its subscriptions cleared
  * \return RCL_RET_OK if cleared successfully, or
- *         RCL_RET_NOT_INIT if the wait set is zero initialized, or
+ *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
- *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or
  *         RCL_RET_ERROR if an unspecified error occurs.
  */
 rcl_ret_t
@@ -176,14 +176,14 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set);
  * \param[inout] wait_set struct to have its subscriptions cleared
  * \param[in] size a size for the new set
  * \return RCL_RET_OK if resized successfully, or
- *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
  *         RCL_RET_BAD_ALLOC if allocating memory failed, or
  *         RCL_RET_ERROR if an unspecified error occurs.
  */
 rcl_ret_t
 rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size);
 
-/// Stores a pointer to the guard condition in next empty spot in the set.
+/// Stores a pointer to the guard condition in the next empty spot in the set.
 /* This function behaves exactly the same as for subscriptions.
  * \see rcl_wait_set_add_subscription
  */
@@ -281,6 +281,9 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
  * The timeout's value is not changed.
  * Passing a timeout struct with uninitialized memory is undefined behavior.
  *
+ * \TODO(wjwwood) this function should probably be thread-safe with itself but
+ *                it's not clear to me what happens if the wait sets being
+ *                waited on can be overlapping or not or if we can even check.
  * This function is not thread-safe and cannot be called concurrently, even if
  * the given wait sets are not the same and non-overlapping in contents.
  *
@@ -290,7 +293,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
  *         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 arugments are invalid, or
+ *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
  *         RCL_RET_ERROR an unspecified error occur.
  */
 rcl_ret_t

rcl/src/rcl/wait.c

@@ -19,6 +19,187 @@ extern "C"
 
 #include "rcl/wait.h"
 
+#include <stdbool.h>
+#include <string.h>
+
+#include "rcl/error_handling.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++.
+} rcl_wait_set_impl_t;
+
+rcl_wait_set_t
+rcl_get_zero_initialized_wait_set()
+{
+  static rcl_wait_set_t null_wait_set = {0};
+  return null_wait_set;
+}
+
+static bool
+__wait_set_is_valid(rcl_wait_set_t * wait_set)
+{
+  return wait_set && wait_set->impl;
+}
+
+rcl_ret_t
+rcl_wait_set_init(
+  rcl_wait_set_t * wait_set,
+  size_t number_of_subscriptions,
+  size_t number_of_guard_conditions,
+  rcl_allocator_t allocator)
+{
+  rcl_ret_t fail_ret = RCL_RET_ERROR;
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
+  if (__wait_set_is_valid(wait_set)) {
+    RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized.");
+    return RCL_RET_ALREADY_INIT;
+  }
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    number_of_subscriptions,
+    "number_of_subscriptions cannot be 0", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    number_of_guard_conditions,
+    "number_of_guard_conditions cannot be 0", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    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.
+  wait_set->impl = (rcl_wait_set_impl_t *)0xDEADBEEF;
+  // Initialize subscription space.
+  rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions);
+  rcl_wait_set_clear_subscriptions(wait_set);
+  // Initialize guard condition space.
+  rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions);
+  rcl_wait_set_clear_guard_conditions(wait_set);
+  // Set allocator.
+  wait_set->allocator = allocator;
+  // Initialize pruned.
+  wait_set->pruned = false;
+  return RCL_RET_OK;
+fail:
+  if (wait_set->subscriptions) {
+    allocator.deallocate(wait_set->subscriptions, allocator.state);
+  }
+  if (wait_set->guard_conditions) {
+    allocator.deallocate(wait_set->guard_conditions, allocator.state);
+  }
+  return fail_ret;
+}
+
+rcl_ret_t
+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->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) \
+  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)) { \
+    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; \
+  return RCL_RET_OK;
+
+#define SET_CLEAR(Type) \
+  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; \
+  return RCL_RET_OK;
+
+#define SET_RESIZE(Type) \
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
+  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; \
+    return RCL_RET_OK; \
+  } \
+  wait_set->size_of_##Type##s = 0; \
+  wait_set->Type##s = (const rcl_##Type##_t **)wait_set->allocator.reallocate( \
+    wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \
+  RCL_CHECK_FOR_NULL_WITH_MSG( \
+    wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \
+  wait_set->size_of_##Type##s = size; \
+  return RCL_RET_OK;
+
+rcl_ret_t
+rcl_wait_set_add_subscription(
+  rcl_wait_set_t * wait_set,
+  const rcl_subscription_t * subscription)
+{
+  SET_ADD(subscription)
+}
+
+rcl_ret_t
+rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set)
+{
+  SET_CLEAR(subscription)
+}
+
+rcl_ret_t
+rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size)
+{
+  SET_RESIZE(subscription)
+}
+
+rcl_ret_t
+rcl_wait_set_add_guard_condition(
+  rcl_wait_set_t * wait_set,
+  const rcl_guard_condition_t * guard_condition)
+{
+  SET_ADD(guard_condition)
+}
+
+rcl_ret_t
+rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set)
+{
+  SET_CLEAR(guard_condition)
+}
+
+rcl_ret_t
+rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size)
+{
+  SET_RESIZE(guard_condition)
+}
+
+rcl_ret_t
+rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout)
+{
+  return RCL_RET_OK;
+}
+
 #if __cplusplus
 }
 #endif