Refactor actions wait set functions to leverage new wait set API

rcl_action/include/rcl_action/wait.h

@@ -30,13 +30,17 @@ extern "C"
  * This function will add the underlying service clients and subscribers to the wait set.
  *
  * This function behaves similar to adding subscriptions to the wait set, but will add
- * five elements:
+ * five entities:
  *
  * - Three service clients
- * - Two subscribers
+ * - Two subscriptions
  *
  * \see rcl_wait_set_add_subscription
  *
+ * If this function fails for any reason, `client_index` and `subscription_index` are not set.
+ * It is also possible the provided wait set is left in an inconsistent state (e.g. some
+ * of the clients and subscriptions were added to the wait set, but not all).
+ *
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------
@@ -48,6 +52,12 @@ extern "C"
  * \param[inout] wait_set struct where action client service client and subscription
  *   are to be stored
  * \param[in] action_client the action client to be added to the wait set
+ * \param[out] client_index the starting index in the wait set's client container where
+ *   the action clients underlying service clients were added. Optionally, set to `NULL`
+ *   if ignored.
+ * \param[out] subscription_index the starting index in the wait set's subscription container
+ *   where the action clients underlying subscriptions were added. Optionally, set to `NULL`
+ *   if ignored.
  * \return `RCL_RET_OK` if added successfully, or
  * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized, or
  * \return `RCL_RET_WAIT_SET_FULL` if the subscription set is full, or
@@ -59,7 +69,9 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_action_wait_set_add_action_client(
   rcl_wait_set_t * wait_set,
-  const rcl_action_client_t * action_client);
+  const rcl_action_client_t * action_client,
+  size_t * client_index,
+  size_t * subscription_index);
 
 /// Add a rcl_action_server_t to a wait set.
 /**
@@ -70,6 +82,10 @@ rcl_action_wait_set_add_action_client(
  *
  * \see rcl_wait_set_add_service
  *
+ * * If this function fails for any reason, `service_index` is not set.
+ * It is also possible the provided wait set is left in an inconsistent state (e.g. some
+ * of the clients and subscribers were added to the wait set, but not all).
+
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------
@@ -80,6 +96,9 @@ rcl_action_wait_set_add_action_client(
  *
  * \param[inout] wait_set struct where action server services are to be stored
  * \param[in] action_server the action server to be added to the wait set
+ * \param[out] service_index the starting index in the wait set's service container where
+ *   the action servers underlying services were added. Optionally, set to `NULL`
+ *   if ignored.
  * \return `RCL_RET_OK` if added successfully, or
  * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized, or
  * \return `RCL_RET_WAIT_SET_FULL` if the subscription set is full, or
@@ -91,7 +110,8 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_action_wait_set_add_action_server(
   rcl_wait_set_t * wait_set,
-  const rcl_action_server_t * action_server);
+  const rcl_action_server_t * action_server,
+  size_t * service_index);
 
 /// Get the number of wait set entities associated with a rcl_action_client_t.
 /**
@@ -179,6 +199,7 @@ rcl_action_server_wait_set_get_num_entities(
  * to call: rcl_action_take_feedback(), rcl_action_take_status(),
  * rcl_action_take_goal_response(), rcl_action_take_cancel_response(), or
  * rcl_action_take_result_response().
+ *
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------
@@ -199,8 +220,7 @@ rcl_action_server_wait_set_get_num_entities(
  * \param[out] is_result_response_ready `true` if there is a result response message ready
  *   to take, `false` otherwise
  * \return `RCL_RET_OK` if call is successful, or
- * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized or not used
+ * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is invalid, or
- *   for the action client alone, or
  * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
  * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action client is invalid, or
  * \return `RCL_RET_ERROR` if an unspecified error occurs.
@@ -222,6 +242,7 @@ rcl_action_client_wait_set_get_entities_ready(
  * The caller can use this function to determine the relevant action server functions
  * to call: rcl_action_take_goal_request(), rcl_action_take_cancel_request(), or
  * rcl_action_take_result_request().
+ *
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------
@@ -238,6 +259,7 @@ rcl_action_client_wait_set_get_entities_ready(
  * \param[out] is_result_request_ready `true` if there is a result request message ready
  *   to take, `false` otherwise
  * \return `RCL_RET_OK` if call is successful, or
+ * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is invalid, or
  * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
  * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action server is invalid, or
  * \return `RCL_RET_ERROR` if an unspecified error occurs.

rcl_action/src/rcl_action/action_client.c

@@ -46,6 +46,12 @@ typedef struct rcl_action_client_impl_t
   rcl_subscription_t status_subscription;
   rcl_action_client_options_t options;
   char * action_name;
+  // Wait set records
+  size_t wait_set_goal_client_index;
+  size_t wait_set_cancel_client_index;
+  size_t wait_set_result_client_index;
+  size_t wait_set_feedback_subscription_index;
+  size_t wait_set_status_subscription_index;
 } rcl_action_client_impl_t;
 
 rcl_action_client_t
@@ -409,39 +415,65 @@ rcl_action_client_is_valid(const rcl_action_client_t * action_client)
 rcl_ret_t
 rcl_action_wait_set_add_action_client(
   rcl_wait_set_t * wait_set,
-  const rcl_action_client_t * action_client)
+  const rcl_action_client_t * action_client,
+  size_t * client_index,
+  size_t * subscription_index)
 {
   rcl_ret_t ret;
   RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID);
   if (!rcl_action_client_is_valid(action_client)) {
     return RCL_RET_ACTION_CLIENT_INVALID;  // error already set
   }
+
   // Wait on action goal service response messages.
-  ret = rcl_wait_set_add_client(wait_set, &action_client->impl->goal_client);
+  ret = rcl_wait_set_add_client(
+    wait_set,
+    &action_client->impl->goal_client,
+    &action_client->impl->wait_set_goal_client_index);
   if (RCL_RET_OK != ret) {
     return ret;
   }
   // Wait on action cancel service response messages.
-  ret = rcl_wait_set_add_client(wait_set, &action_client->impl->cancel_client);
+  ret = rcl_wait_set_add_client(
+    wait_set,
+    &action_client->impl->cancel_client,
+    &action_client->impl->wait_set_cancel_client_index);
   if (RCL_RET_OK != ret) {
     return ret;
   }
   // Wait on action result service response messages.
-  ret = rcl_wait_set_add_client(wait_set, &action_client->impl->result_client);
+  ret = rcl_wait_set_add_client(
+    wait_set,
+    &action_client->impl->result_client,
+    &action_client->impl->wait_set_result_client_index);
   if (RCL_RET_OK != ret) {
     return ret;
   }
   // Wait on action feedback messages.
-  ret = rcl_wait_set_add_subscription(wait_set, &action_client->impl->feedback_subscription);
+  ret = rcl_wait_set_add_subscription(
+    wait_set,
+    &action_client->impl->feedback_subscription,
+    &action_client->impl->wait_set_feedback_subscription_index);
   if (RCL_RET_OK != ret) {
     return ret;
   }
-  return RCL_RET_OK;
   // Wait on action status messages.
-  ret = rcl_wait_set_add_subscription(wait_set, &action_client->impl->status_subscription);
+  ret = rcl_wait_set_add_subscription(
+    wait_set,
+    &action_client->impl->status_subscription,
+    &action_client->impl->wait_set_status_subscription_index);
   if (RCL_RET_OK != ret) {
     return ret;
   }
+
+  if (NULL != client_index) {
+    // The goal client was the first added
+    *client_index = action_client->impl->wait_set_goal_client_index;
+  }
+  if (NULL != subscription_index) {
+    // The feedback subscription was the first added
+    *subscription_index = action_client->impl->wait_set_feedback_subscription_index;
+  }
   return RCL_RET_OK;
 }
 
@@ -480,6 +512,7 @@ rcl_action_client_wait_set_get_entities_ready(
   bool * is_cancel_response_ready,
   bool * is_result_response_ready)
 {
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID);
   if (!rcl_action_client_is_valid(action_client)) {
     return RCL_RET_ACTION_CLIENT_INVALID;  // error already set
   }
@@ -488,19 +521,47 @@ rcl_action_client_wait_set_get_entities_ready(
   RCL_CHECK_ARGUMENT_FOR_NULL(is_goal_response_ready, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(is_cancel_response_ready, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(is_result_response_ready, RCL_RET_INVALID_ARGUMENT);
-  if (2 != wait_set->size_of_subscriptions || 3 != wait_set->size_of_clients) {
+
-    RCL_SET_ERROR_MSG("wait set not initialized or not used by the action client alone");
+  const rcl_action_client_impl_t * impl = action_client->impl;
-    return RCL_RET_WAIT_SET_INVALID;
+  const size_t feedback_index = impl->wait_set_feedback_subscription_index;
+  const size_t status_index = impl->wait_set_status_subscription_index;
+  const size_t goal_index = impl->wait_set_goal_client_index;
+  const size_t cancel_index = impl->wait_set_cancel_client_index;
+  const size_t result_index = impl->wait_set_result_client_index;
+  if (feedback_index >= wait_set->size_of_subscriptions) {
+    RCL_SET_ERROR_MSG("wait set index for feedback subscription is out of bounds");
+    return RCL_RET_ERROR;
   }
-  *is_feedback_ready = !!wait_set->subscriptions[0];
+  if (status_index >= wait_set->size_of_subscriptions) {
-  *is_status_ready = !!wait_set->subscriptions[1];
+    RCL_SET_ERROR_MSG("wait set index for status subscription is out of bounds");
-  *is_goal_response_ready = !!wait_set->clients[0];
+    return RCL_RET_ERROR;
-  *is_cancel_response_ready = !!wait_set->clients[1];
+  }
-  *is_result_response_ready = !!wait_set->clients[2];
+  if (goal_index >= wait_set->size_of_clients) {
+    RCL_SET_ERROR_MSG("wait set index for goal client is out of bounds");
+    return RCL_RET_ERROR;
+  }
+  if (cancel_index >= wait_set->size_of_clients) {
+    RCL_SET_ERROR_MSG("wait set index for cancel client is out of bounds");
+    return RCL_RET_ERROR;
+  }
+  if (result_index >= wait_set->size_of_clients) {
+    RCL_SET_ERROR_MSG("wait set index for result client is out of bounds");
+    return RCL_RET_ERROR;
+  }
+
+  const rcl_subscription_t * feedback_subscription = wait_set->subscriptions[feedback_index];
+  const rcl_subscription_t * status_subscription = wait_set->subscriptions[status_index];
+  const rcl_client_t * goal_client = wait_set->clients[goal_index];
+  const rcl_client_t * cancel_client = wait_set->clients[cancel_index];
+  const rcl_client_t * result_client = wait_set->clients[result_index];
+  *is_feedback_ready = (&impl->feedback_subscription == feedback_subscription);
+  *is_status_ready = (&impl->status_subscription == status_subscription);
+  *is_goal_response_ready = (&impl->goal_client == goal_client);
+  *is_cancel_response_ready = (&impl->cancel_client == cancel_client);
+  *is_result_response_ready = (&impl->result_client == result_client);
   return RCL_RET_OK;
 }
 
-
 #ifdef __cplusplus
 }
 #endif