Add support for user-specified content filters (#68)

* Add support for user-specified content filters.

Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* - Resolve memory leak of custom content-filter resources
- Add missing package dependencies for rti_connext_dds_custom_sql_filter
- Clean up all participants upon factory finalization
- Reset context state upon finalization (rmw_connextddsmicro)
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Assume non-null options argument
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* - Return error when retrieving content-filter from a subscription that doesn't have one.
- Rename internal functions related to content-filters
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Fix compilation error, oops.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* - Define RMW_CONNEXT_DEBUG when building Debug libraries.
- Make sure participant is enabled before deleting contained entities when using Connext debug libraries.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Resolve memory leak for finalization on error.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Rename content filter public API.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Add client/service QoS getters (#67)

Signed-off-by: Mauro Passerino <mpasserino@irobot.com>

* Changelogs

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* 0.8.1

* Fix cpplint errors (#69)

* Use static_cast instead of C-style cast

Fixes cpplint error.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Update NOLINT category

Relates to ament/ament_lint#324

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* 0.8.2

Signed-off-by: Audrow Nash <audrow@hey.com>

* Update rti-connext-dds dependency to 6.0.1. (#71)

Now that this package is available in the ROS bootstrap repository for Ubuntu Focal and Jammy we can bump the expected dependency version.

* 0.8.3

* Add rmw listener apis (#44)

* Add stubs for setting listener callbacks

Signed-off-by: Mauro Passerino <mpasserino@irobot.com>

* Address PR suggestions

Signed-off-by: Mauro Passerino <mpasserino@irobot.com>

* Fix linter issues

Signed-off-by: Mauro Passerino <mpasserino@irobot.com>

Co-authored-by: Mauro Passerino <mpasserino@irobot.com>
Co-authored-by: Alberto Soragna <alberto.soragna@gmail.com>

* Changelog. (#73)

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* 0.9.0

* add stub for content filtered topic

Signed-off-by: Chen Lihui <lihui.chen@sony.com>

* * Rebased branch asorbini/cft on top of 0.9.0.
* Resolved CFT finalization issues on error.
* Verified and cleaned up build for rmw_connextddsmicro.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Move custom SQL filter to rmw_connextdds_common
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Try to resolve linking error on Windows.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Optionally disable writer-side CFT optimizations to support Windows.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* No need to declare private CFT function on Windows.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* remove stub implementation for ContentFilteredTopic.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

* address cpplint error.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

* Avoid conversion warnings on Windows.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Use strtol instead of sscanf to avoid warnings on Windows.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

* Avoid finalizing participants if factory is not available.
Signed-off-by: Andrea Sorbini <asorbini@rti.com>

Co-authored-by: mauropasse <mauropasse@hotmail.com>
Co-authored-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Co-authored-by: Jacob Perron <jacob@openrobotics.org>
Co-authored-by: Audrow Nash <audrow@hey.com>
Co-authored-by: Steven! Ragnarök <nuclearsandwich@users.noreply.github.com>
Co-authored-by: Steven! Ragnarök <steven@nuclearsandwich.com>
Co-authored-by: iRobot ROS <49500531+irobot-ros@users.noreply.github.com>
Co-authored-by: Mauro Passerino <mpasserino@irobot.com>
Co-authored-by: Alberto Soragna <alberto.soragna@gmail.com>
Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>
Co-authored-by: Chen Lihui <lihui.chen@sony.com>
Co-authored-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

rmw_connextdds/src/rmw_api_impl_ndds.cpp

@@ -716,33 +716,25 @@ 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)
 {
- UNUSED_ARG(subscription);
- UNUSED_ARG(options);
- RMW_CONNEXT_LOG_NOT_IMPLEMENTED
- return RMW_RET_UNSUPPORTED;
+ 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)
 {
- UNUSED_ARG(subscription);
- UNUSED_ARG(allocator);
- UNUSED_ARG(options);
- RMW_CONNEXT_LOG_NOT_IMPLEMENTED
- return RMW_RET_UNSUPPORTED;
+ return rmw_api_connextdds_subscription_get_content_filter(
+ subscription, allocator, options);
 }
 
-
 rmw_ret_t
 rmw_destroy_subscription(
  rmw_node_t * node,

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)
@@ -189,6 +195,9 @@ else()
  if("${CONNEXTDDS_VERSION}" VERSION_LESS "6.0.0")
  list(APPEND extra_defines "RMW_CONNEXT_DDS_API_PRO_LEGACY=1")
  endif()
+ if(CONNEXTDDS_ARCH MATCHES ".*Win.*")
+ list(APPEND extra_defines "RMW_CONNEXT_BUILTIN_CFT_COMPATIBILITY_MODE=1")
+ endif()
  rtirmw_add_library(
  NAME ${PROJECT_NAME}_pro
  API PRO
@@ -197,8 +206,10 @@ else()
  src/ndds/rmw_type_support_ndds.cpp
  src/ndds/rmw_typecode.cpp
  src/ndds/dds_api_ndds.cpp
+ src/ndds/custom_sql_filter.cpp
  include/rmw_connextdds/typecode.hpp
  include/rmw_connextdds/dds_api_ndds.hpp
+ include/rmw_connextdds/custom_sql_filter.hpp
  DEPS ${RMW_CONNEXT_DEPS}
  LIBRARIES RTIConnextDDS::c_api
  DEFINES ${extra_defines})

rmw_connextdds_common/include/rmw_connextdds/custom_sql_filter.hpp

@@ -0,0 +1,66 @@
+// Copyright 2021 Real-Time Innovations, Inc. (RTI)
+//
+// 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.
+#ifndef RMW_CONNEXTDDS__CUSTOM_SQL_FILTER_HPP_
+#define RMW_CONNEXTDDS__CUSTOM_SQL_FILTER_HPP_
+
+#include "rmw_connextdds/dds_api.hpp"
+
+#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO
+
+namespace rti_connext_dds_custom_sql_filter
+{
+
+struct CustomSqlFilterData
+{
+ DDS_SqlFilterGeneratorQos base;
+
+ CustomSqlFilterData();
+
+ DDS_ReturnCode_t
+ set_memory_management_property(
+ const DDS_DomainParticipantQos & dp_qos);
+};
+
+RMW_CONNEXTDDS_PUBLIC
+DDS_ReturnCode_t
+register_content_filter(
+ DDS_DomainParticipant * const participant,
+ CustomSqlFilterData * const filter_data);
+
+RMW_CONNEXTDDS_PUBLIC
+extern const char * const PLUGIN_NAME;
+
+} // namespace rti_connext_dds_custom_sql_filter
+
+#if !RMW_CONNEXT_BUILTIN_CFT_COMPATIBILITY_MODE
+extern "C" {
+// This is an internal function from RTI Connext DDS which allows a filter to
+// be registered as "built-in". We need this because we want this custom filter
+// to be a replacement for the built-in SQL-like filter.
+RMW_CONNEXTDDS_PUBLIC
+DDS_ReturnCode_t
+DDS_ContentFilter_register_filter(
+ DDS_DomainParticipant * participant,
+ const char * name,
+ const struct DDS_ContentFilter * filter,
+ const DDS_ContentFilterEvaluateFunction evaluateOnSerialized,
+ const DDS_ContentFilterWriterEvaluateFunction writerEvaluateOnSerialized,
+ const DDS_ContentFilterQueryFunction query,
+ DDS_Boolean isBuiltin);
+}
+#endif // RMW_CONNEXT_BUILTIN_CFT_COMPATIBILITY_MODE
+
+#endif // RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO
+
+#endif // RMW_CONNEXTDDS__CUSTOM_SQL_FILTER_HPP_

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/dds_api_ndds.hpp

@@ -19,18 +19,6 @@
 
 #include "rcutils/types.h"
 
-DDS_SEQUENCE(RMW_Connext_Uint8ArrayPtrSeq, rcutils_uint8_array_t *);
-
-typedef RMW_Connext_Uint8ArrayPtrSeq RMW_Connext_UntypedSampleSeq;
-
-#define RMW_Connext_UntypedSampleSeq_INITIALIZER DDS_SEQUENCE_INITIALIZER
-
-#define DDS_UntypedSampleSeq_get_reference(seq_, i_) \
- *RMW_Connext_Uint8ArrayPtrSeq_get_reference(seq_, i_)
-
-#define DDS_UntypedSampleSeq_get_length(seq_) \
- RMW_Connext_Uint8ArrayPtrSeq_get_length(seq_)
-
 #if RMW_CONNEXT_DDS_API_PRO_LEGACY
 #ifndef RTIXCdrLong_MAX
 #define RTIXCdrLong_MAX 2147483647

rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp

@@ -483,6 +483,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
  ******************************************************************************/
@@ -311,6 +318,14 @@
 #define RMW_CONNEXT_LEGACY_RMW_COMPATIBILITY_MODE 1
 #endif /* RMW_CONNEXT_LEGACY_RMW_COMPATIBILITY_MODE */
 
+/******************************************************************************
+ * On windows, the custom SQL filter cannot be registered as "built-in", so we
+ * must enable some additional code to register it as a user plugin.
+ ******************************************************************************/
+#ifndef RMW_CONNEXT_BUILTIN_CFT_COMPATIBILITY_MODE
+#define RMW_CONNEXT_BUILTIN_CFT_COMPATIBILITY_MODE 0
+#endif /* RMW_CONNEXT_LEGACY_RMW_COMPATIBILITY_MODE */
+
 #include "resource_limits.hpp"
 
 #endif // RMW_CONNEXTDDS__STATIC_CONFIG_HPP_

rmw_connextdds_common/include/rmw_connextdds/type_support.hpp

@@ -167,11 +167,21 @@ class RMW_Connext_MessageTypeSupport
 
 struct RMW_Connext_Message
 {
- const void * user_data;
- bool serialized;
- RMW_Connext_MessageTypeSupport * type_support;
+ const void * user_data{nullptr};
+ bool serialized{false};
+ RMW_Connext_MessageTypeSupport * type_support{nullptr};
+ rcutils_uint8_array_t data_buffer;
 };
 
+rmw_ret_t
+RMW_Connext_Message_initialize(
+ RMW_Connext_Message * const self,
+ RMW_Connext_MessageTypeSupport * const type_support,
+ const size_t data_buffer_size);
+
+void
+RMW_Connext_Message_finalize(RMW_Connext_Message * const self);
+
 class RMW_Connext_ServiceTypeSupportWrapper
 {
 public:
@@ -276,5 +286,19 @@ class RMW_Connext_ServiceTypeSupportWrapper
  const rosidl_service_type_support_t * const type_supports);
 };
 
+#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO
+DDS_SEQUENCE(RMW_Connext_MessagePtrSeq, RMW_Connext_Message *);
+
+typedef RMW_Connext_MessagePtrSeq RMW_Connext_UntypedSampleSeq;
+
+#define RMW_Connext_UntypedSampleSeq_INITIALIZER DDS_SEQUENCE_INITIALIZER
+
+#define DDS_UntypedSampleSeq_get_reference(seq_, i_) \
+ *RMW_Connext_MessagePtrSeq_get_reference(seq_, i_)
+
+#define DDS_UntypedSampleSeq_get_length(seq_) \
+ RMW_Connext_MessagePtrSeq_get_length(seq_)
+
+#endif // RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO
 
 #endif // RMW_CONNEXTDDS__TYPE_SUPPORT_HPP_

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;
  }