niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

add wait_for_action_server() for action clients (#349)

Browse source 
Signed-off-by: William Woodall <william@osrfoundation.org>
This commit is contained in:
William Woodall 2018-12-06 18:57:17 -08:00 • committed by Shane Loretz
parent 228cd0082c
commit 0518125724
2 changed files with 93 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

49
rcl_action/src/rcl_action/action_client.c
View file

@ -26,6 +26,7 @@ extern "C"
#include "rcl/client.h"
#include "rcl/error_handling.h"
#include "rcl/graph.h"
#include "rcl/subscription.h"
#include "rcl/types.h"
#include "rcl/wait.h"
@ -239,6 +240,54 @@ rcl_action_client_get_default_options(void)
return default_options;
}
rcl_ret_t
rcl_action_server_is_available(
const rcl_node_t * node,
const rcl_action_client_t * client,
bool * is_available)
{
if (!rcl_node_is_valid(node)) {
return RCL_RET_NODE_INVALID; // error is already set
}
if (!rcl_action_client_is_valid(client)) {
return RCL_RET_ACTION_CLIENT_INVALID; // error is already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(is_available, RCL_RET_INVALID_ARGUMENT);
bool temp;
rcl_ret_t ret;
*is_available = true;
ret = rcl_service_server_is_available(node, &(client->impl->goal_client), &temp);
if (RCL_RET_OK != ret) {
return ret; // error is already set
}
*is_available = (*is_available && temp);
ret = rcl_service_server_is_available(node, &(client->impl->cancel_client), &temp);
if (RCL_RET_OK != ret) {
return ret; // error is already set
}
*is_available = (*is_available && temp);
ret = rcl_service_server_is_available(node, &(client->impl->result_client), &temp);
if (RCL_RET_OK != ret) {
return ret; // error is already set
}
*is_available = (*is_available && temp);
size_t number_of_publishers;
ret = rcl_subscription_get_publisher_count(
&(client->impl->feedback_subscription), &number_of_publishers);
if (RCL_RET_OK != ret) {
return ret; // error is already set
}
*is_available = *is_available && (number_of_publishers != 0);
return RCL_RET_OK;
}
// \internal Sends an action client specific service request.
#define SEND_SERVICE_REQUEST(Type, request, sequence_number) \
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action " #Type " request"); \