diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h
index 1219499994..28bbf17134 100644
--- a/rcl/include/rcl/subscription.h
+++ b/rcl/include/rcl/subscription.h
@@ -53,6 +53,11 @@ 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
+{
+ 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
@@ -209,6 +214,225 @@ 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 | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | No
+ *
+ * \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.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | No
+ *
+ * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause.
+ * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100.
+ * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder
+ * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression.
+ *
+ * It can be NULL if there is no "%n" tokens placeholder in filter_expression.
+ * \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 zero initialized subscription content filter options.
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_subscription_content_filter_options_t
+rcl_get_zero_initialized_subscription_content_filter_options(void);
+
+/// Initialize the content filter options for the given subscription options.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | No
+ *
+ * \param[in] subscription the handle to the subscription.
+ * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause,
+ * use empty ("") can reset (or clear) the content filter setting of a subscription.
+ * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100.
+ * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder
+ * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression.
+ *
+ * It can be NULL if there is no "%n" tokens placeholder in filter_expression.
+ * \param[out] options The subscription options to be set.
+ * \return `RCL_RET_OK` if set options successfully, or
+ * \return `RCL_RET_SUBSCRIPTION_INVALID` if subscription is invalid, 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_content_filter_options_init(
+ const rcl_subscription_t * subscription,
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_content_filter_options_t * options);
+
+/// Set the content filter options for the given subscription options.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | No
+ *
+ * \param[in] subscription the handle to the subscription.
+ * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause,
+ * use empty ("") can reset (or clear) the content filter setting of a subscription.
+ * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100.
+ * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder
+ * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression.
+ *
+ * It can be NULL if there is no "%n" tokens placeholder in filter_expression.
+ * \param[out] options The subscription options to be set.
+ * \return `RCL_RET_OK` if set options successfully, or
+ * \return `RCL_RET_SUBSCRIPTION_INVALID` if subscription is invalid, 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_content_filter_options_set(
+ const rcl_subscription_t * subscription,
+ 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 | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | No
+ *
+ * \param[in] subscription the handle to the subscription.
+ * \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_SUBSCRIPTION_INVALID` if subscription is invalid, 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(
+ const rcl_subscription_t * subscription,
+ 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 `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/subscription.c b/rcl/src/rcl/subscription.c
index 32c3ca7670..fd5984ded6 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.
@@ -138,6 +141,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 +183,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 +209,290 @@ 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);
+ if (expression_parameters_argc > 100) {
+ RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100");
+ return 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_ret_t rmw_ret;
+ rmw_subscription_content_filter_options_t * original_content_filter_options =
+ options->rmw_subscription_options.content_filter_options;
+ rmw_subscription_content_filter_options_t content_filter_options_backup =
+ rmw_get_zero_initialized_content_filter_options();
+
+ if (original_content_filter_options) {
+ // make a backup, restore the data if failure happened
+ rmw_ret = rmw_subscription_content_filter_options_copy(
+ original_content_filter_options,
+ allocator,
+ &content_filter_options_backup
+ );
+ if (rmw_ret != RMW_RET_OK) {
+ return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ }
+ } else {
+ options->rmw_subscription_options.content_filter_options =
+ allocator->allocate(
+ sizeof(rmw_subscription_content_filter_options_t), allocator->state);
+ if (!options->rmw_subscription_options.content_filter_options) {
+ RCL_SET_ERROR_MSG("failed to allocate memory");
+ return RCL_RET_BAD_ALLOC;
+ }
+ *options->rmw_subscription_options.content_filter_options =
+ rmw_get_zero_initialized_content_filter_options();
+ }
+
+ rmw_ret = rmw_subscription_content_filter_options_set(
+ filter_expression,
+ expression_parameters_argc,
+ expression_parameter_argv,
+ allocator,
+ options->rmw_subscription_options.content_filter_options
+ );
+
+ if (rmw_ret != RMW_RET_OK) {
+ ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ goto failed;
+ }
+
+ rmw_ret = rmw_subscription_content_filter_options_fini(
+ &content_filter_options_backup,
+ allocator
+ );
+ if (rmw_ret != RMW_RET_OK) {
+ return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ }
+
+ return RMW_RET_OK;
+
+failed:
+
+ if (original_content_filter_options == NULL) {
+ 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;
+ }
+ } else {
+ rmw_ret = rmw_subscription_content_filter_options_copy(
+ &content_filter_options_backup,
+ allocator,
+ options->rmw_subscription_options.content_filter_options
+ );
+ if (rmw_ret != RMW_RET_OK) {
+ return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ }
+
+ rmw_ret = rmw_subscription_content_filter_options_fini(
+ &content_filter_options_backup,
+ allocator
+ );
+ if (rmw_ret != RMW_RET_OK) {
+ return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+ }
+ }
+
+ return ret;
+}
+
+rcl_subscription_content_filter_options_t
+rcl_get_zero_initialized_subscription_content_filter_options()
+{
+ return (const rcl_subscription_content_filter_options_t) {
+ .rmw_subscription_content_filter_options =
+ rmw_get_zero_initialized_content_filter_options()
+ }; // NOLINT(readability/braces): false positive
+}
+
+rcl_ret_t
+rcl_subscription_content_filter_options_init(
+ const rcl_subscription_t * subscription,
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_content_filter_options_t * options)
+{
+ 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 = &subscription->impl->options.allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+ if (expression_parameters_argc > 100) {
+ RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100");
+ return RCL_RET_INVALID_ARGUMENT;
+ }
+
+ rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_init(
+ filter_expression,
+ expression_parameters_argc,
+ expression_parameter_argv,
+ allocator,
+ &options->rmw_subscription_content_filter_options
+ );
+
+ return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+}
+
+rcl_ret_t
+rcl_subscription_content_filter_options_set(
+ const rcl_subscription_t * subscription,
+ const char * filter_expression,
+ size_t expression_parameters_argc,
+ const char * expression_parameter_argv[],
+ rcl_subscription_content_filter_options_t * options)
+{
+ if (!rcl_subscription_is_valid(subscription)) {
+ return RCL_RET_SUBSCRIPTION_INVALID;
+ }
+ if (expression_parameters_argc > 100) {
+ RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100");
+ return RCL_RET_INVALID_ARGUMENT;
+ }
+ RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+ const rcl_allocator_t * allocator = &subscription->impl->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(
+ const rcl_subscription_t * subscription,
+ rcl_subscription_content_filter_options_t * options)
+{
+ 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 = &subscription->impl->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
+ );
+
+ return rcl_convert_rmw_ret_to_rcl_ret(ret);
+}
+
+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_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
+ const rmw_subscription_content_filter_options_t * content_filter_options =
+ &options->rmw_subscription_content_filter_options;
+ return rcl_subscription_options_set_content_filter_options(
+ content_filter_options->filter_expression,
+ content_filter_options->expression_parameters.size,
+ (const char **)content_filter_options->expression_parameters.data,
+ &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);
+ rcl_allocator_t * allocator = &subscription->impl->options.allocator;
+ RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+ rmw_ret_t rmw_ret = rmw_subscription_get_content_filter(
+ subscription->impl->rmw_handle,
+ allocator,
+ &options->rmw_subscription_content_filter_options);
+
+ return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+}
+
rcl_ret_t
rcl_take(
const rcl_subscription_t * subscription,
diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt
index 91e9e44bfc..a9969f5795 100644
--- a/rcl/test/CMakeLists.txt
+++ b/rcl/test/CMakeLists.txt
@@ -245,6 +245,7 @@ function(test_target_function)
APPEND_LIBRARY_DIRS ${extra_lib_dirs}
LIBRARIES ${PROJECT_NAME} mimick wait_for_entity_helpers
AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs"
+ TIMEOUT 120
)
# TODO(asorbini) Enable message timestamp tests for rmw_connextdds on Windows
# once clock incompatibilities are resolved.
@@ -443,3 +444,10 @@ 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}
+ AMENT_DEPENDENCIES "osrf_testing_tools_cpp" "test_msgs"
+)
diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp
index 6a6aa81c15..46bd152139 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"
@@ -885,6 +886,432 @@ 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 = '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, 10, 1000));
+
+ // 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, 10, 1000));
+ } else {
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000));
+
+ 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, 10, 1000));
+
+ {
+ 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 = %0";
+ const char * expression_parameters2[] = {"'FilteredOtherData'"};
+ size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *);
+ {
+ rcl_subscription_content_filter_options_t options =
+ rcl_get_zero_initialized_subscription_content_filter_options();
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ &subscription,
+ 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;
+ // waiting to allow for filter propagation
+ std::this_thread::sleep_for(std::chrono::seconds(10));
+ } else {
+ ASSERT_EQ(RCL_RET_UNSUPPORTED, ret);
+ }
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ &subscription, &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, 10, 1000));
+ } else {
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000));
+
+ 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, 10, 1000));
+
+ {
+ 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_get_zero_initialized_subscription_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_STREQ(filter_expression2, options->filter_expression);
+ 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(
+ &subscription,
+ &content_filter_options)
+ );
+ } else {
+ ASSERT_EQ(RCL_RET_UNSUPPORTED, ret);
+ }
+ }
+
+ // reset filter
+ {
+ rcl_subscription_content_filter_options_t options =
+ rcl_get_zero_initialized_subscription_content_filter_options();
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ &subscription,
+ "", 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;
+ // waiting to allow for filter propagation
+ std::this_thread::sleep_for(std::chrono::seconds(10));
+ ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000));
+ 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(
+ &subscription, &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, 10, 1000));
+
+ {
+ 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_initialized_with_content_filtering) {
+ 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, BasicTypes);
+ 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_get_zero_initialized_subscription_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, 10, 1000));
+
+ // publish with a non-filtered data
+ int32_t test_value = 3;
+ {
+ test_msgs__msg__BasicTypes msg;
+ test_msgs__msg__BasicTypes__init(&msg);
+ msg.int32_value = test_value;
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__BasicTypes__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000));
+
+ {
+ test_msgs__msg__BasicTypes msg;
+ test_msgs__msg__BasicTypes__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__BasicTypes__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_TRUE(test_value == msg.int32_value);
+ }
+
+ // set filter
+ const char * filter_expression2 = "int32_value = %0";
+ const char * expression_parameters2[] = {"4"};
+ size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *);
+ bool is_cft_support =
+ (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 ||
+ std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps_cpp") == 0);
+ {
+ rcl_subscription_content_filter_options_t options =
+ rcl_get_zero_initialized_subscription_content_filter_options();
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ &subscription,
+ filter_expression2, expression_parameters2_count, expression_parameters2,
+ &options)
+ );
+
+ ret = rcl_subscription_set_content_filter(
+ &subscription, &options);
+ if (!is_cft_support) {
+ ASSERT_EQ(RCL_RET_UNSUPPORTED, ret);
+ } else {
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ // waiting to allow for filter propagation
+ std::this_thread::sleep_for(std::chrono::seconds(10));
+ }
+
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ &subscription, &options)
+ );
+ }
+
+ // publish no filtered data again
+ {
+ test_msgs__msg__BasicTypes msg;
+ test_msgs__msg__BasicTypes__init(&msg);
+ msg.int32_value = test_value;
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__BasicTypes__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, 10, 1000));
+ } else {
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000));
+
+ test_msgs__msg__BasicTypes msg;
+ test_msgs__msg__BasicTypes__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__BasicTypes__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_TRUE(test_value == msg.int32_value);
+ }
+
+ // publish filtered data
+ int32_t test_filtered_value = 4;
+ {
+ test_msgs__msg__BasicTypes msg;
+ test_msgs__msg__BasicTypes__init(&msg);
+ msg.int32_value = test_filtered_value;
+ ret = rcl_publish(&publisher, &msg, nullptr);
+ test_msgs__msg__BasicTypes__fini(&msg);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000));
+
+ {
+ test_msgs__msg__BasicTypes msg;
+ test_msgs__msg__BasicTypes__init(&msg);
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ test_msgs__msg__BasicTypes__fini(&msg);
+ });
+ ret = rcl_take(&subscription, &msg, nullptr, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_TRUE(test_filtered_value == msg.int32_value);
+ }
+}
+
TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) {
rcl_ret_t ret;
const rosidl_message_type_support_t * ts =
@@ -1125,6 +1552,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();
@@ -1136,6 +1565,116 @@ 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_get_zero_initialized_subscription_content_filter_options();
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ &subscription,
+ "data = '0'",
+ 0,
+ nullptr,
+ &options
+ )
+ );
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(&subscription, &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_get_zero_initialized_subscription_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 0000000000..163bf3db72
--- /dev/null
+++ b/rcl/test/rcl/test_subscription_content_filter_options.cpp
@@ -0,0 +1,354 @@
+// Copyright 2022 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/node.h"
+#include "rcl/rcl.h"
+#include "rcl/subscription.h"
+
+#include "osrf_testing_tools_cpp/scope_exit.hpp"
+#include "test_msgs/msg/basic_types.h"
+
+
+TEST(TestSubscriptionOptionsContentFilter, 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(TestSubscriptionOptionsContentFilter, 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(0u, content_filter_options->expression_parameters.size);
+ EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data);
+ }
+
+ 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_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)
+ );
+}
+
+
+class TestSubscriptionContentFilterOptions : public ::testing::Test
+{
+public:
+ rcl_context_t * context_ptr;
+ rcl_node_t * node_ptr;
+ rcl_subscription_t * subscription_ptr;
+ void SetUp()
+ {
+ rcl_ret_t ret;
+ {
+ rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+ ret = rcl_init_options_init(&init_options, rcl_get_default_allocator());
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str;
+ });
+ this->context_ptr = new rcl_context_t;
+ *this->context_ptr = rcl_get_zero_initialized_context();
+ ret = rcl_init(0, nullptr, &init_options, this->context_ptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+ this->node_ptr = new rcl_node_t;
+ *this->node_ptr = rcl_get_zero_initialized_node();
+ constexpr char name[] = "test_subscription_content_filter_options_node";
+ rcl_node_options_t node_options = rcl_node_get_default_options();
+ ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+ const rosidl_message_type_support_t * ts =
+ ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes);
+ constexpr char topic[] = "chatter";
+
+ this->subscription_ptr = new rcl_subscription_t;
+ *this->subscription_ptr = rcl_get_zero_initialized_subscription();
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+ ret = rcl_subscription_init(
+ this->subscription_ptr, this->node_ptr, ts, topic, &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ void TearDown()
+ {
+ rcl_ret_t ret = rcl_subscription_fini(this->subscription_ptr, this->node_ptr);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ret = rcl_node_fini(this->node_ptr);
+ delete this->node_ptr;
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ret = rcl_shutdown(this->context_ptr);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ret = rcl_context_fini(this->context_ptr);
+ delete this->context_ptr;
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+};
+
+TEST_F(TestSubscriptionContentFilterOptions, content_filter_options_failure) {
+ rcl_subscription_content_filter_options_t content_filter_options =
+ rcl_get_zero_initialized_subscription_content_filter_options();
+
+ const char * filter_expression1 = "filter=1";
+ EXPECT_EQ(
+ RCL_RET_SUBSCRIPTION_INVALID,
+ rcl_subscription_content_filter_options_init(
+ nullptr, nullptr, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_init(
+ this->subscription_ptr, nullptr, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_init(
+ this->subscription_ptr, filter_expression1, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_init(
+ this->subscription_ptr, filter_expression1, 1, nullptr, &content_filter_options)
+ );
+ rcl_reset_error();
+
+ // set
+ EXPECT_EQ(
+ RCL_RET_SUBSCRIPTION_INVALID,
+ rcl_subscription_content_filter_options_set(
+ nullptr, nullptr, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_set(
+ this->subscription_ptr, nullptr, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_set(
+ this->subscription_ptr, filter_expression1, 0, nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_set(
+ this->subscription_ptr, filter_expression1, 1, nullptr, &content_filter_options)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_SUBSCRIPTION_INVALID,
+ rcl_subscription_content_filter_options_fini(
+ nullptr, nullptr)
+ );
+ rcl_reset_error();
+
+ EXPECT_EQ(
+ RCL_RET_INVALID_ARGUMENT,
+ rcl_subscription_content_filter_options_fini(
+ this->subscription_ptr, nullptr)
+ );
+ rcl_reset_error();
+}
+
+TEST_F(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_get_zero_initialized_subscription_content_filter_options();
+ {
+ // init with filter_expression1
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ this->subscription_ptr, filter_expression1, 0, nullptr,
+ &subscription_content_filter_options)
+ );
+
+ content_filter_options =
+ &subscription_content_filter_options.rmw_subscription_content_filter_options;
+ EXPECT_STREQ(filter_expression1, content_filter_options->filter_expression);
+ EXPECT_EQ(0u, content_filter_options->expression_parameters.size);
+ EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data);
+
+ // set with filter_expression1_update
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_set(
+ this->subscription_ptr, filter_expression1_update, 0, nullptr,
+ &subscription_content_filter_options)
+ );
+
+ content_filter_options =
+ &subscription_content_filter_options.rmw_subscription_content_filter_options;
+ EXPECT_STREQ(filter_expression1_update, content_filter_options->filter_expression);
+ EXPECT_EQ(0u, content_filter_options->expression_parameters.size);
+ EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data);
+ }
+
+ 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_get_zero_initialized_subscription_content_filter_options();
+ {
+ // init with filter_expression2 and expression_parameters2
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_init(
+ this->subscription_ptr, 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_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(
+ this->subscription_ptr, 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_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(
+ this->subscription_ptr, &subscription_content_filter_options)
+ );
+ EXPECT_EQ(
+ RCL_RET_OK,
+ rcl_subscription_content_filter_options_fini(
+ this->subscription_ptr, &subscription_content_filter_options2)
+ );
+}