Add support for user-specified content filters

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_content_filter(
+  rmw_subscription_t * subscription,
+  const rmw_subscription_content_filter_options_t * options)
+{
+  return rmw_api_connextdds_subscription_set_content_filter(
+    subscription, options);
+}
+
+rmw_ret_t
+rmw_subscription_get_content_filter(
+  const rmw_subscription_t * subscription,
+  rcutils_allocator_t * allocator,
+  rmw_subscription_content_filter_options_t * options)
+{
+  return rmw_api_connextdds_subscription_get_content_filter(
+    subscription, allocator, options);
+}
 
 rmw_ret_t
 rmw_destroy_subscription(

rmw_connextdds_common/CMakeLists.txt

@@ -62,13 +62,19 @@ function(rtirmw_add_library)
     ament_target_dependencies(${_rti_build_NAME}
         ${_rti_build_DEPS})
 
+    set(_extra_defines)
+    if("${CMAKE_BUILD_TYPE}" MATCHES "[dD]ebug")
+      list(APPEND _extra_defines "RMW_CONNEXT_DEBUG=1")
+    endif()
+
     target_compile_definitions(${_rti_build_NAME}
         PUBLIC
             RMW_VERSION_MAJOR=${rmw_VERSION_MAJOR}
             RMW_VERSION_MINOR=${rmw_VERSION_MINOR}
             RMW_VERSION_PATCH=${rmw_VERSION_PATCH}
             RMW_CONNEXT_DDS_API=RMW_CONNEXT_DDS_API_${_rti_build_API}
             ${_rti_build_DEFINES}
+            ${_extra_defines}
     )
 
     set(private_defines)
@@ -112,7 +118,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_filter_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_content_filter(
+  rmw_subscription_t * subscription,
+  const rmw_subscription_content_filter_options_t * options);
+
+RMW_CONNEXTDDS_PUBLIC
+rmw_ret_t
+rmw_api_connextdds_subscription_get_content_filter(
+  const rmw_subscription_t * subscription,
+  rcutils_allocator_t * const allocator,
+  rmw_subscription_content_filter_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_content_filter(
+    const rmw_subscription_content_filter_options_t * const options);
+
+  rmw_ret_t
+  get_content_filter(
+    rcutils_allocator_t * allocator,
+    rmw_subscription_content_filter_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/include/rmw_connextdds/static_config.hpp

@@ -15,6 +15,13 @@
 #ifndef RMW_CONNEXTDDS__STATIC_CONFIG_HPP_
 #define RMW_CONNEXTDDS__STATIC_CONFIG_HPP_
 
+/******************************************************************************
+ * Debug flags
+ ******************************************************************************/
+#ifndef RMW_CONNEXT_DEBUG
+#define RMW_CONNEXT_DEBUG                          0
+#endif  // RMW_CONNEXT_DEBUG
+
 /******************************************************************************
  * Default User Configuration
  ******************************************************************************/

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. */
 
@@ -332,7 +338,21 @@ rmw_ret_t
 rmw_context_impl_t::finalize_participant()
 {
   RMW_CONNEXT_LOG_DEBUG("finalizing DDS DomainParticipant")
-
+#if RMW_CONNEXT_DEBUG && RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO
+  // If we are building in Debug mode, an issue in Connext may prevent the
+  // participant from being able to delete any content-filtered topic if
+  // the participant has not been enabled.
+  // For this reason, make sure to enable the participant before trying to
+  // finalize it.
+  // TODO(asorbini) reconsider the need for this code in Connext > 6.1.0
+  if (DDS_RETCODE_OK !=
+    DDS_Entity_enable(DDS_DomainParticipant_as_entity(participant)))
+  {
+    RMW_CONNEXT_LOG_ERROR_SET(
+      "failed to enable DomainParticipant before deletion")
+    return RMW_RET_ERROR;
+  }
+#endif  // RMW_CONNEXT_DEBUG && RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO
   if (RMW_RET_OK != rmw_connextdds_graph_finalize(this)) {
     RMW_CONNEXT_LOG_ERROR("failed to finalize graph cache")
     return RMW_RET_ERROR;
@@ -386,7 +406,7 @@ rmw_context_impl_t::finalize_participant()
   if (nullptr != this->participant) {
     // If we are cleaning up after some RMW failure, it is possible for some
     // DataWriter to not have been deleted.
-    // Call DDS_Publisher_delete_contained_entities() to make sure we can
+    // Call DDS_DomainParticipant_delete_contained_entities() to make sure we can
     // dispose the publisher.
     if (DDS_RETCODE_OK !=
       DDS_DomainParticipant_delete_contained_entities(this->participant))
@@ -402,6 +422,7 @@ rmw_context_impl_t::finalize_participant()
       RMW_CONNEXT_LOG_ERROR_SET("failed to delete DDS participant")
       return RMW_RET_ERROR;
     }
+
     this->participant = nullptr;
   }