Add support for user-specified content filters.

Signed-off-by: Andrea Sorbini <[email protected]>

rmw_connextdds/src/rmw_api_impl_ndds.cpp

@@ -647,6 +647,24 @@ rmw_subscription_get_actual_qos(
   return rmw_api_connextdds_subscription_get_actual_qos(subscription, qos);
 }
 
+rmw_ret_t
+rmw_subscription_set_cft_expression_parameters(
+  rmw_subscription_t * subscription,
+  const rmw_subscription_content_filtered_topic_options_t * options)
+{
+  return rmw_api_connextdds_subscription_set_cft_expression_parameters(
+    subscription, options);
+}
+
+rmw_ret_t
+rmw_subscription_get_cft_expression_parameters(
+  const rmw_subscription_t * subscription,
+  rcutils_allocator_t * allocator,
+  rmw_subscription_content_filtered_topic_options_t * options)
+{
+  return rmw_api_connextdds_subscription_get_cft_expression_parameters(
+    subscription, allocator, options);
+}
 
 rmw_ret_t
 rmw_destroy_subscription(

rmw_connextdds_common/CMakeLists.txt

@@ -112,7 +112,8 @@ set(RMW_CONNEXT_DEPS
     rosidl_typesupport_fastrtps_cpp
     rosidl_typesupport_introspection_c
     rosidl_typesupport_introspection_cpp
-    rti_connext_dds_cmake_module)
+    rti_connext_dds_cmake_module
+    rti_connext_dds_custom_sql_filter)
 
 foreach(pkg_dep ${RMW_CONNEXT_DEPS})
     find_package(${pkg_dep} REQUIRED)

rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp

@@ -65,13 +65,19 @@ rmw_connextdds_initialize_participant_qos_impl(
   rmw_context_impl_t * const ctx,
   DDS_DomainParticipantQos * const dp_qos);
 
+rmw_ret_t
+rmw_connextdds_configure_participant(
+  rmw_context_impl_t * const ctx,
+  DDS_DomainParticipant * const participant);
+
 rmw_ret_t
 rmw_connextdds_create_contentfilteredtopic(
   rmw_context_impl_t * const ctx,
   DDS_DomainParticipant * const dp,
   DDS_Topic * const base_topic,
   const char * const cft_name,
   const char * const cft_filter,
+  const rcutils_string_array_t * const cft_expression_parameters,
   DDS_TopicDescription ** const cft_out);
 
 rmw_ret_t
@@ -258,4 +264,16 @@ rmw_connextdds_enable_security(
   DDS_SECURITY_PROPERTY_PREFIX ".logging.log_level"
 #endif /* DDS_SECURITY_LOGGING_LEVEL_PROPERTY */
 
+rmw_ret_t
+rmw_connextdds_set_cft_filter_expression(
+  DDS_TopicDescription * const topic_desc,
+  const char * const cft_expression,
+  const rcutils_string_array_t * const cft_expression_parameters);
+
+rmw_ret_t
+rmw_connextdds_get_cft_filter_expression(
+  DDS_TopicDescription * const topic_desc,
+  rcutils_allocator_t * const allocator,
+  rmw_subscription_content_filtered_topic_options_t * const options);
+
 #endif  // RMW_CONNEXTDDS__DDS_API_HPP_

rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp

@@ -435,6 +435,18 @@ rmw_api_connextdds_subscription_get_actual_qos(
   const rmw_subscription_t * subscription,
   rmw_qos_profile_t * qos);
 
+RMW_CONNEXTDDS_PUBLIC
+rmw_ret_t
+rmw_api_connextdds_subscription_set_cft_expression_parameters(
+  rmw_subscription_t * subscription,
+  const rmw_subscription_content_filtered_topic_options_t * options);
+
+RMW_CONNEXTDDS_PUBLIC
+rmw_ret_t
+rmw_api_connextdds_subscription_get_cft_expression_parameters(
+  const rmw_subscription_t * subscription,
+  rcutils_allocator_t * const allocator,
+  rmw_subscription_content_filtered_topic_options_t * options);
 
 RMW_CONNEXTDDS_PUBLIC
 rmw_ret_t

rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp

@@ -438,6 +438,15 @@ class RMW_Connext_Subscriber
     rmw_message_info_t * const message_info,
     bool * const taken);
 
+  rmw_ret_t
+  set_cft_expression_parameters(
+    const rmw_subscription_content_filtered_topic_options_t * const options);
+
+  rmw_ret_t
+  get_cft_expression_parameters(
+    rcutils_allocator_t * allocator,
+    rmw_subscription_content_filtered_topic_options_t * const options);
+
   bool
   has_data()
   {
@@ -470,6 +479,17 @@ class RMW_Connext_Subscriber
     return this->dds_topic;
   }
 
+  static std::string get_atomic_id()
+  {
+    static std::atomic_uint64_t id;
+    return std::to_string(id++);
+  }
+
+  bool is_cft_enabled()
+  {
+    return !this->cft_expression.empty();
+  }
+
   const bool internal;
   const bool ignore_local;
 
@@ -478,6 +498,7 @@ class RMW_Connext_Subscriber
   DDS_DataReader * dds_reader;
   DDS_Topic * dds_topic;
   DDS_TopicDescription * dds_topic_cft;
+  std::string cft_expression;
   RMW_Connext_MessageTypeSupport * type_support;
   rmw_gid_t ros_gid;
   const bool created_topic;
@@ -496,6 +517,7 @@ class RMW_Connext_Subscriber
     const bool ignore_local,
     const bool created_topic,
     DDS_TopicDescription * const dds_topic_cft,
+    const char * const cft_expression,
     const bool internal);
 
   friend class RMW_Connext_SubscriberStatusCondition;

rmw_connextdds_common/package.xml

@@ -11,6 +11,7 @@
   <buildtool_export_depend>ament_cmake</buildtool_export_depend>
 
   <depend>rti_connext_dds_cmake_module</depend>
+  <depend>rti_connext_dds_custom_sql_filter</depend>
   <depend>fastcdr</depend>
   <depend>rcutils</depend>
   <depend>rcpputils</depend>

rmw_connextdds_common/src/common/rmw_context.cpp

@@ -218,6 +218,12 @@ rmw_context_impl_t::initialize_participant(const bool localhost_only)
     return RMW_RET_ERROR;
   }
 
+  rmw_ret_t cfg_rc = rmw_connextdds_configure_participant(this, this->participant);
+  if (RMW_RET_OK != cfg_rc) {
+    RMW_CONNEXT_LOG_ERROR("failed to configure DDS participant")
+    return cfg_rc;
+  }
+
   /* Create DDS publisher/subscriber objects that will be used for all DDS
      writers/readers created to support RMW publishers/subscriptions. */
 

rmw_connextdds_common/src/common/rmw_impl.cpp

@@ -29,6 +29,7 @@
 const char * const ROS_TOPIC_PREFIX = "rt";
 const char * const ROS_SERVICE_REQUESTER_PREFIX = ROS_SERVICE_REQUESTER_PREFIX_STR;
 const char * const ROS_SERVICE_RESPONSE_PREFIX = ROS_SERVICE_RESPONSE_PREFIX_STR;
+const char * const ROS_CFT_TOPIC_NAME_INFIX = "_ContentFilterTopic";
 
 std::string
 rmw_connextdds_create_topic_name(
@@ -1129,13 +1130,15 @@ RMW_Connext_Subscriber::RMW_Connext_Subscriber(
   const bool ignore_local,
   const bool created_topic,
   DDS_TopicDescription * const dds_topic_cft,
+  const char * const cft_expression,
   const bool internal)
 : internal(internal),
   ignore_local(ignore_local),
   ctx(ctx),
   dds_reader(dds_reader),
   dds_topic(dds_topic),
   dds_topic_cft(dds_topic_cft),
+  cft_expression(cft_expression),
   type_support(type_support),
   created_topic(created_topic),
   status_condition(dds_reader, ignore_local, internal)
@@ -1256,19 +1259,40 @@ RMW_Connext_Subscriber::create(
     });
 
   DDS_TopicDescription * sub_topic = DDS_Topic_as_topicdescription(topic);
+  std::string sub_cft_name;
+  const char * sub_cft_expr = "";
+  const rcutils_string_array_t * sub_cft_params = nullptr;
 
   if (nullptr != cft_name) {
-    rmw_ret_t cft_rc =
-      rmw_connextdds_create_contentfilteredtopic(
-      ctx, dp, topic, cft_name, cft_filter, &cft_topic);
+    sub_cft_name = cft_name;
+    sub_cft_expr = cft_filter;
+  } else {
+    sub_cft_name =
+      fqtopic_name + ROS_CFT_TOPIC_NAME_INFIX + RMW_Connext_Subscriber::get_atomic_id();
+    if (nullptr != subscriber_options->content_filtered_topic_options) {
+      sub_cft_expr =
+        subscriber_options->content_filtered_topic_options->filter_expression;
+      sub_cft_params =
+        subscriber_options->content_filtered_topic_options->expression_parameters;
+    }
+  }
 
-    if (RMW_RET_OK != cft_rc) {
-      if (RMW_RET_UNSUPPORTED != cft_rc) {
-        return nullptr;
-      }
-    } else {
-      sub_topic = cft_topic;
+  rmw_ret_t cft_rc =
+    rmw_connextdds_create_contentfilteredtopic(
+    ctx,
+    dp,
+    topic,
+    sub_cft_name.c_str(),
+    sub_cft_expr,
+    sub_cft_params,
+    &cft_topic);
+
+  if (RMW_RET_OK != cft_rc) {
+    if (RMW_RET_UNSUPPORTED != cft_rc) {
+      return nullptr;
     }
+  } else {
+    sub_topic = cft_topic;
   }
 
   // The following initialization generates warnings when built
@@ -1337,6 +1361,7 @@ RMW_Connext_Subscriber::create(
     subscriber_options->ignore_local_publications,
     topic_created,
     cft_topic,
+    sub_cft_expr,
     internal);
 
   if (nullptr == rmw_sub_impl) {
@@ -1529,6 +1554,37 @@ RMW_Connext_Subscriber::take_serialized(
   return rc;
 }
 
+
+rmw_ret_t
+RMW_Connext_Subscriber::set_cft_expression_parameters(
+  const rmw_subscription_content_filtered_topic_options_t * const options)
+{
+  const char * const filter_expression =
+    (nullptr != options && nullptr != options->filter_expression) ?
+    options->filter_expression : "";
+
+  const rcutils_string_array_t * filter_params =
+    (nullptr != options) ? options->expression_parameters : nullptr;
+
+  rmw_ret_t rc = rmw_connextdds_set_cft_filter_expression(
+    this->dds_topic_cft, filter_expression, filter_params);
+  if (RMW_RET_OK != rc) {
+    return rc;
+  }
+
+  this->cft_expression = filter_expression;
+
+  return RMW_RET_OK;
+}
+
+rmw_ret_t
+RMW_Connext_Subscriber::get_cft_expression_parameters(
+  rcutils_allocator_t * allocator,
+  rmw_subscription_content_filtered_topic_options_t * const options)
+{
+  return rmw_connextdds_get_cft_filter_expression(this->dds_topic_cft, allocator, options);
+}
+
 rmw_ret_t
 RMW_Connext_Subscriber::loan_messages(const bool update_condition)
 {
@@ -1815,6 +1871,7 @@ rmw_connextdds_create_subscriber(
     topic_name_len + 1);
   rmw_subscriber->options = *subscriber_options;
   rmw_subscriber->can_loan_messages = false;
+  rmw_subscriber->is_cft_enabled = rmw_sub_impl->is_cft_enabled();
 
   if (!internal) {
     if (RMW_RET_OK != rmw_sub_impl->enable()) {

rmw_connextdds_common/src/common/rmw_subscription.cpp

@@ -166,6 +166,51 @@ rmw_api_connextdds_subscription_get_actual_qos(
   return sub_impl->qos(qos);
 }
 
+rmw_ret_t
+rmw_api_connextdds_subscription_set_cft_expression_parameters(
+  rmw_subscription_t * subscription,
+  const rmw_subscription_content_filtered_topic_options_t * options)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    subscription,
+    subscription->implementation_identifier,
+    RMW_CONNEXTDDS_ID,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  // RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT);
+
+  RMW_Connext_Subscriber * const sub_impl =
+    reinterpret_cast<RMW_Connext_Subscriber *>(subscription->data);
+
+  rmw_ret_t rc = sub_impl->set_cft_expression_parameters(options);
+  subscription->is_cft_enabled = sub_impl->is_cft_enabled();
+
+  return rc;
+}
+
+
+rmw_ret_t
+rmw_api_connextdds_subscription_get_cft_expression_parameters(
+  const rmw_subscription_t * subscription,
+  rcutils_allocator_t * const allocator,
+  rmw_subscription_content_filtered_topic_options_t * options)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    subscription,
+    subscription->implementation_identifier,
+    RMW_CONNEXTDDS_ID,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT);
+
+  RMW_Connext_Subscriber * const sub_impl =
+    reinterpret_cast<RMW_Connext_Subscriber *>(subscription->data);
+
+  rmw_ret_t rc = sub_impl->get_cft_expression_parameters(allocator, options);
+
+  return rc;
+}
 
 rmw_ret_t
 rmw_api_connextdds_destroy_subscription(