diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt
index 83c9931..79c997b 100644
--- a/rcl_action/CMakeLists.txt
+++ b/rcl_action/CMakeLists.txt
@@ -32,6 +32,7 @@ add_executable(test_compile_headers
)
set(rcl_action_sources
+ src/${PROJECT_NAME}/action_client.c
src/${PROJECT_NAME}/goal_handle.c
src/${PROJECT_NAME}/goal_state_machine.c
src/${PROJECT_NAME}/names.c
@@ -69,9 +70,22 @@ install(TARGETS ${PROJECT_NAME}
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
+ find_package(test_msgs REQUIRED)
ament_lint_auto_find_test_dependencies()
ament_find_gtest()
# Gtests
+ ament_add_gtest(test_action_client
+ test/rcl_action/test_action_client.cpp
+ )
+ if(TARGET test_action_client)
+ target_include_directories(test_action_client PUBLIC
+ include
+ )
+ target_link_libraries(test_action_client
+ ${PROJECT_NAME}
+ )
+ ament_target_dependencies(test_action_client "rcl" "test_msgs")
+ endif()
ament_add_gtest(test_goal_handle
test/rcl_action/test_goal_handle.cpp
)
diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h
index 37ef690..7b680d7 100644
--- a/rcl_action/include/rcl_action/action_client.h
+++ b/rcl_action/include/rcl_action/action_client.h
@@ -20,10 +20,6 @@ extern "C"
{
#endif
-// TODO(jacobperron): replace type support typedef with one defined in rosdl_generator_c
-// #include "rosidl_generator_c/action_type_support_struct.h"
-typedef struct rosidl_action_type_support_t rosidl_action_type_support_t;
-
#include "rcl_action/types.h"
#include "rcl_action/visibility_control.h"
#include "rcl/macros.h"
@@ -167,6 +163,7 @@ rcl_action_get_zero_initialized_client(void);
* \return `RCL_RET_OK` if action_client was initialized successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_NODE_INVALID` if the node is invalid, or
+ * \return `RCL_RET_ALREADY_INIT` if the action client is already initialized, or
* \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
* \return `RCL_RET_ACTION_NAME_INVALID` if the given action name is invalid, or
* \return `RCL_RET_ERROR` if an unspecified error occurs.
@@ -176,7 +173,7 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_client_init(
rcl_action_client_t * action_client,
- const rcl_node_t * node,
+ rcl_node_t * node,
const rosidl_action_type_support_t * type_support,
const char * action_name,
const rcl_action_client_options_t * options);
@@ -355,9 +352,7 @@ rcl_action_take_goal_response(
* Lock-Free | Yes
* [1] only if required when filling the feedback message, avoided for fixed sizes
*
- * \param[in] action_client handle to the client that will take the goal response
- * \param[out] goal_info pointer to a struct for meta-data about the goal associated
- * with taken feedback
+ * \param[in] action_client handle to the client that will take action feedback
* \param[out] ros_feedback pointer to the ROS feedback message.
* \return `RCL_RET_OK` if the response was taken successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@@ -661,7 +656,7 @@ RCL_WARN_UNUSED
const rcl_action_client_options_t *
rcl_action_client_get_options(const rcl_action_client_t * action_client);
-/// Check that a rcl_action_clieint_t is valid.
+/// Check that a rcl_action_client_t is valid.
/**
* The bool returned is `false` if `action_client` is invalid.
* The bool returned is `true` otherwise.
@@ -677,14 +672,12 @@ rcl_action_client_get_options(const rcl_action_client_t * action_client);
* Lock-Free | Yes
*
* \param[in] action_client pointer to the rcl action client
- * \param[in] error_msg_allocator a valid allocator or `NULL`
* \return `true` if `action_client` is valid, otherwise `false`
*/
RCL_ACTION_PUBLIC
bool
rcl_action_client_is_valid(
- const rcl_action_client_t * action_client,
- rcl_allocator_t * error_msg_allocator);
+ const rcl_action_client_t * action_client);
#ifdef __cplusplus
}
diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h
index 80c14e1..4195b61 100644
--- a/rcl_action/include/rcl_action/action_server.h
+++ b/rcl_action/include/rcl_action/action_server.h
@@ -20,10 +20,6 @@ extern "C"
{
#endif
-// TODO(jacobperron): replace type support typedef with one defined in rosdl_generator_c
-// #include "rosidl_generator_c/action_type_support_struct.h"
-typedef struct rosidl_action_type_support_t rosidl_action_type_support_t;
-
#include "rcl_action/goal_handle.h"
#include "rcl_action/types.h"
#include "rcl_action/visibility_control.h"
diff --git a/rcl_action/include/rcl_action/types.h b/rcl_action/include/rcl_action/types.h
index f9a86ad..cb686db 100644
--- a/rcl_action/include/rcl_action/types.h
+++ b/rcl_action/include/rcl_action/types.h
@@ -31,6 +31,7 @@ extern "C"
#include "rcl/macros.h"
#include "rcl/types.h"
+#include "rosidl_generator_c/action_type_support_struct.h"
// rcl action specific ret codes in 2XXX
/// Action name does not pass validation return code.
diff --git a/rcl_action/include/rcl_action/wait.h b/rcl_action/include/rcl_action/wait.h
index 3ea8b55..37d13d2 100644
--- a/rcl_action/include/rcl_action/wait.h
+++ b/rcl_action/include/rcl_action/wait.h
@@ -27,13 +27,13 @@ extern "C"
/// Add a rcl_action_client_t to a wait set.
/**
- * This function will add the underlying service clients and subscriber to the wait set.
+ * 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
- * four elements:
+ * five elements:
*
* - Three service clients
- * - One subscriber
+ * - Two subscribers
*
* \see rcl_wait_set_add_subscription
*
@@ -199,6 +199,8 @@ 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_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.
diff --git a/rcl_action/package.xml b/rcl_action/package.xml
index c5eab74..da9d64c 100644
--- a/rcl_action/package.xml
+++ b/rcl_action/package.xml
@@ -18,6 +18,7 @@
ament_cmake_gtest
ament_lint_common
ament_lint_auto
+ test_msgs
ament_cmake
diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c
new file mode 100644
index 0000000..48d4049
--- /dev/null
+++ b/rcl_action/src/rcl_action/action_client.c
@@ -0,0 +1,506 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "rcl_action/action_client.h"
+
+#include "rcl_action/default_qos.h"
+#include "rcl_action/names.h"
+#include "rcl_action/types.h"
+#include "rcl_action/wait.h"
+
+#include "rcl/client.h"
+#include "rcl/error_handling.h"
+#include "rcl/subscription.h"
+#include "rcl/types.h"
+#include "rcl/wait.h"
+
+#include "rcutils/logging_macros.h"
+#include "rcutils/strdup.h"
+
+#include "rmw/qos_profiles.h"
+#include "rmw/types.h"
+
+
+typedef struct rcl_action_client_impl_t
+{
+ rcl_client_t goal_client;
+ rcl_client_t cancel_client;
+ rcl_client_t result_client;
+ rcl_subscription_t feedback_subscription;
+ rcl_subscription_t status_subscription;
+ rcl_action_client_options_t options;
+ char * action_name;
+} rcl_action_client_impl_t;
+
+rcl_action_client_t
+rcl_action_get_zero_initialized_client(void)
+{
+ static rcl_action_client_t null_action_client = {0};
+ return null_action_client;
+}
+
+// \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; \
+ }
+
+// \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; \
+ }
+
+rcl_ret_t
+rcl_action_client_init(
+ rcl_action_client_t * action_client,
+ rcl_node_t * node,
+ const rosidl_action_type_support_t * type_support,
+ const char * action_name,
+ const rcl_action_client_options_t * options)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT);
+ if (!rcl_node_is_valid(node)) {
+ return RCL_RET_NODE_INVALID;
+ }
+ 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 = 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;
+ RCUTILS_LOG_DEBUG_NAMED(
+ ROS_PACKAGE_NAME, "Initializing client for action name '%s'", action_name);
+ if (NULL != action_client->impl) {
+ RCL_SET_ERROR_MSG("action client already initialized, or memory was uninitialized");
+ return RCL_RET_ALREADY_INIT;
+ }
+ // Allocate space for the implementation struct.
+ 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.
+ action_client->impl->action_name = rcutils_strdup(action_name, allocator);
+ if (NULL == action_client->impl->action_name) {
+ ret = RCL_RET_BAD_ALLOC;
+ 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:
+ fini_ret = rcl_action_client_fini(action_client, node);
+ if (RCL_RET_OK != fini_ret) {
+ RCL_SET_ERROR_MSG("failed to cleanup action client");
+ ret = RCL_RET_ERROR;
+ }
+ return ret;
+}
+
+rcl_ret_t
+rcl_action_client_fini(rcl_action_client_t * action_client, rcl_node_t * node)
+{
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing action client");
+ if (!rcl_action_client_is_valid(action_client)) {
+ return RCL_RET_ACTION_CLIENT_INVALID; // error already set
+ }
+ if (!rcl_node_is_valid(node)) {
+ return RCL_RET_NODE_INVALID; // error already set
+ }
+ rcl_ret_t ret = RCL_RET_OK;
+ if (RCL_RET_OK != rcl_client_fini(&action_client->impl->goal_client, node)) {
+ ret = RCL_RET_ERROR;
+ }
+ if (RCL_RET_OK != rcl_client_fini(&action_client->impl->cancel_client, node)) {
+ ret = RCL_RET_ERROR;
+ }
+ if (RCL_RET_OK != rcl_client_fini(&action_client->impl->result_client, node)) {
+ ret = RCL_RET_ERROR;
+ }
+ if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->feedback_subscription, node)) {
+ 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 = &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");
+ return ret;
+}
+
+rcl_action_client_options_t
+rcl_action_client_get_default_options(void)
+{
+ static rcl_action_client_options_t default_options;
+ default_options.goal_service_qos = rmw_qos_profile_services_default;
+ default_options.cancel_service_qos = rmw_qos_profile_services_default;
+ default_options.result_service_qos = rmw_qos_profile_services_default;
+ default_options.feedback_topic_qos = rmw_qos_profile_default;
+ default_options.status_topic_qos = rcl_action_qos_profile_status_default;
+ default_options.allocator = rcl_get_default_allocator();
+ 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)
+{
+ SEND_SERVICE_REQUEST(goal)
+}
+
+rcl_ret_t
+rcl_action_take_goal_response(
+ const rcl_action_client_t * action_client,
+ void * ros_goal_response)
+{
+ TAKE_SERVICE_RESPONSE(goal);
+}
+
+rcl_ret_t
+rcl_action_send_result_request(
+ const rcl_action_client_t * action_client,
+ const void * ros_result_request)
+{
+ SEND_SERVICE_REQUEST(result);
+}
+
+rcl_ret_t
+rcl_action_take_result_response(
+ const rcl_action_client_t * action_client,
+ void * ros_result_response)
+{
+ TAKE_SERVICE_RESPONSE(result);
+}
+
+rcl_ret_t
+rcl_action_send_cancel_request(
+ const rcl_action_client_t * action_client,
+ const void * ros_cancel_request)
+{
+ SEND_SERVICE_REQUEST(cancel);
+}
+
+rcl_ret_t
+rcl_action_take_cancel_response(
+ const rcl_action_client_t * action_client,
+ void * ros_cancel_response)
+{
+ 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 *
+rcl_action_client_get_action_name(const rcl_action_client_t * action_client)
+{
+ if (!rcl_action_client_is_valid(action_client)) {
+ return NULL;
+ }
+ return action_client->impl->action_name;
+}
+
+const rcl_action_client_options_t *
+rcl_action_client_get_options(const rcl_action_client_t * action_client)
+{
+ if (!rcl_action_client_is_valid(action_client)) {
+ return NULL;
+ }
+ return &action_client->impl->options;
+}
+
+bool
+rcl_action_client_is_valid(const rcl_action_client_t * action_client)
+{
+ RCL_CHECK_FOR_NULL_WITH_MSG(
+ 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;
+}
+
+rcl_ret_t
+rcl_action_wait_set_add_action_client(
+ rcl_wait_set_t * wait_set,
+ const rcl_action_client_t * action_client)
+{
+ 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);
+ 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);
+ 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);
+ 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);
+ 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);
+ if (RCL_RET_OK != ret) {
+ return ret;
+ }
+ return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_action_client_wait_set_get_num_entities(
+ const rcl_action_client_t * action_client,
+ size_t * num_subscriptions,
+ size_t * num_guard_conditions,
+ size_t * num_timers,
+ size_t * num_clients,
+ size_t * num_services)
+{
+ if (!rcl_action_client_is_valid(action_client)) {
+ return RCL_RET_ACTION_CLIENT_INVALID; // error already set
+ }
+ RCL_CHECK_ARGUMENT_FOR_NULL(num_subscriptions, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(num_guard_conditions, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(num_timers, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(num_clients, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(num_services, RCL_RET_INVALID_ARGUMENT);
+ *num_subscriptions = 2;
+ *num_guard_conditions = 0;
+ *num_timers = 0;
+ *num_clients = 3;
+ *num_services = 0;
+ return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_action_client_wait_set_get_entities_ready(
+ const rcl_wait_set_t * wait_set,
+ const rcl_action_client_t * action_client,
+ bool * is_feedback_ready,
+ bool * is_status_ready,
+ bool * is_goal_response_ready,
+ bool * is_cancel_response_ready,
+ bool * is_result_response_ready)
+{
+ if (!rcl_action_client_is_valid(action_client)) {
+ return RCL_RET_ACTION_CLIENT_INVALID; // error already set
+ }
+ RCL_CHECK_ARGUMENT_FOR_NULL(is_feedback_ready, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(is_status_ready, RCL_RET_INVALID_ARGUMENT);
+ 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];
+ return RCL_RET_OK;
+}
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/rcl_action/test/rcl_action/test_action_client.cpp b/rcl_action/test/rcl_action/test_action_client.cpp
new file mode 100644
index 0000000..eb2508d
--- /dev/null
+++ b/rcl_action/test/rcl_action/test_action_client.cpp
@@ -0,0 +1,213 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+
+#include "rcl_action/action_client.h"
+
+#include "rcl/error_handling.h"
+#include "rcl/rcl.h"
+
+#include "test_msgs/action/fibonacci.h"
+
+class TestActionClientBaseFixture : public ::testing::Test
+{
+protected:
+ void SetUp() override
+ {
+ rcl_ret_t ret = rcl_init(0, nullptr, rcl_get_default_allocator());
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ this->node = rcl_get_zero_initialized_node();
+ rcl_node_options_t node_options = rcl_node_get_default_options();
+ const char * node_name = "test_action_client_node";
+ ret = rcl_node_init(&this->node, node_name, "", &node_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ void TearDown() override
+ {
+ rcl_ret_t ret = rcl_node_fini(&this->node);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ret = rcl_shutdown();
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ rcl_node_t node;
+};
+
+
+TEST_F(TestActionClientBaseFixture, test_action_client_init_fini) {
+ rcl_ret_t ret = RCL_RET_OK;
+ rcl_action_client_t invalid_action_client =
+ rcl_action_get_zero_initialized_client();
+ rcl_node_t invalid_node = rcl_get_zero_initialized_node();
+ const char * action_name = "test_action_client_name";
+ const rosidl_action_type_support_t * action_typesupport =
+ ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
+ const rcl_action_client_options_t action_client_options =
+ rcl_action_client_get_default_options();
+ rcl_action_client_options_t invalid_action_client_options =
+ rcl_action_client_get_default_options();
+ invalid_action_client_options.allocator =
+ (rcl_allocator_t)rcutils_get_zero_initialized_allocator();
+ rcl_action_client_t action_client = rcl_action_get_zero_initialized_client();
+
+ ret = rcl_action_client_init(
+ nullptr, &this->node, action_typesupport,
+ action_name, &action_client_options);
+ EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_init(
+ &action_client, nullptr, action_typesupport,
+ action_name, &action_client_options);
+ EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_init(
+ &action_client, &invalid_node, action_typesupport,
+ action_name, &action_client_options);
+ EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_init(
+ &action_client, &this->node, nullptr,
+ action_name, &action_client_options);
+ EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_init(
+ &action_client, &this->node, action_typesupport,
+ nullptr, &action_client_options);
+ EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_init(
+ &action_client, &this->node,
+ action_typesupport, action_name,
+ nullptr);
+ EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_init(
+ &action_client, &this->node,
+ action_typesupport, action_name,
+ &invalid_action_client_options);
+ EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_init(
+ &action_client, &this->node, action_typesupport,
+ action_name, &action_client_options);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_init(
+ &action_client, &this->node, action_typesupport,
+ action_name, &action_client_options);
+ EXPECT_EQ(ret, RCL_RET_ALREADY_INIT) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_fini(nullptr, &this->node);
+ EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_fini(&invalid_action_client, &this->node);
+ EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_fini(&action_client, nullptr);
+ EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_fini(&action_client, &invalid_node);
+ EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ ret = rcl_action_client_fini(&action_client, &this->node);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+ rcl_reset_error();
+}
+
+class TestActionClientFixture : public TestActionClientBaseFixture
+{
+protected:
+ void SetUp() override
+ {
+ TestActionClientBaseFixture::SetUp();
+ this->action_client = rcl_action_get_zero_initialized_client();
+ const rosidl_action_type_support_t * action_typesupport =
+ ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
+ this->action_client_options = rcl_action_client_get_default_options();
+ rcl_ret_t ret = rcl_action_client_init(
+ &this->action_client, &this->node, action_typesupport,
+ this->action_name, &this->action_client_options);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+ this->invalid_action_client = rcl_action_get_zero_initialized_client();
+ }
+
+ void TearDown() override
+ {
+ rcl_ret_t ret = rcl_action_client_fini(&this->action_client, &this->node);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+ TestActionClientBaseFixture::TearDown();
+ }
+
+ const char * const action_name = "test_action_client_name";
+ rcl_action_client_options_t action_client_options;
+ rcl_action_client_t invalid_action_client;
+ rcl_action_client_t action_client;
+};
+
+TEST_F(TestActionClientFixture, test_action_client_is_valid) {
+ bool is_valid = rcl_action_client_is_valid(nullptr);
+ EXPECT_FALSE(is_valid) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ is_valid = rcl_action_client_is_valid(&this->invalid_action_client);
+ EXPECT_FALSE(is_valid) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ is_valid = rcl_action_client_is_valid(&this->action_client);
+ EXPECT_TRUE(is_valid) << rcl_get_error_string().str;
+ rcl_reset_error();
+}
+
+TEST_F(TestActionClientFixture, test_action_client_get_action_name) {
+ const char * name = rcl_action_client_get_action_name(nullptr);
+ EXPECT_EQ(name, nullptr) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ name = rcl_action_client_get_action_name(&this->invalid_action_client);
+ EXPECT_EQ(name, nullptr) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ name = rcl_action_client_get_action_name(&this->action_client);
+ ASSERT_NE(name, nullptr) << rcl_get_error_string().str;
+ EXPECT_STREQ(name, this->action_name);
+}
+
+TEST_F(TestActionClientFixture, test_action_client_get_options) {
+ const rcl_action_client_options_t * options =
+ rcl_action_client_get_options(nullptr);
+ EXPECT_EQ(options, nullptr) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ options = rcl_action_client_get_options(&this->invalid_action_client);
+ EXPECT_EQ(options, nullptr) << rcl_get_error_string().str;
+ rcl_reset_error();
+
+ options = rcl_action_client_get_options(&this->action_client);
+ ASSERT_NE(options, nullptr) << rcl_get_error_string().str;
+}