diff --git a/rcl/include/rcl/macros.h b/rcl/include/rcl/macros.h
index 4df9ff427..973dec116 100644
--- a/rcl/include/rcl/macros.h
+++ b/rcl/include/rcl/macros.h
@@ -27,6 +27,37 @@ extern "C"
#define RCL_UNUSED(x) RCUTILS_UNUSED(x)
+#define RCL_RET_FROM_RCUTIL_RET(rcl_ret_var, rcutils_expr) \
+ { \
+ rcutils_ret_t rcutils_ret = rcutils_expr; \
+ if (RCUTILS_RET_OK != rcutils_ret) { \
+ if (rcutils_error_is_set()) { \
+ RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \
+ } else { \
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("rcutils_ret_t code: %i", rcutils_ret); \
+ } \
+ } \
+ switch (rcutils_ret) { \
+ case RCUTILS_RET_OK: \
+ rcl_ret_var = RCL_RET_OK; \
+ break; \
+ case RCUTILS_RET_ERROR: \
+ rcl_ret_var = RCL_RET_ERROR; \
+ break; \
+ case RCUTILS_RET_BAD_ALLOC: \
+ rcl_ret_var = RCL_RET_BAD_ALLOC; \
+ break; \
+ case RCUTILS_RET_INVALID_ARGUMENT: \
+ rcl_ret_var = RCL_RET_INVALID_ARGUMENT; \
+ break; \
+ case RCUTILS_RET_NOT_INITIALIZED: \
+ rcl_ret_var = RCL_RET_NOT_INIT; \
+ break; \
+ default: \
+ rcl_ret_var = RCUTILS_RET_ERROR; \
+ } \
+ }
+
#ifdef __cplusplus
}
#endif
diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c
index 4133a52f6..c1803f72a 100644
--- a/rcl/src/rcl/logging_rosout.c
+++ b/rcl/src/rcl/logging_rosout.c
@@ -15,6 +15,7 @@
#include "rcl/allocator.h"
#include "rcl/error_handling.h"
#include "rcl/logging_rosout.h"
+#include "rcl/macros.h"
#include "rcl/node.h"
#include "rcl/publisher.h"
#include "rcl/time.h"
@@ -43,37 +44,6 @@ extern "C"
return RCL_RET_OK; \
}
-#define RCL_RET_FROM_RCUTIL_RET(rcl_ret_var, rcutils_expr) \
- { \
- rcutils_ret_t rcutils_ret = rcutils_expr; \
- if (RCUTILS_RET_OK != rcutils_ret) { \
- if (rcutils_error_is_set()) { \
- RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \
- } else { \
- RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("rcutils_ret_t code: %i", rcutils_ret); \
- } \
- } \
- switch (rcutils_ret) { \
- case RCUTILS_RET_OK: \
- rcl_ret_var = RCL_RET_OK; \
- break; \
- case RCUTILS_RET_ERROR: \
- rcl_ret_var = RCL_RET_ERROR; \
- break; \
- case RCUTILS_RET_BAD_ALLOC: \
- rcl_ret_var = RCL_RET_BAD_ALLOC; \
- break; \
- case RCUTILS_RET_INVALID_ARGUMENT: \
- rcl_ret_var = RCL_RET_INVALID_ARGUMENT; \
- break; \
- case RCUTILS_RET_NOT_INITIALIZED: \
- rcl_ret_var = RCL_RET_NOT_INIT; \
- break; \
- default: \
- rcl_ret_var = RCUTILS_RET_ERROR; \
- } \
- }
-
typedef struct rosout_map_entry_t
{
rcl_node_t * node;
diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h
index 7fdf6df78..ce896d5d8 100644
--- a/rcl_action/include/rcl_action/action_client.h
+++ b/rcl_action/include/rcl_action/action_client.h
@@ -24,6 +24,7 @@ extern "C"
#include "rcl_action/visibility_control.h"
#include "rcl/macros.h"
#include "rcl/node.h"
+#include "rcl/subscription.h"
/// Internal action client implementation struct.
@@ -741,6 +742,64 @@ bool
rcl_action_client_is_valid(
const rcl_action_client_t * action_client);
+/// Add a goal uuid.
+/**
+ * This function is to add a goal uuid to the map of rcl_action_client_t
+ * and then try to set content filtered topic if it is supported.
+ *
+ * The caller must provide a lock to call this interface
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] action_client handle to the client that will take the goal response
+ * \param[in] uuid pointer to a uuid which length is 16
+ * \return `RCL_RET_OK` if success on setting a goal uuid, 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_UNSUPPORTED` if setting content filtered topic is not supported
+ * in the middleware, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_ACTION_PUBLIC
+rcl_ret_t rcl_action_add_goal_uuid(
+ const rcl_action_client_t * action_client,
+ const uint8_t * uuid);
+
+/// Remove a goal uuid.
+/**
+ * This function is to remove a goal uuid from the map of rcl_action_client_t
+ * and then try to reset content filtered topic if it is supported.
+ *
+ * The caller must provide a lock to call this interface
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] action_client handle to the client that will take the goal response
+ * \param[in] uuid pointer to a uuid which length is 16
+ * \return `RCL_RET_OK` if success on removing a goal uuid, 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_UNSUPPORTED` if setting content filtered topic is not supported
+ * in the middleware, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_ACTION_PUBLIC
+rcl_ret_t rcl_action_remove_goal_uuid(
+ const rcl_action_client_t * action_client,
+ const uint8_t * uuid);
+
#ifdef __cplusplus
}
#endif
diff --git a/rcl_action/include/rcl_action/types.h b/rcl_action/include/rcl_action/types.h
index c34bd42db..5a31aa6bf 100644
--- a/rcl_action/include/rcl_action/types.h
+++ b/rcl_action/include/rcl_action/types.h
@@ -58,6 +58,25 @@ extern "C"
#define uuidcmp(uuid0, uuid1) (0 == memcmp(uuid0, uuid1, UUID_SIZE))
#define zerouuid (uint8_t[UUID_SIZE]) {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define uuidcmpzero(uuid) uuidcmp(uuid, (zerouuid))
+#define UUID_FMT \
+ "%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X"
+#define UUID_FMT_ARGS(uuid) \
+ uuid[0], \
+ uuid[1], \
+ uuid[2], \
+ uuid[3], \
+ uuid[4], \
+ uuid[5], \
+ uuid[6], \
+ uuid[7], \
+ uuid[8], \
+ uuid[9], \
+ uuid[10], \
+ uuid[11], \
+ uuid[12], \
+ uuid[13], \
+ uuid[14], \
+ uuid[15]
// Typedef generated messages for convenience
typedef action_msgs__msg__GoalInfo rcl_action_goal_info_t;
diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c
index a03a61ec3..790da3f92 100644
--- a/rcl_action/src/rcl_action/action_client.c
+++ b/rcl_action/src/rcl_action/action_client.c
@@ -32,6 +32,7 @@ extern "C"
#include "rcl/types.h"
#include "rcl/wait.h"
+#include "rcutils/format_string.h"
#include "rcutils/logging_macros.h"
#include "rcutils/strdup.h"
@@ -63,7 +64,8 @@ _rcl_action_get_zero_initialized_client_impl(void)
0,
0,
0,
- 0
+ 0,
+ rcutils_get_zero_initialized_hash_map()
};
return null_action_client;
}
@@ -92,6 +94,29 @@ _rcl_action_client_fini_impl(
ret = RCL_RET_ERROR;
}
allocator.deallocate(action_client->impl->action_name, allocator.state);
+ if (NULL != action_client->impl->goal_uuids.impl) {
+ uint8_t uuid[UUID_SIZE];
+ char * value = NULL;
+ rcl_allocator_t default_allocator = rcl_get_default_allocator();
+ rcutils_ret_t hashmap_ret = rcutils_hash_map_get_next_key_and_data(
+ &action_client->impl->goal_uuids, NULL, uuid, &value);
+ while (RCUTILS_RET_OK == hashmap_ret) {
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "remove a uuid: %s", value);
+ default_allocator.deallocate(value, default_allocator.state);
+ RCL_RET_FROM_RCUTIL_RET(
+ ret, rcutils_hash_map_unset(&action_client->impl->goal_uuids, uuid));
+ if (ret == RCL_RET_OK) {
+ hashmap_ret = rcutils_hash_map_get_next_key_and_data(
+ &action_client->impl->goal_uuids, NULL, uuid, &value);
+ }
+ }
+ if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != hashmap_ret) {
+ RCL_RET_FROM_RCUTIL_RET(ret, hashmap_ret);
+ }
+ if (RCL_RET_OK == ret) {
+ RCL_RET_FROM_RCUTIL_RET(ret, rcutils_hash_map_fini(&action_client->impl->goal_uuids));
+ }
+ }
allocator.deallocate(action_client->impl, allocator.state);
action_client->impl = NULL;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client finalized");
@@ -171,6 +196,26 @@ _rcl_action_client_fini_impl(
goto fail; \
}
+size_t hash_map_uuid_hash_func(const void * uuid)
+{
+ const uint8_t * ckey_str = (const uint8_t *) uuid;
+ size_t hash = 5381;
+
+ for (size_t i = 0; i < UUID_SIZE; ++i) {
+ const char c = *(ckey_str++);
+ hash = ((hash << 5) + hash) + (size_t)c; /* hash * 33 + c */
+ }
+
+ return hash;
+}
+
+int hash_map_uuid_cmp_func(const void * val1, const void * val2)
+{
+ const uint8_t * cval1 = (const uint8_t *)val1;
+ const uint8_t * cval2 = (const uint8_t *)val2;
+ return memcmp(cval1, cval2, UUID_SIZE);
+}
+
rcl_ret_t
rcl_action_client_init(
rcl_action_client_t * action_client,
@@ -222,6 +267,15 @@ rcl_action_client_init(
SUBSCRIPTION_INIT(feedback);
SUBSCRIPTION_INIT(status);
+ // Initialize goal_uuids map
+ RCL_RET_FROM_RCUTIL_RET(
+ ret, rcutils_hash_map_init(
+ &action_client->impl->goal_uuids, 2, sizeof(uint8_t[16]), sizeof(const char **),
+ hash_map_uuid_hash_func, hash_map_uuid_cmp_func, &allocator));
+ if (RCL_RET_OK != ret) {
+ goto fail;
+ }
+
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized");
return ret;
fail:
@@ -649,6 +703,220 @@ rcl_action_client_wait_set_get_entities_ready(
return RCL_RET_OK;
}
+static char *
+to_uuid_string(const uint8_t * uuid, rcl_allocator_t allocator)
+{
+ char * uuid_str = rcutils_format_string(allocator, UUID_FMT, UUID_FMT_ARGS(uuid));
+ return uuid_str;
+}
+
+static
+rcl_ret_t set_content_filtered_topic(
+ const rcl_action_client_t * action_client)
+{
+ rcl_ret_t ret;
+ rcutils_ret_t rcutils_ret;
+ uint8_t uuid[UUID_SIZE];
+ char * uuid_str = NULL;
+ size_t size;
+ char * feedback_filter = NULL;
+ char * feedback_filter_update = NULL;
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ rcl_subscription_content_filter_options_t options =
+ rcl_subscription_get_default_content_filter_options();
+
+ // content filter with empty string to reset
+ feedback_filter = rcutils_strdup("", allocator);
+ if (NULL == feedback_filter) {
+ RCL_SET_ERROR_MSG("failed to allocate memory for feedback filter string");
+ ret = RCL_RET_BAD_ALLOC;
+ goto clean;
+ }
+
+ RCL_RET_FROM_RCUTIL_RET(
+ ret, rcutils_hash_map_get_size(&action_client->impl->goal_uuids, &size));
+ if (RCL_RET_OK != ret) {
+ goto clean;
+ }
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "size: %zu", size);
+ if (0 == size) {
+ goto set_cft;
+ }
+
+ RCL_RET_FROM_RCUTIL_RET(
+ ret, rcutils_hash_map_get_next_key_and_data(
+ &action_client->impl->goal_uuids, NULL, uuid, &uuid_str));
+ if (RCL_RET_OK != ret) {
+ goto clean;
+ }
+
+ feedback_filter_update =
+ rcutils_format_string(allocator, "goal_id.uuid = &hex(%s)", uuid_str);
+ if (NULL == feedback_filter_update) {
+ RCL_SET_ERROR_MSG("failed to format string for feedback filter");
+ ret = RCL_RET_BAD_ALLOC;
+ goto clean;
+ }
+ allocator.deallocate(feedback_filter, allocator.state);
+ feedback_filter = feedback_filter_update;
+
+ while (RCUTILS_RET_OK == (rcutils_ret = rcutils_hash_map_get_next_key_and_data(
+ &action_client->impl->goal_uuids, uuid, uuid, &uuid_str)))
+ {
+ feedback_filter_update = rcutils_format_string(
+ allocator, "%s or goal_id.uuid = &hex(%s)", feedback_filter, uuid_str);
+ if (NULL == feedback_filter_update) {
+ RCL_SET_ERROR_MSG("failed to format string for feedback filter");
+ ret = RCL_RET_BAD_ALLOC;
+ goto clean;
+ }
+ allocator.deallocate(feedback_filter, allocator.state);
+ feedback_filter = feedback_filter_update;
+ }
+ if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != rcutils_ret) {
+ RCL_RET_FROM_RCUTIL_RET(ret, rcutils_ret);
+ }
+ if (RCL_RET_OK != ret) {
+ goto clean;
+ }
+
+set_cft:
+
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "feedback_filter: %s", feedback_filter);
+
+ ret = rcl_subscription_content_filter_options_init(feedback_filter, 0, NULL, &options);
+ if (RCL_RET_OK != ret) {
+ goto clean;
+ }
+
+ ret = rcl_subscription_set_content_filter(
+ &action_client->impl->feedback_subscription, &options);
+ if (RCL_RET_OK != ret) {
+ goto clean;
+ }
+
+clean:
+ {
+ rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options);
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG("failed to deallocate memory for options");
+ }
+ }
+
+ allocator.deallocate(feedback_filter, allocator.state);
+ return ret;
+}
+
+rcl_ret_t rcl_action_add_goal_uuid(
+ const rcl_action_client_t * action_client,
+ const uint8_t * uuid)
+{
+ if (!rcl_action_client_is_valid(action_client)) {
+ return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */
+ }
+ RCL_CHECK_ARGUMENT_FOR_NULL(uuid, RCL_RET_INVALID_ARGUMENT);
+
+ rcl_ret_t ret;
+ char * uuid_str = NULL;
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ uuid_str = to_uuid_string(uuid, allocator);
+ if (NULL == uuid_str) {
+ RCL_SET_ERROR_MSG("failed to allocate memory for uuid value");
+ ret = RCL_RET_BAD_ALLOC;
+ goto end;
+ }
+
+ RCL_RET_FROM_RCUTIL_RET(
+ ret, rcutils_hash_map_set(&action_client->impl->goal_uuids, uuid, &uuid_str));
+ if (RCL_RET_OK != ret) {
+ goto clean;
+ }
+
+ // to set content filtered topic
+ RCUTILS_LOG_DEBUG_NAMED(
+ ROS_PACKAGE_NAME, "set content filtered topic after adding a uuid: %s", uuid_str);
+ ret = set_content_filtered_topic(action_client);
+ if (RCL_RET_OK != ret) {
+ char * err = rcutils_strdup(rcl_get_error_string().str, allocator);
+ if (NULL == err) {
+ RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "failed to allocate memory for error");
+ ret = RCL_RET_BAD_ALLOC;
+ goto clean;
+ }
+ rcl_reset_error();
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("failed to set_content_filtered_topic: %s", err);
+ allocator.deallocate(err, allocator.state);
+ }
+ goto end;
+
+clean:
+ allocator.deallocate(uuid_str, allocator.state);
+
+end:
+ return ret;
+}
+
+rcl_ret_t rcl_action_remove_goal_uuid(
+ const rcl_action_client_t * action_client,
+ const uint8_t * uuid)
+{
+ if (!rcl_action_client_is_valid(action_client)) {
+ return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */
+ }
+ RCL_CHECK_ARGUMENT_FOR_NULL(uuid, RCL_RET_INVALID_ARGUMENT);
+
+ rcl_ret_t ret;
+ char * uuid_str = NULL;
+ char * uuid_value = NULL;
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ uuid_str = to_uuid_string(uuid, allocator);
+ if (NULL == uuid_str) {
+ RCL_SET_ERROR_MSG("failed to allocate memory for uuid value");
+ ret = RCL_RET_BAD_ALLOC;
+ goto end;
+ }
+ if (!rcutils_hash_map_key_exists(&action_client->impl->goal_uuids, uuid)) {
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+ "item key [%s] not found in the map of goal uuids",
+ uuid_str);
+ ret = RCL_RET_ERROR;
+ goto end;
+ }
+
+ RCL_RET_FROM_RCUTIL_RET(
+ ret, rcutils_hash_map_get(&action_client->impl->goal_uuids, uuid, &uuid_value));
+ if (RCL_RET_OK != ret) {
+ goto end;
+ }
+ allocator.deallocate(uuid_value, allocator.state);
+
+ RCL_RET_FROM_RCUTIL_RET(
+ ret, rcutils_hash_map_unset(&action_client->impl->goal_uuids, uuid));
+ if (RCL_RET_OK != ret) {
+ goto end;
+ }
+
+ RCUTILS_LOG_DEBUG_NAMED(
+ ROS_PACKAGE_NAME, "set content filtered topic after removing a uuid: %s", uuid_str);
+ ret = set_content_filtered_topic(action_client);
+ if (RCL_RET_OK != ret) {
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ char * err = rcutils_strdup(rcl_get_error_string().str, allocator);
+ if (NULL == err) {
+ RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "failed to allocate memory for error");
+ ret = RCL_RET_BAD_ALLOC;
+ goto end;
+ }
+ rcl_reset_error();
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("failed to set_content_filtered_topic: %s", err);
+ allocator.deallocate(err, allocator.state);
+ }
+
+end:
+ allocator.deallocate(uuid_str, allocator.state);
+ return ret;
+}
+
#ifdef __cplusplus
}
#endif
diff --git a/rcl_action/src/rcl_action/action_client_impl.h b/rcl_action/src/rcl_action/action_client_impl.h
index 777416bfd..c16fa59e9 100644
--- a/rcl_action/src/rcl_action/action_client_impl.h
+++ b/rcl_action/src/rcl_action/action_client_impl.h
@@ -18,6 +18,8 @@
#include "rcl_action/types.h"
#include "rcl/rcl.h"
+#include "rcutils/types.h"
+
typedef struct rcl_action_client_impl_s
{
rcl_client_t goal_client;
@@ -33,6 +35,7 @@ typedef struct rcl_action_client_impl_s
size_t wait_set_result_client_index;
size_t wait_set_feedback_subscription_index;
size_t wait_set_status_subscription_index;
+ rcutils_hash_map_t goal_uuids;
} rcl_action_client_impl_t;
diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp
index c13685ebc..1e485b02b 100644
--- a/rcl_action/test/rcl_action/test_action_communication.cpp
+++ b/rcl_action/test/rcl_action/test_action_communication.cpp
@@ -14,6 +14,7 @@
#include
#include
+#include
#include
#include "osrf_testing_tools_cpp/scope_exit.hpp"
@@ -727,6 +728,106 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal
test_msgs__action__Fibonacci_SendGoal_Request__fini(&incoming_goal_request);
}
+TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedback_comm_cft)
+{
+ #define FEEDBACK_SIZE 2
+ test_msgs__action__Fibonacci_FeedbackMessage outgoing_feedback[FEEDBACK_SIZE];
+ test_msgs__action__Fibonacci_FeedbackMessage incoming_feedback[FEEDBACK_SIZE];
+ for (size_t i = 0; i < FEEDBACK_SIZE; ++i) {
+ test_msgs__action__Fibonacci_FeedbackMessage__init(&outgoing_feedback[i]);
+ test_msgs__action__Fibonacci_FeedbackMessage__init(&incoming_feedback[i]);
+ }
+
+ uint8_t uuid[UUID_SIZE];
+ init_test_uuid0(uuid);
+ rcl_ret_t ret = rcl_action_add_goal_uuid(&this->action_client, uuid);
+ bool if_cft_supported = false;
+ if (ret != RMW_RET_OK) {
+ ASSERT_EQ(ret, RCL_RET_UNSUPPORTED) << rcl_get_error_string().str;
+ } else {
+ if_cft_supported = true;
+ }
+
+ // Initialize feedback
+ // set uuid of feedback with uuid0 if index is 0, otherwise the uuid of feedback is uuid1
+ for (size_t i = 0; i < FEEDBACK_SIZE; ++i) {
+ ASSERT_TRUE(
+ rosidl_runtime_c__int32__Sequence__init(
+ &outgoing_feedback[i].feedback.sequence, 3));
+ outgoing_feedback[i].feedback.sequence.data[0] = 0;
+ outgoing_feedback[i].feedback.sequence.data[1] = 1;
+ outgoing_feedback[i].feedback.sequence.data[2] = 2;
+ i == 0 ? init_test_uuid0(outgoing_feedback[i].goal_id.uuid) :
+ init_test_uuid1(outgoing_feedback[i].goal_id.uuid);
+ }
+
+ // Publish feedback with valid arguments
+ for (size_t i = 0; i < FEEDBACK_SIZE; ++i) {
+ ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback[i]);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_wait_set_clear(&this->wait_set);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_wait_set_add_action_client(
+ &this->wait_set, &this->action_client, NULL, NULL);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
+ // if content filter is unsupported, the action client will receive different action feedback
+ // message, otherwise it will only receive the matched uuid0 feedback message.
+ if (!if_cft_supported || 0 == i) {
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+ ret = rcl_action_client_wait_set_get_entities_ready(
+ &this->wait_set,
+ &this->action_client,
+ &this->is_feedback_ready,
+ &this->is_status_ready,
+ &this->is_goal_response_ready,
+ &this->is_cancel_response_ready,
+ &this->is_result_response_ready);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ EXPECT_TRUE(this->is_feedback_ready);
+ EXPECT_FALSE(this->is_status_ready);
+ EXPECT_FALSE(this->is_result_response_ready);
+ EXPECT_FALSE(this->is_cancel_response_ready);
+ EXPECT_FALSE(this->is_goal_response_ready);
+
+ // Take feedback with valid arguments
+ ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback[i]);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ // Check that feedback was received correctly
+ EXPECT_TRUE(
+ uuidcmp(
+ outgoing_feedback[i].goal_id.uuid,
+ incoming_feedback[i].goal_id.uuid));
+ ASSERT_EQ(
+ outgoing_feedback[i].feedback.sequence.size, incoming_feedback[i].feedback.sequence.size);
+ EXPECT_TRUE(
+ !memcmp(
+ outgoing_feedback[i].feedback.sequence.data,
+ incoming_feedback[i].feedback.sequence.data,
+ outgoing_feedback[i].feedback.sequence.size));
+ } else {
+ EXPECT_EQ(ret, RCL_RET_TIMEOUT) << rcl_get_error_string().str;
+ }
+ }
+
+ ret = rcl_action_remove_goal_uuid(&this->action_client, uuid);
+ if (if_cft_supported) {
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+ } else {
+ EXPECT_EQ(ret, RCL_RET_UNSUPPORTED) << rcl_get_error_string().str;
+ }
+
+ for (size_t i = 0; i < FEEDBACK_SIZE; ++i) {
+ test_msgs__action__Fibonacci_FeedbackMessage__fini(&outgoing_feedback[i]);
+ test_msgs__action__Fibonacci_FeedbackMessage__fini(&incoming_feedback[i]);
+ }
+}
+
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_response_opts)
{
test_msgs__action__Fibonacci_SendGoal_Response outgoing_goal_response;