15.4.0

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+15.4.0 (2022-04-05)
+-------------------
+* add take_data_by_entity_id API to waitable (`#1892 <https://github.com/ros2/rclcpp/issues/1892>`_)
+* add content-filtered-topic interfaces (`#1561 <https://github.com/ros2/rclcpp/issues/1561>`_)
+* Contributors: Alberto Soragna, Chen Lihui
+
 15.3.0 (2022-03-30)
 -------------------
 * [NodeParameters] Set name in param info pre-check (`#1908 <https://github.com/ros2/rclcpp/issues/1908>`_)

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/experimental/subscription_intra_process_base.hpp

@@ -38,7 +38,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
 
-  enum class EntityType
+  enum class EntityType : std::size_t
   {
     Subscription,
   };
@@ -68,6 +68,13 @@ public:
   std::shared_ptr<void>
   take_data() override = 0;
 
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
   void
   execute(std::shared_ptr<void> & data) override = 0;
 

rclcpp/include/rclcpp/qos_event.hpp

@@ -89,7 +89,7 @@ public:
 class QOSEventHandlerBase : public Waitable
 {
 public:
-  enum class EntityType
+  enum class EntityType : std::size_t
   {
     Event,
   };
@@ -259,6 +259,13 @@ public:
     return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
   }
 
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
   /// Execute any entities of the Waitable that are ready.
   void
   execute(std::shared_ptr<void> & data) override

rclcpp/include/rclcpp/subscription_base.hpp

@@ -39,6 +39,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -491,6 +492,42 @@ public:
     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 A filter expression to set.
+   *   \sa ContentFilterOptions::filter_expression
+   *   An empty string ("") will clear the content filter setting of the subscription.
+   * \param[in] expression_parameters Array of expression parameters to set.
+   *   \sa ContentFilterOptions::expression_parameters
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  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.
+  /**
+   * \return rclcpp::ContentFilterOptions The content filter options to get.
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ContentFilterOptions
+  get_content_filter() const;
+#endif  // CONTENT_FILTER_ENABLE
+
 protected:
   template<typename EventCallbackT>
   void

rclcpp/include/rclcpp/subscription_content_filter_options.hpp

@@ -0,0 +1,38 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// 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 RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_
+#define RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_
+
+#include <string>
+#include <vector>
+
+namespace rclcpp
+{
+
+/// 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. The maximum expression_parameters size is 100.
+   */
+  std::vector<std::string> expression_parameters;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_

rclcpp/include/rclcpp/subscription_options.hpp

@@ -28,6 +28,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/qos_overriding_options.hpp"
+#include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/topic_statistics_state.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -81,6 +82,8 @@ struct SubscriptionOptionsBase
   TopicStatisticsOptions topic_stats_options;
 
   QosOverridingOptions qos_overriding_options;
+
+  ContentFilterOptions content_filter_options;
 };
 
 /// Structure containing optional configuration for Subscriptions.
@@ -123,6 +126,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] strings_in is a std::vector of 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_in);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/include/rclcpp/waitable.hpp

@@ -160,6 +160,24 @@ public:
   std::shared_ptr<void>
   take_data() = 0;
 
+  /// Take the data so that it can be consumed with `execute`.
+  /**
+   * This function allows to specify an entity ID to take the data from.
+   * Entity IDs are identifiers that can be defined by waitable-derived
+   * classes that are composed of several distinct entities.
+   * The main use-case is in conjunction with the listener APIs.
+   *
+   * \param[in] id the id of the entity from which to take
+   * \returns the type-erased data taken from entity specified
+   *
+   * \sa rclcpp::Waitable::take_data
+   * \sa rclcpp::Waitable::set_on_ready_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id);
+
   /// Execute data that is passed in.
   /**
    * Before calling this method, the Waitable should be added to a wait set,

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>15.3.0</version>
+  <version>15.4.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>

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,93 @@ 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(
+    subscription_handle_.get(),
+    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(
+      subscription_handle_.get(), &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");
+  }
+}
+
+rclcpp::ContentFilterOptions
+SubscriptionBase::get_content_filter() const
+{
+  rclcpp::ContentFilterOptions ret_options;
+  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(
+      subscription_handle_.get(), &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;
+  ret_options.filter_expression = content_filter_options.filter_expression;
+
+  for (size_t i = 0; i < content_filter_options.expression_parameters.size; ++i) {
+    ret_options.expression_parameters.push_back(
+      content_filter_options.expression_parameters.data[i]);
+  }
+
+  return ret_options;
+}
+#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_in)
+{
+  std::vector<const char *> cstrings;
+  cstrings.reserve(strings_in.size());
+
+  for (size_t i = 0; i < strings_in.size(); ++i) {
+    cstrings.push_back(strings_in[i].c_str());
+  }
+
+  return cstrings;
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/waitable.cpp

@@ -54,6 +54,15 @@ Waitable::get_number_of_ready_guard_conditions()
   return 0u;
 }
 
+std::shared_ptr<void>
+Waitable::take_data_by_entity_id(size_t id)
+{
+  (void)id;
+  throw std::runtime_error(
+          "Custom waitables should override take_data_by_entity_id "
+          "if they want to use it.");
+}
+
 bool
 Waitable::exchange_in_use_by_wait_set_state(bool in_use_state)
 {

rclcpp/test/rclcpp/CMakeLists.txt

@@ -733,3 +733,23 @@ 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}
+      TIMEOUT 120
+    )
+    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()

rclcpp/test/rclcpp/test_subscription_content_filter.cpp

@@ -0,0 +1,312 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_for_message.hpp"
+
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/basic_types.hpp"
+
+#ifdef RMW_IMPLEMENTATION
+# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
+# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
+#else
+# define CLASSNAME(NAME, SUFFIX) NAME
+#endif
+
+class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("test_content_filter_node", "/ns");
+    context = node->get_node_options().context();
+    qos.reliable().transient_local();
+
+    auto options = rclcpp::SubscriptionOptions();
+    options.content_filter_options.filter_expression = filter_expression_init;
+    options.content_filter_options.expression_parameters = expression_parameters_1;
+
+    auto callback = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
+    sub = node->create_subscription<test_msgs::msg::BasicTypes>(
+      "content_filter_topic", qos, callback, options);
+  }
+
+  void TearDown()
+  {
+    sub.reset();
+    node.reset();
+  }
+
+  template<typename Condition, typename Duration>
+  bool wait_for(const Condition & condition, const Duration & timeout)
+  {
+    using clock = std::chrono::system_clock;
+    auto start = clock::now();
+    while (!condition()) {
+      if ((clock::now() - start) > timeout) {
+        return false;
+      }
+      rclcpp::spin_some(node);
+    }
+    return true;
+  }
+
+protected:
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Context::SharedPtr context;
+  rclcpp::QoS qos{rclcpp::KeepLast{10}};
+  rclcpp::Subscription<test_msgs::msg::BasicTypes>::SharedPtr sub;
+
+  std::string filter_expression_init = "int32_value = %0";
+  std::vector<std::string> expression_parameters_1 = {"3"};
+  std::vector<std::string> expression_parameters_2 = {"4"};
+};
+
+bool operator==(const test_msgs::msg::BasicTypes & m1, const test_msgs::msg::BasicTypes & m2)
+{
+  return m1.bool_value == m2.bool_value &&
+         m1.byte_value == m2.byte_value &&
+         m1.char_value == m2.char_value &&
+         m1.float32_value == m2.float32_value &&
+         m1.float64_value == m2.float64_value &&
+         m1.int8_value == m2.int8_value &&
+         m1.uint8_value == m2.uint8_value &&
+         m1.int16_value == m2.int16_value &&
+         m1.uint16_value == m2.uint16_value &&
+         m1.int32_value == m2.int32_value &&
+         m1.uint32_value == m2.uint32_value &&
+         m1.int64_value == m2.int64_value &&
+         m1.uint64_value == m2.uint64_value;
+}
+
+TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), is_cft_enabled) {
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_subscription_is_cft_enabled, false);
+    EXPECT_FALSE(sub->is_cft_enabled());
+  }
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_subscription_is_cft_enabled, true);
+    EXPECT_TRUE(sub->is_cft_enabled());
+  }
+}
+
+TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter_error) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR);
+
+  rclcpp::ContentFilterOptions options;
+  EXPECT_THROW(
+    options = sub->get_content_filter(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter_error) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR);
+
+  std::string filter_expression = "int32_value = %0";
+  std::string expression_parameter = "100";
+  EXPECT_THROW(
+    sub->set_content_filter(filter_expression, {expression_parameter}),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter) {
+  rclcpp::ContentFilterOptions options;
+
+  if (sub->is_cft_enabled()) {
+    EXPECT_NO_THROW(
+      options = sub->get_content_filter());
+
+    EXPECT_EQ(options.filter_expression, filter_expression_init);
+    EXPECT_EQ(options.expression_parameters, expression_parameters_1);
+  } else {
+    EXPECT_THROW(
+      options = sub->get_content_filter(),
+      rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter) {
+  if (sub->is_cft_enabled()) {
+    EXPECT_NO_THROW(
+      sub->set_content_filter(filter_expression_init, expression_parameters_2));
+  } else {
+    EXPECT_THROW(
+      sub->set_content_filter(filter_expression_init, expression_parameters_2),
+      rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_begin) {
+  using namespace std::chrono_literals;
+  {
+    test_msgs::msg::BasicTypes msg;
+    rclcpp::MessageInfo msg_info;
+    EXPECT_FALSE(sub->take(msg, msg_info));
+  }
+  {
+    rclcpp::PublisherOptions po;
+    auto pub = node->create_publisher<test_msgs::msg::BasicTypes>("content_filter_topic", qos, po);
+
+    auto connected = [pub, sub = this->sub]() -> bool {
+        return pub->get_subscription_count() && sub->get_publisher_count();
+      };
+    ASSERT_TRUE(wait_for(connected, 10s));
+
+    test_msgs::msg::BasicTypes original_message;
+    original_message.int32_value = 3;
+    pub->publish(original_message);
+
+    test_msgs::msg::BasicTypes output_message;
+    bool receive = wait_for_message(output_message, sub, context, 10s);
+    EXPECT_TRUE(receive);
+    EXPECT_EQ(original_message, output_message);
+
+    if (sub->is_cft_enabled()) {
+      EXPECT_NO_THROW(
+        sub->set_content_filter(filter_expression_init, expression_parameters_2));
+      // waiting to allow for filter propagation
+      std::this_thread::sleep_for(std::chrono::seconds(10));
+
+      test_msgs::msg::BasicTypes original_message;
+      original_message.int32_value = 3;
+      pub->publish(original_message);
+
+      test_msgs::msg::BasicTypes output_message;
+      bool receive = wait_for_message(output_message, sub, context, 10s);
+      EXPECT_FALSE(receive);
+    }
+  }
+}
+
+TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_later) {
+  using namespace std::chrono_literals;
+  {
+    test_msgs::msg::BasicTypes msg;
+    rclcpp::MessageInfo msg_info;
+    EXPECT_FALSE(sub->take(msg, msg_info));
+  }
+  {
+    rclcpp::PublisherOptions po;
+    auto pub = node->create_publisher<test_msgs::msg::BasicTypes>("content_filter_topic", qos, po);
+
+    auto connected = [pub, sub = this->sub]() -> bool {
+        return pub->get_subscription_count() && sub->get_publisher_count();
+      };
+    ASSERT_TRUE(wait_for(connected, 10s));
+
+    test_msgs::msg::BasicTypes original_message;
+    original_message.int32_value = 4;
+    pub->publish(original_message);
+
+    test_msgs::msg::BasicTypes output_message;
+    bool receive = wait_for_message(output_message, sub, context, 10s);
+    if (sub->is_cft_enabled()) {
+      EXPECT_FALSE(receive);
+    } else {
+      EXPECT_TRUE(receive);
+      EXPECT_EQ(original_message, output_message);
+    }
+
+    if (sub->is_cft_enabled()) {
+      EXPECT_NO_THROW(
+        sub->set_content_filter(filter_expression_init, expression_parameters_2));
+      // waiting to allow for filter propagation
+      std::this_thread::sleep_for(std::chrono::seconds(10));
+
+      test_msgs::msg::BasicTypes original_message;
+      original_message.int32_value = 4;
+      pub->publish(original_message);
+
+      test_msgs::msg::BasicTypes output_message;
+      bool receive = wait_for_message(output_message, sub, context, 10s);
+      EXPECT_TRUE(receive);
+      EXPECT_EQ(original_message, output_message);
+    }
+  }
+}
+
+TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_reset) {
+  using namespace std::chrono_literals;
+  {
+    test_msgs::msg::BasicTypes msg;
+    rclcpp::MessageInfo msg_info;
+    EXPECT_FALSE(sub->take(msg, msg_info));
+  }
+  {
+    rclcpp::PublisherOptions po;
+    auto pub = node->create_publisher<test_msgs::msg::BasicTypes>("content_filter_topic", qos, po);
+
+    auto connected = [pub, sub = this->sub]() -> bool {
+        return pub->get_subscription_count() && sub->get_publisher_count();
+      };
+    ASSERT_TRUE(wait_for(connected, 10s));
+
+    test_msgs::msg::BasicTypes original_message;
+    original_message.int32_value = 4;
+    pub->publish(original_message);
+
+    test_msgs::msg::BasicTypes output_message;
+    bool receive = wait_for_message(output_message, sub, context, 10s);
+    if (sub->is_cft_enabled()) {
+      EXPECT_FALSE(receive);
+    } else {
+      EXPECT_TRUE(receive);
+      EXPECT_EQ(original_message, output_message);
+    }
+
+    if (sub->is_cft_enabled()) {
+      EXPECT_NO_THROW(
+        sub->set_content_filter(""));
+      // waiting to allow for filter propagation
+      std::this_thread::sleep_for(std::chrono::seconds(10));
+
+      test_msgs::msg::BasicTypes original_message;
+      original_message.int32_value = 4;
+      pub->publish(original_message);
+
+      test_msgs::msg::BasicTypes output_message;
+      bool receive = wait_for_message(output_message, sub, context, 10s);
+      EXPECT_TRUE(receive);
+      EXPECT_EQ(original_message, output_message);
+    }
+  }
+}

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,11 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+15.4.0 (2022-04-05)
+-------------------
+* add take_data_by_entity_id API to waitable (`#1892 <https://github.com/ros2/rclcpp/issues/1892>`_)
+* Contributors: Alberto Soragna
+
 15.3.0 (2022-03-30)
 -------------------
 

rclcpp_action/include/rclcpp_action/client.hpp

@@ -66,7 +66,7 @@ public:
   virtual ~ClientBase();
 
   /// Enum to identify entities belonging to the action client
-  enum class EntityType
+  enum class EntityType : std::size_t
   {
     GoalClient,
     ResultClient,
@@ -134,6 +134,11 @@ public:
   std::shared_ptr<void>
   take_data() override;
 
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override;
+
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void

rclcpp_action/include/rclcpp_action/server.hpp

@@ -72,7 +72,7 @@ class ServerBase : public rclcpp::Waitable
 {
 public:
   /// Enum to identify entities belonging to the action server
-  enum class EntityType
+  enum class EntityType : std::size_t
   {
     GoalService,
     ResultService,
@@ -131,6 +131,10 @@ public:
   std::shared_ptr<void>
   take_data() override;
 
+  RCLCPP_ACTION_PUBLIC
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override;
+
   /// Act on entities in the wait set which are ready to be acted upon.
   /// \internal
   RCLCPP_ACTION_PUBLIC

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>15.3.0</version>
+  <version>15.4.0</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>

rclcpp_action/src/client.cpp

@@ -589,6 +589,31 @@ ClientBase::take_data()
   }
 }
 
+std::shared_ptr<void>
+ClientBase::take_data_by_entity_id(size_t id)
+{
+  // Mark as ready the entity from which we want to take data
+  switch (static_cast<EntityType>(id)) {
+    case EntityType::GoalClient:
+      pimpl_->is_goal_response_ready = true;
+      break;
+    case EntityType::ResultClient:
+      pimpl_->is_result_response_ready = true;
+      break;
+    case EntityType::CancelClient:
+      pimpl_->is_cancel_response_ready = true;
+      break;
+    case EntityType::FeedbackSubscription:
+      pimpl_->is_feedback_ready = true;
+      break;
+    case EntityType::StatusSubscription:
+      pimpl_->is_status_ready = true;
+      break;
+  }
+
+  return take_data();
+}
+
 void
 ClientBase::execute(std::shared_ptr<void> & data)
 {

rclcpp_action/src/server.cpp

@@ -268,6 +268,25 @@ ServerBase::take_data()
   }
 }
 
+std::shared_ptr<void>
+ServerBase::take_data_by_entity_id(size_t id)
+{
+  // Mark as ready the entity from which we want to take data
+  switch (static_cast<EntityType>(id)) {
+    case EntityType::GoalService:
+      pimpl_->goal_request_ready_ = true;
+      break;
+    case EntityType::ResultService:
+      pimpl_->result_request_ready_ = true;
+      break;
+    case EntityType::CancelService:
+      pimpl_->cancel_request_ready_ = true;
+      break;
+  }
+
+  return take_data();
+}
+
 void
 ServerBase::execute(std::shared_ptr<void> & data)
 {
@@ -398,6 +417,7 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
   }
   auto request = std::get<1>(*shared_ptr);
   auto request_header = std::get<2>(*shared_ptr);
+  pimpl_->cancel_request_ready_ = false;
 
   // Convert c++ message to C message
   rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+15.4.0 (2022-04-05)
+-------------------
+
 15.3.0 (2022-03-30)
 -------------------
 

rclcpp_components/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_components</name>
-  <version>15.3.0</version>
+  <version>15.4.0</version>
   <description>Package containing tools for dynamically loadable components</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+15.4.0 (2022-04-05)
+-------------------
+
 15.3.0 (2022-03-30)
 -------------------
 

rclcpp_lifecycle/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_lifecycle</name>
-  <version>15.3.0</version>
+  <version>15.4.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>