Add support for content-filtered topics

rmw_connextdds/src/rmw_api_impl_ndds.cpp

@@ -637,6 +637,28 @@ rmw_subscription_get_actual_qos(
 }
 
 
+rmw_ret_t
+rmw_subscription_set_cft_expression_parameters(
+  rmw_subscription_t * subscription,
+  const char * filter_expression,
+  const rcutils_string_array_t * expression_parameters)
+{
+  return rmw_api_connextdds_subscription_set_cft_expression_parameters(
+    subscription, filter_expression, expression_parameters);
+}
+
+
+rmw_ret_t
+rmw_subscription_get_cft_expression_parameters(
+  const rmw_subscription_t * subscription,
+  char ** filter_expression,
+  rcutils_string_array_t * expression_parameters)
+{
+  return rmw_api_connextdds_subscription_get_cft_expression_parameters(
+    subscription, filter_expression, expression_parameters);
+}
+
+
 rmw_ret_t
 rmw_destroy_subscription(
   rmw_node_t * node,

rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp

@@ -72,6 +72,7 @@ rmw_connextdds_create_contentfilteredtopic(
   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
@@ -207,6 +208,18 @@ rmw_connextdds_enable_security(
   rmw_context_impl_t * const ctx,
   DDS_DomainParticipantQos * const qos);
 
+rmw_ret_t
+rmw_connextdds_set_cft_filter_expression(
+  DDS_TopicDescription * const topic_desc,
+  const char * const filter_expression,
+  const rcutils_string_array_t * const expression_parameters);
+
+rmw_ret_t
+rmw_connextdds_get_cft_filter_expression(
+  DDS_TopicDescription * const topic_desc,
+  char ** const expr_out,
+  rcutils_string_array_t * const cft_params_out);
+
 // Define some macro aliases for security-related properties
 #ifndef DDS_SECURITY_PROPERTY_PREFIX
 #define DDS_SECURITY_PROPERTY_PREFIX \

rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp

@@ -430,6 +430,22 @@ rmw_api_connextdds_subscription_get_actual_qos(
   rmw_qos_profile_t * qos);
 
 
+RMW_CONNEXTDDS_PUBLIC
+rmw_ret_t
+rmw_api_connextdds_subscription_set_cft_expression_parameters(
+  rmw_subscription_t * subscription,
+  const char * filter_expression,
+  const rcutils_string_array_t * expression_parameters);
+
+
+RMW_CONNEXTDDS_PUBLIC
+rmw_ret_t
+rmw_api_connextdds_subscription_get_cft_expression_parameters(
+  const rmw_subscription_t * subscription,
+  char ** filter_expression,
+  rcutils_string_array_t * expression_parameters);
+
+
 RMW_CONNEXTDDS_PUBLIC
 rmw_ret_t
 rmw_api_connextdds_destroy_subscription(

rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp

@@ -18,6 +18,7 @@
 #include <string>
 #include <vector>
 #include <map>
+#include <memory>
 #include <mutex>
 #include <condition_variable>
 #include <atomic>
@@ -273,6 +274,7 @@ class RMW_Connext_Subscriber
     const rmw_qos_profile_t * const qos_policies,
     const rmw_subscription_options_t * const subscriber_options,
     const bool internal = false,
+    const rmw_node_t * const node = nullptr,
     const RMW_Connext_MessageType msg_type = RMW_CONNEXT_MESSAGE_USERDATA,
     const void * const intro_members = nullptr,
     const bool intro_members_cpp = false,
@@ -281,7 +283,7 @@ class RMW_Connext_Subscriber
     const char * const cft_filter = nullptr);
 
   rmw_ret_t
-  finalize();
+  finalize(const bool reset_cft = false);
 
   DDS_DataReader *
   reader() const
@@ -353,7 +355,7 @@ class RMW_Connext_Subscriber
   RMW_Connext_SubscriberStatusCondition *
   condition()
   {
-    return &this->status_condition;
+    return this->status_condition.get();
   }
 
   const rmw_gid_t * gid() const
@@ -396,7 +398,7 @@ class RMW_Connext_Subscriber
     }
 
     if (this->internal) {
-      return this->status_condition.trigger_loan_guard_condition(this->loan_len > 0);
+      return this->status_condition->trigger_loan_guard_condition(this->loan_len > 0);
     }
 
     return RMW_RET_OK;
@@ -437,6 +439,18 @@ class RMW_Connext_Subscriber
     rmw_message_info_t * const message_info,
     bool * const taken);
 
+  rmw_ret_t
+  set_cft_expression_parameters(
+    const char * const filter_expression,
+    const rcutils_string_array_t * const expression_parameters
+  );
+
+  rmw_ret_t
+  get_cft_expression_parameters(
+    char ** const filter_expression,
+    rcutils_string_array_t * const expression_parameters
+  );
+
   bool
   has_data()
   {
@@ -467,6 +481,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_supported()
+  {
+    return nullptr != dds_topic_cft;
+  }
+
   const bool internal;
   const bool ignore_local;
 
@@ -478,12 +503,16 @@ class RMW_Connext_Subscriber
   RMW_Connext_MessageTypeSupport * type_support;
   rmw_gid_t ros_gid;
   const bool created_topic;
-  RMW_Connext_SubscriberStatusCondition status_condition;
+  std::shared_ptr<RMW_Connext_SubscriberStatusCondition> status_condition;
   RMW_Connext_UntypedSampleSeq loan_data;
   DDS_SampleInfoSeq loan_info;
   size_t loan_len;
   size_t loan_next;
   std::mutex loan_mutex;
+  std::mutex cft_mutex;
+  const rmw_node_t * const node;
+  rmw_qos_profile_t qos_policies;
+  rmw_subscription_options_t subscriber_options;
 
   RMW_Connext_Subscriber(
     rmw_context_impl_t * const ctx,
@@ -493,7 +522,23 @@ class RMW_Connext_Subscriber
     const bool ignore_local,
     const bool created_topic,
     DDS_TopicDescription * const dds_topic_cft,
-    const bool internal);
+    const bool internal,
+    const rmw_node_t * const node,
+    const rmw_qos_profile_t * const qos_policies,
+    const rmw_subscription_options_t * const subscriber_options);
+
+  static
+  DDS_DataReader *
+  initialize_datareader(
+    rmw_context_impl_t * const ctx,
+    DDS_DomainParticipant * const dp,
+    DDS_Subscriber * const sub,
+    const std::string & fqtopic_name,
+    const rmw_qos_profile_t * const qos_policies,
+    RMW_Connext_MessageTypeSupport * const type_support,
+    const rmw_subscription_options_t * const subscriber_options,
+    const bool internal,
+    DDS_TopicDescription * const sub_topic);
 
   friend class RMW_Connext_SubscriberStatusCondition;
 };

rmw_connextdds_common/src/common/rmw_event.cpp

@@ -124,12 +124,16 @@ rmw_api_connextdds_take_event(
   } else {
     condition = RMW_Connext_Event::publisher(event_handle)->condition();
   }
-  rmw_ret_t rc = condition->get_status(event_handle->event_type, event_info);
-  if (RMW_RET_OK != rc) {
-    RMW_CONNEXT_LOG_ERROR_SET("failed to get status from DDS entity")
-    return rc;
+  if (nullptr != condition) {
+    rmw_ret_t rc = condition->get_status(event_handle->event_type, event_info);
+    if (RMW_RET_OK != rc) {
+      RMW_CONNEXT_LOG_ERROR_SET("failed to get status from DDS entity")
+      return rc;
+    }
+
+    *taken = true;
+  } else {
+    RMW_CONNEXT_LOG_WARNING("condition was reset because of resetting cft")
   }
-
-  *taken = true;
   return RMW_RET_OK;
 }