call rcl content filter fallback

rcl/CMakeLists.txt

@@ -5,6 +5,7 @@ project(rcl)
 find_package(ament_cmake_ros REQUIRED)
 
 find_package(libyaml_vendor REQUIRED)
+find_package(rcl_content_filter_fallback REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rcl_logging_interface REQUIRED)
 find_package(rcl_yaml_param_parser REQUIRED)
@@ -85,6 +86,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 target_link_libraries(${PROJECT_NAME} PUBLIC
   # TODO(clalancette): rcl_interfaces should be PRIVATE, but downstream depends on it for now
   ${rcl_interfaces_TARGETS}
+  rcl_content_filter_fallback::rcl_content_filter_fallback
   # TODO(clalancette): rcl_logging_interface should be PRIVATE, but downstream depends on it for now
   rcl_logging_interface::rcl_logging_interface
   rcl_yaml_param_parser::rcl_yaml_param_parser
@@ -136,6 +138,7 @@ ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(${PROJECT_NAME})
 
 ament_export_dependencies(ament_cmake)
+ament_export_dependencies(rcl_content_filter_fallback)
 # TODO(clalancette): rcl_interfaces shouldn't be exported, but downstream depends on it for now
 ament_export_dependencies(rcl_interfaces)
 # TODO(clalancette): rcl_logging_interface shouldn't be exported, but downstream depends on it for now

rcl/package.xml

@@ -20,6 +20,7 @@
   <build_export_depend>rmw</build_export_depend>
 
   <depend>libyaml_vendor</depend>
+  <depend>rcl_content_filter_fallback</depend>
   <depend>rcl_interfaces</depend>
   <depend>rcl_logging_interface</depend>
   <depend>rcl_logging_spdlog</depend> <!-- the default logging impl -->

rcl/src/rcl/subscription.c

@@ -21,6 +21,8 @@ extern "C"
 
 #include <stdio.h>
 
+#include "rcl_content_filter_fallback/rcl_content_filter_fallback.h"
+
 #include "rcl/error_handling.h"
 #include "rcl/node.h"
 #include "rcl/node_type_cache.h"
@@ -45,6 +47,54 @@ rcl_get_zero_initialized_subscription()
   return null_subscription;
 }
 
+static
+bool
+rcl_subscription_rcl_content_filter_fallback_set(
+  const rcl_subscription_t * subscription,
+  const rmw_subscription_content_filter_options_t * options)
+{
+  if (!subscription->impl->rcl_content_filter_fallback) {
+    subscription->impl->rcl_content_filter_fallback =
+      rcl_content_filter_fallback_create(subscription->impl->type_support);
+    if (!subscription->impl->rcl_content_filter_fallback) {
+      RCL_SET_ERROR_MSG("Failed to create rcl content filter fallback");
+      return false;
+    }
+    RCUTILS_LOG_DEBUG_NAMED(
+      ROS_PACKAGE_NAME, "rcl content filter fallback is created for topic '%s'",
+      rcl_subscription_get_topic_name(subscription));
+  }
+
+  if (!rcl_content_filter_fallback_set(
+      subscription->impl->rcl_content_filter_fallback,
+      options))
+  {
+    RCL_SET_ERROR_MSG("Failed to set options for rcl content filter fallback");
+    return false;
+  }
+
+  return true;
+}
+
+static
+bool
+rcl_subscription_rcl_content_filter_fallback_is_relevant(
+  const rcl_subscription_t * subscription,
+  void * data,
+  bool serialized)
+{
+  if (subscription->impl->rcl_content_filter_fallback &&
+    rcl_content_filter_fallback_is_enabled(subscription->impl->rcl_content_filter_fallback))
+  {
+    return rcl_content_filter_fallback_evaluate(
+      subscription->impl->rcl_content_filter_fallback,
+      data,
+      serialized);
+  }
+
+  return true;
+}
+
 rcl_ret_t
 rcl_subscription_init(
   rcl_subscription_t * subscription,
@@ -124,6 +174,23 @@ rcl_subscription_init(
   // options
   subscription->impl->options = *options;
 
+  subscription->impl->type_support = type_support;
+
+  if (options->rmw_subscription_options.content_filter_options) {
+    // Content filter topic not supported (or not enabled as some failed cases) on rmw
+    // implementation.
+    // TODO(iuhilnehc-ynos): enable rcl content filter fallback with an environment variable
+    // (e.g. FORCE_RCL_CONTENT_FILTER) regardless of whether cft is enabled on RMW implementation.
+    if (!subscription->impl->rmw_handle->is_cft_enabled) {
+      if (!rcl_subscription_rcl_content_filter_fallback_set(
+          subscription,
+          options->rmw_subscription_options.content_filter_options))
+      {
+        goto fail;
+      }
+    }
+  }
+
   if (RCL_RET_OK != rcl_node_type_cache_register_type(
       node, type_support->get_type_hash_func(type_support),
       type_support->get_type_description_func(type_support),
@@ -163,6 +230,10 @@ rcl_subscription_init(
       RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
     }
 
+    if (subscription->impl->rcl_content_filter_fallback) {
+      rcl_content_filter_fallback_destroy(subscription->impl->rcl_content_filter_fallback);
+    }
+
     allocator->deallocate(subscription->impl, allocator->state);
     subscription->impl = NULL;
   }
@@ -206,6 +277,10 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node)
       result = RCL_RET_ERROR;
     }
 
+    if (subscription->impl->rcl_content_filter_fallback) {
+      rcl_content_filter_fallback_destroy(subscription->impl->rcl_content_filter_fallback);
+    }
+
     if (
       ROSIDL_TYPE_HASH_VERSION_UNSET != subscription->impl->type_hash.version &&
       RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &subscription->impl->type_hash))
@@ -470,7 +545,9 @@ rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription)
   if (!rcl_subscription_is_valid(subscription)) {
     return false;
   }
-  return subscription->impl->rmw_handle->is_cft_enabled;
+  return subscription->impl->rmw_handle->is_cft_enabled ||
+         (subscription->impl->rcl_content_filter_fallback &&
+         rcl_content_filter_fallback_is_enabled(subscription->impl->rcl_content_filter_fallback));
 }
 
 rcl_ret_t
@@ -492,8 +569,13 @@ rcl_subscription_set_content_filter(
     &options->rmw_subscription_content_filter_options);
 
   if (ret != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
-    return rcl_convert_rmw_ret_to_rcl_ret(ret);
+    rcl_reset_error();
+    if (!rcl_subscription_rcl_content_filter_fallback_set(
+        subscription,
+        &options->rmw_subscription_content_filter_options))
+    {
+      return RMW_RET_ERROR;
+    }
   }
 
   // copy options into subscription_options
@@ -527,8 +609,20 @@ rcl_subscription_get_content_filter(
     subscription->impl->rmw_handle,
     allocator,
     &options->rmw_subscription_content_filter_options);
-
-  return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+  // If options can be get from rmw implementation, it's unnecessary to get them from rcl
+  // content filter fallback.
+  if (rmw_ret != RMW_RET_OK) {
+    rcl_reset_error();
+    if (!rcl_content_filter_fallback_get(
+        subscription->impl->rcl_content_filter_fallback,
+        allocator,
+        &options->rmw_subscription_content_filter_options))
+    {
+      RCL_SET_ERROR_MSG("Failed to get options from rcl content filter fallback");
+      return RMW_RET_ERROR;
+    }
+  }
+  return RMW_RET_OK;
 }
 
 rcl_ret_t
@@ -563,6 +657,16 @@ rcl_take(
   if (!taken) {
     return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
   }
+
+  // filter ros message with rcl content filter fallback
+  if (!rcl_subscription_rcl_content_filter_fallback_is_relevant(
+      subscription,
+      ros_message,
+      false))
+  {
+    return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
+  }
+
   return RCL_RET_OK;
 }
 
@@ -642,6 +746,16 @@ rcl_take_serialized_message(
   if (!taken) {
     return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
   }
+
+  // filter ros message with rcl content filter fallback
+  if (!rcl_subscription_rcl_content_filter_fallback_is_relevant(
+      subscription,
+      serialized_message,
+      true))
+  {
+    return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
+  }
+
   return RCL_RET_OK;
 }
 
@@ -710,6 +824,16 @@ rcl_take_loaned_message(
   if (!taken) {
     return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
   }
+
+  // filter ros message with rcl content filter fallback
+  if (!rcl_subscription_rcl_content_filter_fallback_is_relevant(
+      subscription,
+      *loaned_message,
+      false))
+  {
+    return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
+  }
+
   return RCL_RET_OK;
 }
 

rcl/src/rcl/subscription_impl.h

@@ -18,12 +18,15 @@
 #include "rmw/rmw.h"
 
 #include "rcl/subscription.h"
+#include "rosidl_runtime_c/message_type_support_struct.h"
 
 struct rcl_subscription_impl_s
 {
   rcl_subscription_options_t options;
   rmw_qos_profile_t actual_qos;
   rmw_subscription_t * rmw_handle;
+  void * rcl_content_filter_fallback;
+  const rosidl_message_type_support_t * type_support;
   rosidl_type_hash_t type_hash;
 };