add content-filtered-topic interfaces

rclcpp/CMakeLists.txt

@@ -2,6 +2,11 @@ cmake_minimum_required(VERSION 3.12)
 
 project(rclcpp)
 
+option(DEFINE_CONTENT_FILTER "Content filter feature support" ON)
+if(DEFINE_CONTENT_FILTER)
+  add_definitions(-DCONTENT_FILTER_ENABLE)
+endif()
+
 find_package(Threads REQUIRED)
 
 find_package(ament_cmake_ros REQUIRED)

rclcpp/include/rclcpp/subscription_base.hpp

@@ -491,6 +491,47 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
     event_handlers_[event_type]->clear_on_ready_callback();
   }
 
+#ifdef CONTENT_FILTER_ENABLE
+  /// Check if content filtered topic feature of the subscription instance is enabled.
+  /**
+   * \return boolean flag indicating if the content filtered topic of this subscription is enabled.
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_cft_enabled() const;
+
+  /// Set the filter expression and expression parameters for the subscription.
+  /**
+   * \param[in] filter_expression An filter expression to set.
+   *   \sa SubscriptionOptionsBase::ContentFilterOptions::filter_expression
+   *   Use empty string ("") can reset (or clear) the content filter setting of a subscription.
+   * \param[in] expression_parameters Array of expression parameters to set.
+   *   \sa SubscriptionOptionsBase::ContentFilterOptions::expression_parameters
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  set_content_filter(
+    const std::string & filter_expression,
+    const std::vector<std::string> & expression_parameters = {});
+
+  /// Get the filter expression and expression parameters for the subscription.
+  /**
+   * \param[out] filter_expression An filter expression to get.
+   * \param[out] expression_parameters Array of expression parameters to get.
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  get_content_filter(
+    std::string & filter_expression,
+    std::vector<std::string> & expression_parameters) const;
+#endif  // CONTENT_FILTER_ENABLE
+
 protected:
   template<typename EventCallbackT>
   void

rclcpp/include/rclcpp/subscription_options.hpp

@@ -81,6 +81,22 @@ struct SubscriptionOptionsBase
   TopicStatisticsOptions topic_stats_options;
 
   QosOverridingOptions qos_overriding_options;
+
+#ifdef CONTENT_FILTER_ENABLE
+  /// Options to configure content filtered topic in the subscription.
+  struct ContentFilterOptions
+  {
+    /// Filter expression is similar to the WHERE part of an SQL clause
+    std::string filter_expression;
+    /**
+     * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0)
+     * in the filter_expression
+     */
+    std::vector<std::string> expression_parameters;
+  };
+
+  ContentFilterOptions content_filter_options;
+#endif  // CONTENT_FILTER_ENABLE
 };
 
 /// Structure containing optional configuration for Subscriptions.
@@ -123,6 +139,22 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
       rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options);
     }
 
+#ifdef CONTENT_FILTER_ENABLE
+    // Copy content_filter_options into rcl_subscription_options.
+    if (!content_filter_options.filter_expression.empty()) {
+      std::vector<const char *> cstrings =
+        get_c_vector_string(content_filter_options.expression_parameters);
+      rcl_ret_t ret = rcl_subscription_options_set_content_filter_options(
+        get_c_string(content_filter_options.filter_expression),
+        cstrings.size(),
+        cstrings.data(),
+        &result);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(
+          ret, "failed to set content_filter_options");
+      }
+    }
+#endif  // CONTENT_FILTER_ENABLE
     return result;
   }
 

rclcpp/include/rclcpp/utilities.hpp

@@ -321,6 +321,15 @@ RCLCPP_PUBLIC
 const char *
 get_c_string(const std::string & string_in);
 
+/// Return the std::vector of C string from the given std::vector<std::string>.
+/**
+ * \param[in] string_in is a std::string
+ * \return the std::vector of C string from the std::vector<std::string>
+ */
+RCLCPP_PUBLIC
+std::vector<const char *>
+get_c_vector_string(const std::vector<std::string> & strings);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/src/rclcpp/subscription_base.cpp

@@ -20,6 +20,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
@@ -354,3 +356,89 @@ SubscriptionBase::set_on_new_message_callback(
     throw_from_rcl_error(ret, "failed to set the on new message callback for subscription");
   }
 }
+
+#ifdef CONTENT_FILTER_ENABLE
+bool
+SubscriptionBase::is_cft_enabled() const
+{
+  return rcl_subscription_is_cft_enabled(subscription_handle_.get());
+}
+
+void
+SubscriptionBase::set_content_filter(
+  const std::string & filter_expression,
+  const std::vector<std::string> & expression_parameters)
+{
+  rcl_subscription_content_filter_options_t options =
+    rcl_get_zero_initialized_subscription_content_filter_options();
+
+  std::vector<const char *> cstrings =
+    get_c_vector_string(expression_parameters);
+  rcl_ret_t ret = rcl_subscription_content_filter_options_init(
+    get_c_string(filter_expression),
+    cstrings.size(),
+    cstrings.data(),
+    &options);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to init subscription content_filtered_topic option");
+  }
+  RCPPUTILS_SCOPE_EXIT(
+  {
+    rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options);
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "Failed to fini subscription content_filtered_topic option: %s",
+        rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+  });
+
+  ret = rcl_subscription_set_content_filter(
+    subscription_handle_.get(),
+    &options);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters");
+  }
+}
+
+void
+SubscriptionBase::get_content_filter(
+  std::string & filter_expression,
+  std::vector<std::string> & expression_parameters) const
+{
+  rcl_subscription_content_filter_options_t options =
+    rcl_get_zero_initialized_subscription_content_filter_options();
+
+  rcl_ret_t ret = rcl_subscription_get_content_filter(
+    subscription_handle_.get(),
+    &options);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters");
+  }
+
+  RCPPUTILS_SCOPE_EXIT(
+  {
+    rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options);
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "Failed to fini subscription content_filtered_topic option: %s",
+        rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+  });
+
+  rmw_subscription_content_filter_options_t & content_filter_options =
+    options.rmw_subscription_content_filter_options;
+  filter_expression = content_filter_options.filter_expression;
+
+  for (size_t i = 0; i < content_filter_options.expression_parameters.size; ++i) {
+    expression_parameters.push_back(
+      content_filter_options.expression_parameters.data[i]);
+  }
+}
+#endif  // CONTENT_FILTER_ENABLE

rclcpp/src/rclcpp/utilities.cpp

@@ -214,4 +214,17 @@ get_c_string(const std::string & string_in)
   return string_in.c_str();
 }
 
+std::vector<const char *>
+get_c_vector_string(const std::vector<std::string> & strings)
+{
+  std::vector<const char *> cstrings;
+  cstrings.reserve(strings.size());
+
+  for (size_t i = 0; i < strings.size(); ++i) {
+    cstrings.push_back(strings[i].c_str());
+  }
+
+  return cstrings;
+}
+
 }  // namespace rclcpp

rclcpp/test/rclcpp/CMakeLists.txt

@@ -733,3 +733,22 @@ ament_add_gtest(test_graph_listener test_graph_listener.cpp)
 if(TARGET test_graph_listener)
   target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick)
 endif()
+
+if(DEFINE_CONTENT_FILTER)
+  function(test_subscription_content_filter_for_rmw_implementation)
+    set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
+    ament_add_gmock(test_subscription_content_filter${target_suffix}
+      test_subscription_content_filter.cpp
+      ENV ${rmw_implementation_env_var}
+    )
+    if(TARGET test_subscription_content_filter${target_suffix})
+      target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick)
+      ament_target_dependencies(test_subscription_content_filter${target_suffix}
+        "rcpputils"
+        "rosidl_typesupport_cpp"
+        "test_msgs"
+      )
+    endif()
+  endfunction()
+  call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation)
+endif()