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/include/rcl/subscription.h b/rcl/include/rcl/subscription.h
index 09d6e6bc4..732624453 100644
--- a/rcl/include/rcl/subscription.h
+++ b/rcl/include/rcl/subscription.h
@@ -52,6 +52,15 @@ typedef struct rcl_subscription_options_s
rmw_subscription_options_t rmw_subscription_options;
} rcl_subscription_options_t;
+typedef struct rcl_subscription_content_filter_options_s
+{
+ /// Custom allocator for the options, used for incidental allocations.
+ /** For default behavior (malloc/free), see: rcl_get_default_allocator() */
+ rcl_allocator_t allocator;
+ /// rmw specific subscription content filter options
+ rmw_subscription_content_filter_options_t * rmw_subscription_content_filter_options;
+} rcl_subscription_content_filter_options_t;
+
/// Return a rcl_subscription_t struct with members set to `NULL`.
/**
* Should be called to get a null rcl_subscription_t before passing to
@@ -208,6 +217,184 @@ RCL_WARN_UNUSED
rcl_subscription_options_t
rcl_subscription_get_default_options(void);
+/// Reclaim resources held inside rcl_subscription_options_t structure.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] option The structure which its resources have to be deallocated.
+ * \return `RCL_RET_OK` if the memory was successfully freed, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if option is NULL, or
+ * \return `RCL_RET_BAD_ALLOC` if deallocating memory fails.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_subscription_options_fini(rcl_subscription_options_t * option);
+
+/// Set the content filter options for the given subscription options.
+/**
+ * \param[in] filter_expression The filter expression.
+ * \param[in] expression_parameters_argc The expression parameters argc.
+ * \param[in] expression_parameter_argv The expression parameters argv.
+ * \param[out] options The subscription options to be set.
+ * \return `RCL_RET_OK` if set options successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if arguments invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory fails.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_subscription_options_set_content_filter_options(
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_options_t * options);
+
+/// Return the default subscription content filter options.
+/**
+ * The defaults are:
+ *
+ * - allocator = rcl_get_default_allocator()
+ * - rmw_subscription_content_filter_options = NULL;
+ *
+ * \return A structure containing the default options for a subscription.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_subscription_content_filter_options_t
+rcl_subscription_get_default_content_filter_options(void);
+
+/// Allocate.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] option The structure which its resources have to be deallocated.
+ * \return `RCL_RET_OK` if the memory was successfully freed, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if option is NULL, or
+ * if its allocator is invalid and the structure contains initialized memory.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_subscription_content_filter_options_init(
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_content_filter_options_t * options);
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_subscription_content_filter_options_set(
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_content_filter_options_t * options);
+
+/// Reclaim rcl_subscription_content_filter_options_t structure.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] options The structure which its resources have to be deallocated.
+ * \return `RCL_RET_OK` if the memory was successfully freed, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if option is NULL, or
+ * if its allocator is invalid and the structure contains initialized memory.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_subscription_content_filter_options_fini(
+ rcl_subscription_content_filter_options_t * options);
+
+/// Check if the content filtered topic feature is enabled in the subscription.
+/**
+ * Depending on the middleware and whether cft is enabled in the subscription.
+ *
+ * \return `true` if the content filtered topic of `subscription` is enabled, otherwise `false`
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+bool
+rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription);
+
+/// Set the filter expression and expression parameters for the subscription.
+/**
+ * This function will set a filter expression and an array of expression parameters
+ * for the given subscription.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | No
+ * Uses Atomics | Maybe [1]
+ * Lock-Free | Maybe [1]
+ *
+ * \param[in] subscription The subscription to set content filter options.
+ * \param[in] options The rcl content filter options.
+ * \return `RCL_RET_OK` if the query was successful, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if `rcl_content_filter_options` is NULL, or
+ * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_subscription_set_content_filter(
+ const rcl_subscription_t * subscription,
+ const rcl_subscription_content_filter_options_t * options
+);
+
+/// Retrieve the filter expression of the subscription.
+/**
+ * This function will return an filter expression by the given subscription.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | Maybe [1]
+ * Lock-Free | Maybe [1]
+ *
+ * \param[in] subscription The subscription object to inspect.
+ * \param[out] options The rcl content filter options.
+ * It is up to the caller to finalize this options later on, using
+ * rcl_subscription_content_filter_options_fini().
+ * \return `RCL_RET_OK` if the query was successful, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if `options` is NULL, or
+ * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or
+ * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_subscription_get_content_filter(
+ const rcl_subscription_t * subscription,
+ rcl_subscription_content_filter_options_t * options
+);
+
/// Take a ROS message from a topic using a rcl subscription.
/**
* It is the job of the caller to ensure that the type of the ros_message
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/src/rcl/subscription.c b/rcl/src/rcl/subscription.c
index fcea1e767..2580289b4 100644
--- a/rcl/src/rcl/subscription.c
+++ b/rcl/src/rcl/subscription.c
@@ -24,7 +24,10 @@ extern "C"
#include "rcl/error_handling.h"
#include "rcl/node.h"
#include "rcutils/logging_macros.h"
+#include "rcutils/strdup.h"
+#include "rcutils/types/string_array.h"
#include "rmw/error_handling.h"
+#include "rmw/subscription_content_filter_options.h"
#include "rmw/validate_full_topic_name.h"
#include "tracetools/tracetools.h"
@@ -88,8 +91,8 @@ rcl_subscription_init(
ROS_PACKAGE_NAME, "Expanded and remapped topic name '%s'", remapped_topic_name);
// Allocate memory for the implementation struct.
- subscription->impl = (rcl_subscription_impl_t *)allocator->allocate(
- sizeof(rcl_subscription_impl_t), allocator->state);
+ subscription->impl = (rcl_subscription_impl_t *)allocator->zero_allocate(
+ 1, sizeof(rcl_subscription_impl_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
subscription->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup);
// Fill out the implemenation struct.
@@ -117,6 +120,7 @@ rcl_subscription_init(
options->qos.avoid_ros_namespace_conventions;
// options
subscription->impl->options = *options;
+
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized");
ret = RCL_RET_OK;
TRACEPOINT(
@@ -138,6 +142,12 @@ rcl_subscription_init(
}
}
+ ret = rcl_subscription_options_fini(&subscription->impl->options);
+ if (RCL_RET_OK != ret) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str);
+ RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+ }
+
allocator->deallocate(subscription->impl, allocator->state);
subscription->impl = NULL;
}
@@ -174,6 +184,13 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node)
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
result = RCL_RET_ERROR;
}
+ rcl_ret_t rcl_ret = rcl_subscription_options_fini(&subscription->impl->options);
+ if (RCL_RET_OK != rcl_ret) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str);
+ RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+ result = RCL_RET_ERROR;
+ }
+
allocator.deallocate(subscription->impl, allocator.state);
subscription->impl = NULL;
}
@@ -193,6 +210,277 @@ rcl_subscription_get_default_options()
return default_options;
}
+rcl_ret_t
+rcl_subscription_options_fini(rcl_subscription_options_t * option)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(option, RCL_RET_INVALID_ARGUMENT);
+ // fini rmw_subscription_options_t
+ const rcl_allocator_t * allocator = &option->allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+ if (option->rmw_subscription_options.content_filter_options) {
+ rmw_ret_t ret = rmw_subscription_content_filter_options_fini(
+ option->rmw_subscription_options.content_filter_options, allocator);
+ if (RCUTILS_RET_OK != ret) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to fini content filter options.\n");
+ return rcl_convert_rmw_ret_to_rcl_ret(ret);
+ }
+ allocator->deallocate(
+ option->rmw_subscription_options.content_filter_options, allocator->state);
+ option->rmw_subscription_options.content_filter_options = NULL;
+ }
+ return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_subscription_options_set_content_filter_options(
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_options_t * options)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+ const rcl_allocator_t * allocator = &options->allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+ rcl_ret_t ret;
+ rmw_subscription_content_filter_options_t * content_filter_options =
+ allocator->allocate(
+ sizeof(rmw_subscription_content_filter_options_t), allocator->state);
+ if (!content_filter_options) {
+ RCL_SET_ERROR_MSG("allocating memory failed");
+ return RCL_RET_BAD_ALLOC;
+ }
+
+ *content_filter_options = rmw_get_zero_initialized_content_filter_options();
+
+ rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_set(
+ filter_expression,
+ expression_parameters_argc,
+ expression_parameter_argv,
+ allocator,
+ content_filter_options
+ );
+
+ if (rmw_ret != RMW_RET_OK) {
+ ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ goto failed;
+ }
+
+ if (options->rmw_subscription_options.content_filter_options) {
+ rmw_ret = rmw_subscription_content_filter_options_fini(
+ options->rmw_subscription_options.content_filter_options,
+ allocator
+ );
+
+ if (rmw_ret != RMW_RET_OK) {
+ return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ }
+
+ allocator->deallocate(
+ options->rmw_subscription_options.content_filter_options, allocator->state);
+ options->rmw_subscription_options.content_filter_options = NULL;
+ }
+ options->rmw_subscription_options.content_filter_options = content_filter_options;
+
+ return RCL_RET_OK;
+
+failed:
+ allocator->deallocate(content_filter_options, allocator->state);
+ return ret;
+}
+
+rcl_subscription_content_filter_options_t
+rcl_subscription_get_default_content_filter_options()
+{
+ static rcl_subscription_content_filter_options_t default_options;
+ default_options.allocator = rcl_get_default_allocator();
+ default_options.rmw_subscription_content_filter_options = NULL;
+ return default_options;
+}
+
+rcl_ret_t
+rcl_subscription_content_filter_options_init(
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_content_filter_options_t * options)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+ const rcl_allocator_t * allocator = &options->allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+ rcl_ret_t ret;
+ rmw_subscription_content_filter_options_t * content_filter_options =
+ allocator->allocate(
+ sizeof(rmw_subscription_content_filter_options_t), allocator->state);
+ if (!content_filter_options) {
+ RCL_SET_ERROR_MSG("allocating memory failed");
+ return RCL_RET_BAD_ALLOC;
+ }
+ *content_filter_options = rmw_get_zero_initialized_content_filter_options();
+
+ rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_init(
+ filter_expression,
+ expression_parameters_argc,
+ expression_parameter_argv,
+ allocator,
+ content_filter_options
+ );
+ if (rmw_ret != RMW_RET_OK) {
+ ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ goto failed;
+ }
+ options->rmw_subscription_content_filter_options = content_filter_options;
+
+ return RCL_RET_OK;
+
+failed:
+ allocator->deallocate(content_filter_options, allocator->state);
+ return ret;
+}
+
+rcl_ret_t
+rcl_subscription_content_filter_options_set(
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_content_filter_options_t * options)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+ const rcl_allocator_t * allocator = &options->allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+ rmw_ret_t ret = rmw_subscription_content_filter_options_set(
+ filter_expression,
+ expression_parameters_argc,
+ expression_parameter_argv,
+ allocator,
+ options->rmw_subscription_content_filter_options
+ );
+ return rcl_convert_rmw_ret_to_rcl_ret(ret);
+}
+
+rcl_ret_t
+rcl_subscription_content_filter_options_fini(
+ rcl_subscription_content_filter_options_t * options)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+ const rcl_allocator_t * allocator = &options->allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+ rmw_ret_t ret = rmw_subscription_content_filter_options_fini(
+ options->rmw_subscription_content_filter_options,
+ allocator
+ );
+
+ if (ret != RMW_RET_OK) {
+ return rcl_convert_rmw_ret_to_rcl_ret(ret);
+ }
+
+ allocator->deallocate(options->rmw_subscription_content_filter_options, allocator->state);
+ return RCL_RET_OK;
+}
+
+bool
+rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription)
+{
+ if (!rcl_subscription_is_valid(subscription)) {
+ return false;
+ }
+ return subscription->impl->rmw_handle->is_cft_enabled;
+}
+
+rcl_ret_t
+rcl_subscription_set_content_filter(
+ const rcl_subscription_t * subscription,
+ const rcl_subscription_content_filter_options_t * options
+)
+{
+ RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID);
+ RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
+
+ if (!rcl_subscription_is_valid(subscription)) {
+ return RCL_RET_SUBSCRIPTION_INVALID;
+ }
+
+ rcl_allocator_t * allocator = (rcl_allocator_t *)&subscription->impl->options.allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+ RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+ rmw_ret_t ret = rmw_subscription_set_content_filter(
+ subscription->impl->rmw_handle,
+ options->rmw_subscription_content_filter_options);
+
+ if (ret != RMW_RET_OK) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+ return rcl_convert_rmw_ret_to_rcl_ret(ret);
+ }
+
+ // copy options into subscription_options
+ rmw_subscription_content_filter_options_t * content_filter_options =
+ options->rmw_subscription_content_filter_options;
+ size_t expression_parameters_size = 0;
+ const char ** expression_parameters = NULL;
+ if (content_filter_options->expression_parameters) {
+ expression_parameters_size = content_filter_options->expression_parameters->size;
+ expression_parameters =
+ (const char **)content_filter_options->expression_parameters->data;
+ }
+ return rcl_subscription_options_set_content_filter_options(
+ content_filter_options->filter_expression,
+ expression_parameters_size,
+ expression_parameters,
+ &subscription->impl->options
+ );
+}
+
+rcl_ret_t
+rcl_subscription_get_content_filter(
+ const rcl_subscription_t * subscription,
+ rcl_subscription_content_filter_options_t * options
+)
+{
+ RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID);
+ RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
+
+ if (!rcl_subscription_is_valid(subscription)) {
+ return RCL_RET_SUBSCRIPTION_INVALID;
+ }
+ RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+ const rcl_allocator_t * allocator = &options->allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+ rcl_ret_t ret;
+ rmw_subscription_content_filter_options_t * content_filter_options =
+ allocator->allocate(
+ sizeof(rmw_subscription_content_filter_options_t), allocator->state);
+ if (!content_filter_options) {
+ RCL_SET_ERROR_MSG("allocating memory failed");
+ return RCL_RET_BAD_ALLOC;
+ }
+ *content_filter_options = rmw_get_zero_initialized_content_filter_options();
+
+ rmw_ret_t rmw_ret = rmw_subscription_get_content_filter(
+ subscription->impl->rmw_handle,
+ &options->allocator, content_filter_options);
+
+ if (rmw_ret != RMW_RET_OK) {
+ ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ goto failed;
+ }
+
+ options->rmw_subscription_content_filter_options = content_filter_options;
+
+ return RCL_RET_OK;
+
+failed:
+ allocator->deallocate(content_filter_options, allocator->state);
+ return ret;
+}
+
rcl_ret_t
rcl_take(
const rcl_subscription_t * subscription,
diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt
index 6ce895f04..d65f72691 100644
--- a/rcl/test/CMakeLists.txt
+++ b/rcl/test/CMakeLists.txt
@@ -433,3 +433,9 @@ rcl_add_custom_gtest(test_log_level
LIBRARIES ${PROJECT_NAME} mimick
AMENT_DEPENDENCIES "osrf_testing_tools_cpp"
)
+
+rcl_add_custom_gtest(test_subscription_content_filter_options
+ SRCS rcl/test_subscription_content_filter_options.cpp
+ APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+ LIBRARIES ${PROJECT_NAME}
+)
diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp
index adedd7bb9..4cc278c8c 100644
--- a/rcl/test/rcl/test_subscription.cpp
+++ b/rcl/test/rcl/test_subscription.cpp
@@ -23,6 +23,7 @@
#include "rmw/rmw.h"
#include "rmw/validate_full_topic_name.h"
+#include "rcutils/strdup.h"
#include "rcutils/testing/fault_injection.h"
#include "test_msgs/msg/basic_types.h"
#include "test_msgs/msg/strings.h"
@@ -847,6 +848,428 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_bad_return_l
rcl_subscription_fini(&subscription, this->node_ptr)) << rcl_get_error_string().str;
}
+
+/* A subscription with a content filtered topic setting.
+ */
+TEST_F(
+ CLASSNAME(
+ TestSubscriptionFixture,
+ RMW_IMPLEMENTATION), test_subscription_content_filtered) {
+ const char * filter_expression1 = "string_value MATCH 'FilteredData'";
+ rcl_ret_t ret;
+ rcl_publisher_t publisher = rcl_get_zero_initialized_publisher();
+ const rosidl_message_type_support_t * ts =
+ ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings);
+ constexpr char topic[] = "rcl_test_subscription_content_filtered_chatter";
+ rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+ ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ });
+ rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_options_set_content_filter_options(
+ filter_expression1, 0, nullptr, &subscription_options)
+ );
+
+ ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ });
+ bool is_cft_support = rcl_subscription_is_cft_enabled(&subscription);
+ ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100));
+
+ // publish with a non-filtered data
+ constexpr char test_string[] = "NotFilteredData";
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string));
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__Strings__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ if (is_cft_support) {
+ ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+ } else {
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__Strings__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(
+ std::string(test_string),
+ std::string(msg.string_value.data, msg.string_value.size));
+ }
+
+ constexpr char test_filtered_string[] = "FilteredData";
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string));
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__Strings__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__Strings__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(
+ std::string(test_filtered_string),
+ std::string(msg.string_value.data, msg.string_value.size));
+ }
+
+ // set filter
+ const char * filter_expression2 = "string_value MATCH %0";
+ const char * expression_parameters2[] = {"'FilteredOtherData'"};
+ size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *);
+ {
+ rcl_subscription_content_filter_options_t options =
+ rcl_subscription_get_default_content_filter_options();
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ filter_expression2, expression_parameters2_count, expression_parameters2,
+ &options)
+ );
+
+ ret = rcl_subscription_set_content_filter(
+ &subscription, &options);
+ if (is_cft_support) {
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ } else {
+ ASSERT_EQ(RCL_RET_UNSUPPORTED, ret);
+ }
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ &options)
+ );
+ }
+
+ // publish FilteredData again
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string));
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__Strings__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ if (is_cft_support) {
+ ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+ } else {
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__Strings__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(
+ std::string(test_filtered_string),
+ std::string(msg.string_value.data, msg.string_value.size));
+ }
+
+ constexpr char test_filtered_other_string[] = "FilteredOtherData";
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_other_string));
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__Strings__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__Strings__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(
+ std::string(test_filtered_other_string),
+ std::string(msg.string_value.data, msg.string_value.size));
+ }
+
+ // get filter
+ {
+ rcl_subscription_content_filter_options_t content_filter_options =
+ rcl_subscription_get_default_content_filter_options();
+
+ ret = rcl_subscription_get_content_filter(
+ &subscription, &content_filter_options);
+ if (is_cft_support) {
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+ rmw_subscription_content_filter_options_t * options =
+ content_filter_options.rmw_subscription_content_filter_options;
+ ASSERT_NE(nullptr, options);
+ ASSERT_STREQ(filter_expression2, options->filter_expression);
+ ASSERT_NE(nullptr, options->expression_parameters);
+ ASSERT_EQ(expression_parameters2_count, options->expression_parameters->size);
+ for (size_t i = 0; i < expression_parameters2_count; ++i) {
+ EXPECT_STREQ(
+ options->expression_parameters->data[i],
+ expression_parameters2[i]);
+ }
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ &content_filter_options)
+ );
+ } else {
+ ASSERT_EQ(RCL_RET_UNSUPPORTED, ret);
+ }
+ }
+
+ // reset filter
+ {
+ rcl_subscription_content_filter_options_t options =
+ rcl_subscription_get_default_content_filter_options();
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ "", 0, nullptr,
+ &options)
+ );
+
+ ret = rcl_subscription_set_content_filter(
+ &subscription, &options);
+ if (is_cft_support) {
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100));
+ ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription));
+ } else {
+ ASSERT_EQ(RCL_RET_UNSUPPORTED, ret);
+ }
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ &options)
+ );
+ }
+
+ // publish with a non-filtered data again
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string));
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__Strings__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__Strings__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(
+ std::string(test_string),
+ std::string(msg.string_value.data, msg.string_value.size));
+ }
+}
+
+/* A subscription without a content filtered topic setting at beginning.
+ */
+TEST_F(
+ CLASSNAME(
+ TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) {
+ rcl_ret_t ret;
+ rcl_publisher_t publisher = rcl_get_zero_initialized_publisher();
+ const rosidl_message_type_support_t * ts =
+ ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings);
+ constexpr char topic[] = "rcl_test_subscription_not_begin_content_filtered_chatter";
+ rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+ ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ });
+ rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
+ // not to set filter expression
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+ ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ });
+ ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription));
+
+ // failed to get filter
+ {
+ rcl_subscription_content_filter_options_t content_filter_options =
+ rcl_subscription_get_default_content_filter_options();
+
+ ret = rcl_subscription_get_content_filter(
+ &subscription, &content_filter_options);
+ ASSERT_NE(RCL_RET_OK, ret);
+ }
+
+ ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100));
+
+ // publish with a non-filtered data
+ constexpr char test_string[] = "NotFilteredData";
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string));
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__Strings__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__Strings__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(
+ std::string(test_string),
+ std::string(msg.string_value.data, msg.string_value.size));
+ }
+
+ // set filter
+ const char * filter_expression2 = "string_value MATCH %0";
+ const char * expression_parameters2[] = {"'FilteredData'"};
+ size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *);
+ bool is_cft_support{true};
+ {
+ rcl_subscription_content_filter_options_t options =
+ rcl_subscription_get_default_content_filter_options();
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ filter_expression2, expression_parameters2_count, expression_parameters2,
+ &options)
+ );
+
+ ret = rcl_subscription_set_content_filter(
+ &subscription, &options);
+ if (RCL_RET_UNSUPPORTED == ret) {
+ is_cft_support = false;
+ } else {
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ &options)
+ );
+ }
+
+ // publish no filtered data again
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string));
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__Strings__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ if (is_cft_support) {
+ ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+ } else {
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__Strings__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(
+ std::string(test_string),
+ std::string(msg.string_value.data, msg.string_value.size));
+ }
+
+ // publish filtered data
+ constexpr char test_filtered_string[] = "FilteredData";
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string));
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__Strings__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100));
+
+ {
+ test_msgs__msg__Strings msg;
+ test_msgs__msg__Strings__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__Strings__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(
+ std::string(test_filtered_string),
+ std::string(msg.string_value.data, msg.string_value.size));
+ }
+}
+
TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) {
rcl_ret_t ret;
const rosidl_message_type_support_t * ts =
@@ -1087,6 +1510,8 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip
rcl_reset_error();
EXPECT_EQ(NULL, rcl_subscription_get_options(nullptr));
rcl_reset_error();
+ EXPECT_FALSE(rcl_subscription_is_cft_enabled(nullptr));
+ rcl_reset_error();
EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(&subscription_zero_init));
rcl_reset_error();
@@ -1098,6 +1523,115 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip
rcl_reset_error();
EXPECT_EQ(NULL, rcl_subscription_get_options(&subscription_zero_init));
rcl_reset_error();
+ EXPECT_FALSE(rcl_subscription_is_cft_enabled(&subscription_zero_init));
+ rcl_reset_error();
+}
+
+/* Test for all failure modes in rcl_subscription_set_content_filter function.
+ */
+TEST_F(
+ CLASSNAME(
+ TestSubscriptionFixtureInit,
+ RMW_IMPLEMENTATION), test_bad_rcl_subscription_set_content_filter) {
+ EXPECT_EQ(
+ RCL_RET_SUBSCRIPTION_INVALID,
+ rcl_subscription_set_content_filter(nullptr, nullptr));
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_SUBSCRIPTION_INVALID,
+ rcl_subscription_set_content_filter(&subscription_zero_init, nullptr));
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_set_content_filter(&subscription, nullptr));
+ rcl_reset_error();
+
+ // an options used later
+ rcl_subscription_content_filter_options_t options =
+ rcl_subscription_get_default_content_filter_options();
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ "data MATCH '0'",
+ 0,
+ nullptr,
+ &options
+ )
+ );
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(&options)
+ );
+ });
+
+ {
+ auto mock = mocking_utils::patch_and_return(
+ "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_UNSUPPORTED);
+ EXPECT_EQ(
+ RMW_RET_UNSUPPORTED,
+ rcl_subscription_set_content_filter(
+ &subscription, &options));
+ rcl_reset_error();
+ }
+
+ {
+ auto mock = mocking_utils::patch_and_return(
+ "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_ERROR);
+ EXPECT_EQ(
+ RMW_RET_ERROR,
+ rcl_subscription_set_content_filter(
+ &subscription, &options));
+ rcl_reset_error();
+ }
+}
+
+/* Test for all failure modes in rcl_subscription_get_content_filter function.
+ */
+TEST_F(
+ CLASSNAME(
+ TestSubscriptionFixtureInit,
+ RMW_IMPLEMENTATION), test_bad_rcl_subscription_get_content_filter) {
+ EXPECT_EQ(
+ RCL_RET_SUBSCRIPTION_INVALID,
+ rcl_subscription_get_content_filter(nullptr, nullptr));
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_SUBSCRIPTION_INVALID,
+ rcl_subscription_get_content_filter(&subscription_zero_init, nullptr));
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_get_content_filter(&subscription, nullptr));
+ rcl_reset_error();
+
+ rcl_subscription_content_filter_options_t options =
+ rcl_subscription_get_default_content_filter_options();
+
+ {
+ auto mock = mocking_utils::patch_and_return(
+ "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_UNSUPPORTED);
+ EXPECT_EQ(
+ RMW_RET_UNSUPPORTED,
+ rcl_subscription_get_content_filter(
+ &subscription, &options));
+ rcl_reset_error();
+ }
+
+ {
+ auto mock = mocking_utils::patch_and_return(
+ "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_ERROR);
+ EXPECT_EQ(
+ RMW_RET_ERROR,
+ rcl_subscription_get_content_filter(
+ &subscription, &options));
+ rcl_reset_error();
+ }
}
TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_init_fini_maybe_fail)
diff --git a/rcl/test/rcl/test_subscription_content_filter_options.cpp b/rcl/test/rcl/test_subscription_content_filter_options.cpp
new file mode 100644
index 000000000..d42b9b71c
--- /dev/null
+++ b/rcl/test/rcl/test_subscription_content_filter_options.cpp
@@ -0,0 +1,270 @@
+// Copyright 2021 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/error_handling.h"
+#include "rcl/subscription.h"
+
+TEST(TestSubscriptionContentFilterOptions, subscription_options_failure) {
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+
+ const char * filter_expression1 = "filter=1";
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_options_set_content_filter_options(
+ nullptr, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_options_set_content_filter_options(
+ filter_expression1, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_options_set_content_filter_options(
+ filter_expression1, 1, nullptr, &subscription_options)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_options_fini(
+ nullptr)
+ );
+ rcl_reset_error();
+}
+
+TEST(TestSubscriptionContentFilterOptions, subscription_options_success)
+{
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+
+ const char * filter_expression1 = "filter=1";
+
+ {
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_options_set_content_filter_options(
+ filter_expression1, 0, nullptr, &subscription_options)
+ );
+
+ rmw_subscription_content_filter_options_t * content_filter_options =
+ subscription_options.rmw_subscription_options.content_filter_options;
+ ASSERT_NE(nullptr, content_filter_options);
+ EXPECT_STREQ(filter_expression1, content_filter_options->filter_expression);
+ EXPECT_EQ(nullptr, content_filter_options->expression_parameters);
+ }
+
+ const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2";
+ const char * expression_parameters2[] = {
+ "1", "2", "3",
+ };
+ size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *);
+
+ {
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_options_set_content_filter_options(
+ filter_expression2, expression_parameters_count2,
+ expression_parameters2, &subscription_options)
+ );
+
+ rmw_subscription_content_filter_options_t * content_filter_options =
+ subscription_options.rmw_subscription_options.content_filter_options;
+ ASSERT_NE(nullptr, content_filter_options);
+ EXPECT_STREQ(filter_expression2, content_filter_options->filter_expression);
+ ASSERT_NE(nullptr, content_filter_options->expression_parameters);
+ ASSERT_EQ(
+ expression_parameters_count2,
+ content_filter_options->expression_parameters->size);
+ for (size_t i = 0; i < expression_parameters_count2; ++i) {
+ EXPECT_STREQ(
+ content_filter_options->expression_parameters->data[i],
+ expression_parameters2[i]);
+ }
+ }
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_options_fini(&subscription_options)
+ );
+}
+
+TEST(TestSubscriptionContentFilterOptions, content_filter_options_failure) {
+ rcl_subscription_content_filter_options_t content_filter_options =
+ rcl_subscription_get_default_content_filter_options();
+
+ const char * filter_expression1 = "filter=1";
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_init(
+ nullptr, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_init(
+ filter_expression1, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_init(
+ filter_expression1, 1, nullptr, &content_filter_options)
+ );
+ rcl_reset_error();
+
+ // set
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_set(
+ nullptr, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_set(
+ filter_expression1, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_set(
+ filter_expression1, 1, nullptr, &content_filter_options)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_fini(
+ nullptr)
+ );
+ rcl_reset_error();
+}
+
+TEST(TestSubscriptionContentFilterOptions, content_filter_options_success)
+{
+ rmw_subscription_content_filter_options_t * content_filter_options;
+ const char * filter_expression1 = "filter=1";
+ const char * filter_expression1_update = "filter=2";
+
+ rcl_subscription_content_filter_options_t subscription_content_filter_options =
+ rcl_subscription_get_default_content_filter_options();
+ {
+ // init with filter_expression1
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ filter_expression1, 0, nullptr, &subscription_content_filter_options)
+ );
+
+ content_filter_options =
+ subscription_content_filter_options.rmw_subscription_content_filter_options;
+ ASSERT_NE(nullptr, content_filter_options);
+ EXPECT_STREQ(filter_expression1, content_filter_options->filter_expression);
+ EXPECT_EQ(nullptr, content_filter_options->expression_parameters);
+
+ // set with filter_expression1_update
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_set(
+ filter_expression1_update, 0, nullptr, &subscription_content_filter_options)
+ );
+
+ content_filter_options =
+ subscription_content_filter_options.rmw_subscription_content_filter_options;
+ ASSERT_NE(nullptr, content_filter_options);
+ EXPECT_STREQ(filter_expression1_update, content_filter_options->filter_expression);
+ EXPECT_EQ(nullptr, content_filter_options->expression_parameters);
+ }
+
+ const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2";
+ const char * expression_parameters2[] = {
+ "1", "2", "3",
+ };
+ size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *);
+
+ const char * filter_expression2_update = "(filter1=%0 AND filter1=%1) OR filter2=%2";
+ const char * expression_parameters2_update[] = {
+ "11", "22", "33",
+ };
+ size_t expression_parameters_count2_update = sizeof(expression_parameters2) / sizeof(char *);
+
+ rcl_subscription_content_filter_options_t subscription_content_filter_options2 =
+ rcl_subscription_get_default_content_filter_options();
+ {
+ // init with filter_expression2 and expression_parameters2
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ filter_expression2, expression_parameters_count2,
+ expression_parameters2, &subscription_content_filter_options2)
+ );
+
+ content_filter_options =
+ subscription_content_filter_options2.rmw_subscription_content_filter_options;
+ ASSERT_NE(nullptr, content_filter_options);
+ EXPECT_STREQ(filter_expression2, content_filter_options->filter_expression);
+ ASSERT_NE(nullptr, content_filter_options->expression_parameters);
+ ASSERT_EQ(
+ expression_parameters_count2,
+ content_filter_options->expression_parameters->size);
+ for (size_t i = 0; i < expression_parameters_count2; ++i) {
+ EXPECT_STREQ(
+ content_filter_options->expression_parameters->data[i],
+ expression_parameters2[i]);
+ }
+
+ // set with filter_expression2_update and expression_parameters2_update
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_set(
+ filter_expression2_update, expression_parameters_count2_update,
+ expression_parameters2_update, &subscription_content_filter_options2)
+ );
+
+ content_filter_options =
+ subscription_content_filter_options2.rmw_subscription_content_filter_options;
+ ASSERT_NE(nullptr, content_filter_options);
+ EXPECT_STREQ(filter_expression2_update, content_filter_options->filter_expression);
+ ASSERT_NE(nullptr, content_filter_options->expression_parameters);
+ ASSERT_EQ(
+ expression_parameters_count2_update,
+ content_filter_options->expression_parameters->size);
+ for (size_t i = 0; i < expression_parameters_count2_update; ++i) {
+ EXPECT_STREQ(
+ content_filter_options->expression_parameters->data[i],
+ expression_parameters2_update[i]);
+ }
+ }
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ &subscription_content_filter_options)
+ );
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ &subscription_content_filter_options2)
+ );
+}
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;