[rcl action] Use macros instead of static function for action client impl.
This commit is contained in:
Michel Hidalgo
parent
78168bb52a
commit
c7e61834b4
2 changed files with 202 additions and 498 deletions
|
|
@ -415,7 +415,6 @@ rcl_action_take_status(
|
|||
const rcl_action_client_t * action_client,
|
||||
void * ros_status_array);
|
||||
|
||||
|
||||
/// Send a request for the result of a completed goal associated with a rcl_action_client_t.
|
||||
/**
|
||||
* This is a non-blocking call.
|
||||
|
|
|
|||
|
|
@ -55,244 +55,77 @@ rcl_action_get_zero_initialized_client(void)
|
|||
return null_action_client;
|
||||
}
|
||||
|
||||
// \internal Initialize client for the given action's goal service.
|
||||
static rcl_ret_t rcl_action_goal_service_client_init(
|
||||
rcl_client_t * goal_client,
|
||||
const rcl_node_t * node,
|
||||
const rosidl_service_type_support_t * goal_service_type_support,
|
||||
const char * action_name,
|
||||
const rcl_client_options_t * goal_client_options)
|
||||
{
|
||||
assert(NULL != goal_client);
|
||||
assert(NULL != node);
|
||||
assert(NULL != goal_service_type_support);
|
||||
assert(NULL != action_name);
|
||||
assert(NULL != goal_client_options);
|
||||
|
||||
rcl_ret_t ret;
|
||||
rcl_allocator_t allocator = goal_client_options->allocator;
|
||||
|
||||
char * goal_service_name = NULL;
|
||||
ret = rcl_action_get_goal_service_name(action_name, allocator, &goal_service_name);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
// \internal Initializes an action client specific service client.
|
||||
#define CLIENT_INIT(Type) \
|
||||
char * Type ## _service_name = NULL; \
|
||||
ret = rcl_action_get_ ## Type ## _service_name(action_name, allocator, &Type ## _service_name); \
|
||||
if (RCL_RET_OK != ret) { \
|
||||
RCL_SET_ERROR_MSG("failed to get " #Type " service name"); \
|
||||
if (RCL_RET_BAD_ALLOC == ret) { \
|
||||
ret = RCL_RET_BAD_ALLOC; \
|
||||
} else { \
|
||||
ret = RCL_RET_ERROR; \
|
||||
} \
|
||||
goto fail; \
|
||||
} \
|
||||
rcl_client_options_t Type ## _service_client_options = { \
|
||||
.qos = options->Type ## _service_qos, .allocator = allocator \
|
||||
}; \
|
||||
action_client->impl->Type ## _client = rcl_get_zero_initialized_client(); \
|
||||
ret = rcl_client_init( \
|
||||
&action_client->impl->Type ## _client, \
|
||||
node, \
|
||||
type_support->Type ## _service_type_support, \
|
||||
Type ## _service_name, \
|
||||
&Type ## _service_client_options); \
|
||||
allocator.deallocate(Type ## _service_name, allocator.state); \
|
||||
if (RCL_RET_OK != ret) { \
|
||||
if (RCL_RET_BAD_ALLOC == ret) { \
|
||||
ret = RCL_RET_BAD_ALLOC; \
|
||||
} else if (RCL_RET_SERVICE_NAME_INVALID == ret) { \
|
||||
ret = RCL_RET_ACTION_NAME_INVALID; \
|
||||
} else { \
|
||||
ret = RCL_RET_ERROR; \
|
||||
} \
|
||||
goto fail; \
|
||||
}
|
||||
|
||||
*goal_client = rcl_get_zero_initialized_client();
|
||||
|
||||
ret = rcl_client_init(
|
||||
goal_client, node,
|
||||
goal_service_type_support,
|
||||
goal_service_name,
|
||||
goal_client_options);
|
||||
|
||||
allocator.deallocate(goal_service_name, allocator.state);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_SERVICE_NAME_INVALID == ret) {
|
||||
return RCL_RET_ACTION_NAME_INVALID;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
// \internal Initializes an action client specific topic subscription.
|
||||
#define SUBSCRIPTION_INIT(Type) \
|
||||
char * Type ## _topic_name = NULL; \
|
||||
ret = rcl_action_get_ ## Type ## _topic_name(action_name, allocator, &Type ## _topic_name); \
|
||||
if (RCL_RET_OK != ret) { \
|
||||
RCL_SET_ERROR_MSG("failed to get " #Type " topic name"); \
|
||||
if (RCL_RET_BAD_ALLOC == ret) { \
|
||||
ret = RCL_RET_BAD_ALLOC; \
|
||||
} else { \
|
||||
ret = RCL_RET_ERROR; \
|
||||
} \
|
||||
goto fail; \
|
||||
} \
|
||||
rcl_subscription_options_t Type ## _topic_subscription_options = { \
|
||||
.qos = options->Type ## _topic_qos, \
|
||||
.ignore_local_publications = false, \
|
||||
.allocator = allocator \
|
||||
}; \
|
||||
action_client->impl->Type ## _subscription = rcl_get_zero_initialized_subscription(); \
|
||||
ret = rcl_subscription_init( \
|
||||
&action_client->impl->Type ## _subscription, \
|
||||
node, \
|
||||
type_support->Type ## _message_type_support, \
|
||||
Type ## _topic_name, \
|
||||
&Type ## _topic_subscription_options); \
|
||||
allocator.deallocate(Type ## _topic_name, allocator.state); \
|
||||
if (RCL_RET_OK != ret) { \
|
||||
if (RCL_RET_BAD_ALLOC == ret) { \
|
||||
ret = RCL_RET_BAD_ALLOC; \
|
||||
} else if (RCL_RET_TOPIC_NAME_INVALID == ret) { \
|
||||
ret = RCL_RET_ACTION_NAME_INVALID; \
|
||||
} else { \
|
||||
ret = RCL_RET_ERROR; \
|
||||
} \
|
||||
goto fail; \
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
// \internal Initialize client for the given action's goal cancel service.
|
||||
static rcl_ret_t rcl_action_cancel_service_client_init(
|
||||
rcl_client_t * cancel_client,
|
||||
const rcl_node_t * node,
|
||||
const rosidl_service_type_support_t * cancel_service_type_support,
|
||||
const char * action_name,
|
||||
const rcl_client_options_t * cancel_client_options)
|
||||
{
|
||||
assert(NULL != cancel_client);
|
||||
assert(NULL != node);
|
||||
assert(NULL != cancel_service_type_support);
|
||||
assert(NULL != action_name);
|
||||
assert(NULL != cancel_client_options);
|
||||
|
||||
rcl_ret_t ret;
|
||||
rcl_allocator_t allocator = cancel_client_options->allocator;
|
||||
|
||||
char * cancel_service_name = NULL;
|
||||
ret = rcl_action_get_cancel_service_name(action_name, allocator, &cancel_service_name);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
*cancel_client = rcl_get_zero_initialized_client();
|
||||
|
||||
ret = rcl_client_init(
|
||||
cancel_client, node,
|
||||
cancel_service_type_support,
|
||||
cancel_service_name,
|
||||
cancel_client_options);
|
||||
|
||||
allocator.deallocate(cancel_service_name, allocator.state);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_SERVICE_NAME_INVALID == ret) {
|
||||
return RCL_RET_ACTION_NAME_INVALID;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
// \internal Initialize client for the given action's goal result service.
|
||||
static rcl_ret_t rcl_action_result_client_init(
|
||||
rcl_client_t * result_client,
|
||||
const rcl_node_t * node,
|
||||
const rosidl_service_type_support_t * result_service_type_support,
|
||||
const char * action_name,
|
||||
const rcl_client_options_t * result_client_options)
|
||||
{
|
||||
assert(NULL != result_client);
|
||||
assert(NULL != node);
|
||||
assert(NULL != result_service_type_support);
|
||||
assert(NULL != action_name);
|
||||
assert(NULL != result_client_options);
|
||||
|
||||
rcl_ret_t ret;
|
||||
rcl_allocator_t allocator = result_client_options->allocator;
|
||||
|
||||
char * result_service_name = NULL;
|
||||
ret = rcl_action_get_result_service_name(action_name, allocator, &result_service_name);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
*result_client = rcl_get_zero_initialized_client();
|
||||
|
||||
ret = rcl_client_init(
|
||||
result_client, node,
|
||||
result_service_type_support,
|
||||
result_service_name,
|
||||
result_client_options);
|
||||
|
||||
allocator.deallocate(result_service_name, allocator.state);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_SERVICE_NAME_INVALID == ret) {
|
||||
return RCL_RET_ACTION_NAME_INVALID;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
// \internal Initialize subscription to the given action's feedback topic.
|
||||
static rcl_ret_t rcl_action_feedback_subscription_init(
|
||||
rcl_subscription_t * feedback_subscription,
|
||||
const rcl_node_t * node,
|
||||
const rosidl_message_type_support_t * feedback_message_type_support,
|
||||
const char * action_name,
|
||||
const rcl_subscription_options_t * feedback_subscription_options)
|
||||
{
|
||||
assert(NULL != feedback_subscription);
|
||||
assert(NULL != node);
|
||||
assert(NULL != feedback_message_type_support);
|
||||
assert(NULL != action_name);
|
||||
assert(NULL != feedback_subscription_options);
|
||||
|
||||
rcl_ret_t ret;
|
||||
rcl_allocator_t allocator = feedback_subscription_options->allocator;
|
||||
|
||||
char * feedback_topic_name = NULL;
|
||||
ret = rcl_action_get_feedback_topic_name(action_name, allocator, &feedback_topic_name);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
*feedback_subscription = rcl_get_zero_initialized_subscription();
|
||||
|
||||
ret = rcl_subscription_init(
|
||||
feedback_subscription, node,
|
||||
feedback_message_type_support,
|
||||
feedback_topic_name,
|
||||
feedback_subscription_options);
|
||||
|
||||
allocator.deallocate(feedback_topic_name, allocator.state);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_TOPIC_NAME_INVALID == ret) {
|
||||
return RCL_RET_ACTION_NAME_INVALID;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
// \internal Initialize subscription to the given action's status topic.
|
||||
static rcl_ret_t rcl_action_status_subscription_init(
|
||||
rcl_subscription_t * status_subscription, const rcl_node_t * node,
|
||||
const rosidl_message_type_support_t * status_message_type_support,
|
||||
const char * action_name,
|
||||
const rcl_subscription_options_t * status_subscription_options)
|
||||
{
|
||||
assert(NULL != status_subscription);
|
||||
assert(NULL != node);
|
||||
assert(NULL != status_message_type_support);
|
||||
assert(NULL != action_name);
|
||||
assert(NULL != status_subscription_options);
|
||||
|
||||
rcl_ret_t ret;
|
||||
rcl_allocator_t allocator = status_subscription_options->allocator;
|
||||
|
||||
char * status_topic_name = NULL;
|
||||
ret = rcl_action_get_status_topic_name(action_name, allocator, &status_topic_name);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
*status_subscription = rcl_get_zero_initialized_subscription();
|
||||
|
||||
ret = rcl_subscription_init(
|
||||
status_subscription, node,
|
||||
status_message_type_support,
|
||||
status_topic_name,
|
||||
status_subscription_options);
|
||||
|
||||
allocator.deallocate(status_topic_name, allocator.state);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_TOPIC_NAME_INVALID == ret) {
|
||||
return RCL_RET_ACTION_NAME_INVALID;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_action_client_init(
|
||||
|
|
@ -309,9 +142,8 @@ rcl_action_client_init(
|
|||
RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(action_name, RCL_RET_INVALID_ARGUMENT);
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
|
||||
rcl_allocator_t * allocator = (rcl_allocator_t *)&options->allocator;
|
||||
RCL_CHECK_ALLOCATOR_WITH_MSG(
|
||||
allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
|
||||
rcl_allocator_t allocator = options->allocator;
|
||||
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
|
||||
|
||||
rcl_ret_t ret = RCL_RET_OK;
|
||||
rcl_ret_t fini_ret = RCL_RET_OK;
|
||||
|
|
@ -322,85 +154,27 @@ rcl_action_client_init(
|
|||
return RCL_RET_ALREADY_INIT;
|
||||
}
|
||||
// Allocate space for the implementation struct.
|
||||
rcl_action_client_impl_t * impl = action_client->impl = allocator->allocate(
|
||||
sizeof(rcl_action_client_impl_t), allocator->state);
|
||||
RCL_CHECK_FOR_NULL_WITH_MSG(impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
|
||||
action_client->impl = allocator.allocate(sizeof(rcl_action_client_impl_t), allocator.state);
|
||||
RCL_CHECK_FOR_NULL_WITH_MSG(
|
||||
action_client->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
|
||||
|
||||
// Copy action client name and options.
|
||||
impl->options = *options;
|
||||
impl->action_name = rcutils_strdup(action_name, *((rcutils_allocator_t *)allocator));
|
||||
if (NULL == impl->action_name) {
|
||||
action_client->impl->action_name = rcutils_strdup(action_name, allocator);
|
||||
if (NULL == action_client->impl->action_name) {
|
||||
ret = RCL_RET_BAD_ALLOC;
|
||||
goto fail;
|
||||
}
|
||||
// Initialize action goal service client.
|
||||
rcl_client_options_t goal_client_options = {
|
||||
.qos = options->goal_service_qos, .allocator = *allocator
|
||||
};
|
||||
ret = rcl_action_goal_service_client_init(
|
||||
&impl->goal_client, node,
|
||||
type_support->goal_service_type_support,
|
||||
impl->action_name, &goal_client_options);
|
||||
if (RCL_RET_OK != ret) {
|
||||
RCUTILS_LOG_DEBUG_NAMED(
|
||||
ROS_PACKAGE_NAME, "Failed to initialize action goal client");
|
||||
goto fail;
|
||||
}
|
||||
// Initialize action cancel service client.
|
||||
rcl_client_options_t cancel_client_options = {
|
||||
.qos = options->cancel_service_qos, .allocator = *allocator
|
||||
};
|
||||
ret = rcl_action_cancel_service_client_init(
|
||||
&impl->cancel_client, node,
|
||||
type_support->cancel_service_type_support,
|
||||
impl->action_name, &cancel_client_options);
|
||||
if (RCL_RET_OK != ret) {
|
||||
RCUTILS_LOG_DEBUG_NAMED(
|
||||
ROS_PACKAGE_NAME, "Failed to initialize action cancel client");
|
||||
goto fail;
|
||||
}
|
||||
// Initialize action result service client.
|
||||
rcl_client_options_t result_client_options = {
|
||||
.qos = options->result_service_qos, .allocator = *allocator
|
||||
};
|
||||
ret = rcl_action_result_client_init(
|
||||
&impl->result_client, node,
|
||||
type_support->result_service_type_support,
|
||||
impl->action_name, &result_client_options);
|
||||
if (RCL_RET_OK != ret) {
|
||||
RCUTILS_LOG_DEBUG_NAMED(
|
||||
ROS_PACKAGE_NAME, "Failed to initialize action result client");
|
||||
goto fail;
|
||||
}
|
||||
// Initialize action feedback subscription client.
|
||||
rcl_subscription_options_t feedback_subscription_options = {
|
||||
.qos = options->feedback_topic_qos,
|
||||
.ignore_local_publications = false,
|
||||
.allocator = *allocator
|
||||
};
|
||||
ret = rcl_action_feedback_subscription_init(
|
||||
&impl->feedback_subscription, node,
|
||||
type_support->feedback_message_type_support,
|
||||
impl->action_name, &feedback_subscription_options);
|
||||
if (RCL_RET_OK != ret) {
|
||||
RCUTILS_LOG_DEBUG_NAMED(
|
||||
ROS_PACKAGE_NAME, "Failed to initialize feedback subscription");
|
||||
goto fail;
|
||||
}
|
||||
// Initialize action status subscription client.
|
||||
rcl_subscription_options_t status_subscription_options = {
|
||||
.qos = options->status_topic_qos,
|
||||
.ignore_local_publications = false,
|
||||
.allocator = *allocator
|
||||
};
|
||||
ret = rcl_action_status_subscription_init(
|
||||
&impl->status_subscription, node,
|
||||
type_support->status_message_type_support,
|
||||
impl->action_name, &status_subscription_options);
|
||||
if (RCL_RET_OK != ret) {
|
||||
RCUTILS_LOG_DEBUG_NAMED(
|
||||
ROS_PACKAGE_NAME, "Failed to initialize status subscription");
|
||||
goto fail;
|
||||
}
|
||||
action_client->impl->options = *options;
|
||||
|
||||
// Initialize action service clients.
|
||||
CLIENT_INIT(goal);
|
||||
CLIENT_INIT(cancel);
|
||||
CLIENT_INIT(result);
|
||||
|
||||
// Initialize action topic subscriptions.
|
||||
SUBSCRIPTION_INIT(feedback);
|
||||
SUBSCRIPTION_INIT(status);
|
||||
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized");
|
||||
return ret;
|
||||
fail:
|
||||
|
|
@ -423,40 +197,23 @@ rcl_action_client_fini(rcl_action_client_t * action_client, rcl_node_t * node)
|
|||
return RCL_RET_NODE_INVALID; // error already set
|
||||
}
|
||||
rcl_ret_t ret = RCL_RET_OK;
|
||||
rcl_ret_t fini_ret = RCL_RET_OK;
|
||||
rcl_action_client_impl_t * impl = action_client->impl;
|
||||
if (rcl_client_is_valid(&impl->goal_client)) {
|
||||
fini_ret = rcl_client_fini(&impl->goal_client, node);
|
||||
if (RCL_RET_OK != fini_ret) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (RCL_RET_OK != rcl_client_fini(&action_client->impl->goal_client, node)) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (rcl_client_is_valid(&impl->cancel_client)) {
|
||||
fini_ret = rcl_client_fini(&impl->cancel_client, node);
|
||||
if (RCL_RET_OK != fini_ret) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (RCL_RET_OK != rcl_client_fini(&action_client->impl->cancel_client, node)) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (rcl_client_is_valid(&impl->result_client)) {
|
||||
fini_ret = rcl_client_fini(&impl->result_client, node);
|
||||
if (RCL_RET_OK != fini_ret) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (RCL_RET_OK != rcl_client_fini(&action_client->impl->result_client, node)) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (rcl_subscription_is_valid(&impl->feedback_subscription)) {
|
||||
fini_ret = rcl_subscription_fini(&impl->feedback_subscription, node);
|
||||
if (RCL_RET_OK != fini_ret) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->feedback_subscription, node)) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (rcl_subscription_is_valid(&impl->status_subscription)) {
|
||||
fini_ret = rcl_subscription_fini(&impl->status_subscription, node);
|
||||
if (RCL_RET_OK != fini_ret) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->status_subscription, node)) {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
rcl_allocator_t * allocator = &impl->options.allocator;
|
||||
allocator->deallocate(impl->action_name, allocator->state);
|
||||
rcl_allocator_t * allocator = &action_client->impl->options.allocator;
|
||||
allocator->deallocate(action_client->impl->action_name, allocator->state);
|
||||
allocator->deallocate(action_client->impl, allocator->state);
|
||||
action_client->impl = NULL;
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client finalized");
|
||||
|
|
@ -476,25 +233,51 @@ rcl_action_client_get_default_options(void)
|
|||
return default_options;
|
||||
}
|
||||
|
||||
// \internal Sends an action client specific service request.
|
||||
#define SEND_SERVICE_REQUEST(Type) \
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action " #Type " request"); \
|
||||
if (!rcl_action_client_is_valid(action_client)) { \
|
||||
return RCL_RET_ACTION_SERVER_INVALID; /* error already set */ \
|
||||
} \
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type ## _request, RCL_RET_INVALID_ARGUMENT); \
|
||||
int64_t sequence_number; /* ignored */ \
|
||||
rcl_ret_t ret = rcl_send_request( \
|
||||
&action_client->impl->Type ## _client, ros_ ## Type ## _request, &sequence_number); \
|
||||
if (RCL_RET_OK != ret) { \
|
||||
return RCL_RET_ERROR; /* error already set */ \
|
||||
} \
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " request sent"); \
|
||||
return RCL_RET_OK;
|
||||
|
||||
// \internal Takes an action client specific service response.
|
||||
#define TAKE_SERVICE_RESPONSE(Type) \
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type " response"); \
|
||||
if (!rcl_action_client_is_valid(action_client)) { \
|
||||
return RCL_RET_ACTION_SERVER_INVALID; /* error already set */ \
|
||||
} \
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type ## _response, RCL_RET_INVALID_ARGUMENT); \
|
||||
rmw_request_id_t request_header; /* ignored */ \
|
||||
rcl_ret_t ret = rcl_take_response( \
|
||||
&action_client->impl->Type ## _client, &request_header, ros_ ## Type ## _response); \
|
||||
if (RCL_RET_OK != ret) { \
|
||||
if (RCL_RET_BAD_ALLOC == ret) { \
|
||||
return RCL_RET_BAD_ALLOC; /* error already set */ \
|
||||
} \
|
||||
if (RCL_RET_CLIENT_TAKE_FAILED == ret) { \
|
||||
return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \
|
||||
} \
|
||||
return RCL_RET_ERROR; /* error already set */ \
|
||||
} \
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " response taken"); \
|
||||
return RCL_RET_OK;
|
||||
|
||||
|
||||
rcl_ret_t
|
||||
rcl_action_send_goal_request(
|
||||
const rcl_action_client_t * action_client,
|
||||
const void * ros_goal_request)
|
||||
{
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action goal request");
|
||||
if (!rcl_action_client_is_valid(action_client)) {
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; // error already set
|
||||
}
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_goal_request, RCL_RET_INVALID_ARGUMENT);
|
||||
int64_t ignored_sequence_number;
|
||||
rcl_ret_t ret = rcl_send_request(
|
||||
&action_client->impl->goal_client,
|
||||
ros_goal_request, &ignored_sequence_number);
|
||||
if (RCL_RET_OK != ret) {
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action goal request sent");
|
||||
return RCL_RET_OK;
|
||||
SEND_SERVICE_REQUEST(goal)
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
|
|
@ -502,79 +285,7 @@ rcl_action_take_goal_response(
|
|||
const rcl_action_client_t * action_client,
|
||||
void * ros_goal_response)
|
||||
{
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action goal response");
|
||||
if (!rcl_action_client_is_valid(action_client)) {
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; // error already set
|
||||
}
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_goal_response, RCL_RET_INVALID_ARGUMENT);
|
||||
rmw_request_id_t ignored_request_header;
|
||||
rcl_ret_t ret = rcl_take_response(
|
||||
&action_client->impl->goal_client,
|
||||
&ignored_request_header, ros_goal_response);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_CLIENT_TAKE_FAILED == ret) {
|
||||
ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED;
|
||||
} else {
|
||||
ret = RCL_RET_ERROR;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action goal response taken");
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_action_take_feedback(
|
||||
const rcl_action_client_t * action_client,
|
||||
void * ros_feedback)
|
||||
{
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action feedback");
|
||||
if (!rcl_action_client_is_valid(action_client)) {
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; // error already set
|
||||
}
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_feedback, RCL_RET_INVALID_ARGUMENT);
|
||||
rmw_message_info_t ignored_message_info;
|
||||
rcl_ret_t ret = rcl_take(
|
||||
&action_client->impl->feedback_subscription,
|
||||
ros_feedback, &ignored_message_info);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
|
||||
return RCL_RET_ACTION_CLIENT_TAKE_FAILED;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action feedback taken");
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_action_take_status(
|
||||
const rcl_action_client_t * action_client,
|
||||
void * ros_status_array)
|
||||
{
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action status");
|
||||
if (!rcl_action_client_is_valid(action_client)) {
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; // error already set
|
||||
}
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_status_array, RCL_RET_INVALID_ARGUMENT);
|
||||
rmw_message_info_t ignored_message_info;
|
||||
rcl_ret_t ret = rcl_take(
|
||||
&action_client->impl->status_subscription,
|
||||
ros_status_array, &ignored_message_info);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
|
||||
return RCL_RET_ACTION_CLIENT_TAKE_FAILED;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action status taken");
|
||||
return RCL_RET_OK;
|
||||
TAKE_SERVICE_RESPONSE(goal);
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
|
|
@ -582,47 +293,15 @@ rcl_action_send_result_request(
|
|||
const rcl_action_client_t * action_client,
|
||||
const void * ros_result_request)
|
||||
{
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action result request");
|
||||
if (!rcl_action_client_is_valid(action_client)) {
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; // error already set
|
||||
}
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_result_request, RCL_RET_INVALID_ARGUMENT);
|
||||
int64_t ignored_sequence_number;
|
||||
rcl_ret_t ret = rcl_send_request(
|
||||
&action_client->impl->result_client,
|
||||
ros_result_request, &ignored_sequence_number);
|
||||
if (RCL_RET_OK != ret) {
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action result request sent");
|
||||
return RCL_RET_OK;
|
||||
SEND_SERVICE_REQUEST(result);
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_action_take_result_response(
|
||||
const rcl_action_client_t * action_client,
|
||||
void * ros_result)
|
||||
void * ros_result_response)
|
||||
{
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action result response");
|
||||
if (!rcl_action_client_is_valid(action_client)) {
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; // error already set
|
||||
}
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_result, RCL_RET_INVALID_ARGUMENT);
|
||||
rmw_request_id_t ignored_response_header;
|
||||
rcl_ret_t ret = rcl_take_response(
|
||||
&action_client->impl->result_client,
|
||||
&ignored_response_header, ros_result);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_CLIENT_TAKE_FAILED == ret) {
|
||||
return RCL_RET_ACTION_CLIENT_TAKE_FAILED;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action result response taken");
|
||||
return RCL_RET_OK;
|
||||
TAKE_SERVICE_RESPONSE(result);
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
|
|
@ -630,20 +309,7 @@ rcl_action_send_cancel_request(
|
|||
const rcl_action_client_t * action_client,
|
||||
const void * ros_cancel_request)
|
||||
{
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action cancel request");
|
||||
if (!rcl_action_client_is_valid(action_client)) {
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; // error already set
|
||||
}
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_cancel_request, RCL_RET_INVALID_ARGUMENT);
|
||||
int64_t ignored_sequence_number;
|
||||
rcl_ret_t ret = rcl_send_request(
|
||||
&action_client->impl->cancel_client,
|
||||
ros_cancel_request, &ignored_sequence_number);
|
||||
if (RCL_RET_OK != ret) {
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel request sent");
|
||||
return RCL_RET_OK;
|
||||
SEND_SERVICE_REQUEST(cancel);
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
|
|
@ -651,26 +317,45 @@ rcl_action_take_cancel_response(
|
|||
const rcl_action_client_t * action_client,
|
||||
void * ros_cancel_response)
|
||||
{
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action cancel response");
|
||||
if (!rcl_action_client_is_valid(action_client)) {
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; // error already set
|
||||
}
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_cancel_response, RCL_RET_INVALID_ARGUMENT);
|
||||
rmw_request_id_t ignored_response_header;
|
||||
rcl_ret_t ret = rcl_take_response(
|
||||
&action_client->impl->cancel_client,
|
||||
&ignored_response_header, ros_cancel_response);
|
||||
if (RCL_RET_OK != ret) {
|
||||
if (RCL_RET_CLIENT_TAKE_FAILED == ret) {
|
||||
return RCL_RET_ACTION_CLIENT_TAKE_FAILED;
|
||||
}
|
||||
if (RCL_RET_BAD_ALLOC == ret) {
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel response taken");
|
||||
TAKE_SERVICE_RESPONSE(cancel);
|
||||
}
|
||||
|
||||
// \internal Takes an action client specific topic message.
|
||||
#define TAKE_MESSAGE(Type) \
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type); \
|
||||
if (!rcl_action_client_is_valid(action_client)) { \
|
||||
return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \
|
||||
} \
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type, RCL_RET_INVALID_ARGUMENT); \
|
||||
rmw_message_info_t message_info; /* ignored */ \
|
||||
rcl_ret_t ret = rcl_take( \
|
||||
&action_client->impl->Type ## _subscription, ros_ ## Type, &message_info); \
|
||||
if (RCL_RET_OK != ret) { \
|
||||
if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { \
|
||||
return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \
|
||||
} \
|
||||
if (RCL_RET_BAD_ALLOC == ret) { \
|
||||
return RCL_RET_BAD_ALLOC; \
|
||||
} \
|
||||
return RCL_RET_ERROR; \
|
||||
} \
|
||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " taken"); \
|
||||
return RCL_RET_OK;
|
||||
|
||||
rcl_ret_t
|
||||
rcl_action_take_feedback(
|
||||
const rcl_action_client_t * action_client,
|
||||
void * ros_feedback)
|
||||
{
|
||||
TAKE_MESSAGE(feedback);
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_action_take_status(
|
||||
const rcl_action_client_t * action_client,
|
||||
void * ros_status)
|
||||
{
|
||||
TAKE_MESSAGE(status);
|
||||
}
|
||||
|
||||
const char *
|
||||
|
|
@ -698,6 +383,26 @@ rcl_action_client_is_valid(const rcl_action_client_t * action_client)
|
|||
action_client, "action client pointer is invalid", return false);
|
||||
RCL_CHECK_FOR_NULL_WITH_MSG(
|
||||
action_client->impl, "action client implementation is invalid", return false);
|
||||
if (!rcl_client_is_valid(&action_client->impl->goal_client)) {
|
||||
RCL_SET_ERROR_MSG("goal client is invalid");
|
||||
return false;
|
||||
}
|
||||
if (!rcl_client_is_valid(&action_client->impl->cancel_client)) {
|
||||
RCL_SET_ERROR_MSG("cancel client is invalid");
|
||||
return false;
|
||||
}
|
||||
if (!rcl_client_is_valid(&action_client->impl->result_client)) {
|
||||
RCL_SET_ERROR_MSG("result client is invalid");
|
||||
return false;
|
||||
}
|
||||
if (!rcl_subscription_is_valid(&action_client->impl->feedback_subscription)) {
|
||||
RCL_SET_ERROR_MSG("feedback subscription is invalid");
|
||||
return false;
|
||||
}
|
||||
if (!rcl_subscription_is_valid(&action_client->impl->status_subscription)) {
|
||||
RCL_SET_ERROR_MSG("status subscription is invalid");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue