Skip to content

Commit

Permalink
Refactor actions wait set functions to leverage new wait set API
Browse files Browse the repository at this point in the history
  • Loading branch information
@jacobperron
jacobperron committed Nov 20, 2018
1 parent 87d41f0 commit d86d1c4
Show file tree
Hide file tree
Showing 2 changed files with 106 additions and 23 deletions.
34 changes: 28 additions & 6 deletions rcl_action/include/rcl_action/wait.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
* ------------------ | -------------
Expand All @@ -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
Expand All @@ -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.
/**
Expand All @@ -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
* ------------------ | -------------
Expand All @@ -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
Expand All @@ -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.
/**
Expand Down Expand Up @@ -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
* ------------------ | -------------
Expand All @@ -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
* for the action client alone, 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 client is invalid, or
* \return `RCL_RET_ERROR` if an unspecified error occurs.
Expand All @@ -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
* ------------------ | -------------
Expand All @@ -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.
Expand Down
95 changes: 78 additions & 17 deletions rcl_action/src/rcl_action/action_client.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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
}
Expand All @@ -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");
return RCL_RET_WAIT_SET_INVALID;
}
*is_feedback_ready = !!wait_set->subscriptions[0];
*is_status_ready = !!wait_set->subscriptions[1];
*is_goal_response_ready = !!wait_set->clients[0];
*is_cancel_response_ready = !!wait_set->clients[1];
*is_result_response_ready = !!wait_set->clients[2];

const rcl_action_client_impl_t * impl = action_client->impl;
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;
}
if (status_index >= wait_set->size_of_subscriptions) {
RCL_SET_ERROR_MSG("wait set index for status subscription is out of bounds");
return RCL_RET_ERROR;
}
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

0 comments on commit d86d1c4

Please sign in to comment.