Add coverage for wait_set_policies (#1316)

* Add mocking utils for rclcpp

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Add coverage for wait_set_policies

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Address PR feedback

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Fix windows issues

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Add test comment

Signed-off-by: Stephen Brawner <brawner@gmail.com>

rclcpp/package.xml

@@ -35,6 +35,7 @@
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>mimick_vendor</test_depend>
   <test_depend>rmw</test_depend>
   <test_depend>rmw_implementation_cmake</test_depend>
   <test_depend>rosidl_default_generators</test_depend>

rclcpp/test/CMakeLists.txt

@@ -566,6 +566,30 @@ if(TARGET test_subscription_options)
   target_link_libraries(test_subscription_options ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_dynamic_storage rclcpp/wait_set_policies/test_dynamic_storage.cpp)
+if(TARGET test_dynamic_storage)
+  ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs")
+  target_link_libraries(test_dynamic_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_storage_policy_common rclcpp/wait_set_policies/test_storage_policy_common.cpp)
+if(TARGET test_storage_policy_common)
+  ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs")
+  target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_static_storage rclcpp/wait_set_policies/test_static_storage.cpp)
+if(TARGET test_static_storage)
+  ament_target_dependencies(test_static_storage "rcl" "test_msgs")
+  target_link_libraries(test_static_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_thread_safe_synchronization rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp)
+if(TARGET test_thread_safe_synchronization)
+  ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs")
+  target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp)
 if(TARGET test_rclcpp_gtest_macros)
   target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME})

rclcpp/test/mocking_utils/patch.hpp

@@ -0,0 +1,522 @@
+// Copyright 2020 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.
+
+// Original file taken from:
+// https://github.com/ros2/rcutils/blob/master/test/mocking_utils/patch.hpp
+
+#ifndef MOCKING_UTILS__PATCH_HPP_
+#define MOCKING_UTILS__PATCH_HPP_
+
+#define MOCKING_UTILS_SUPPORT_VA_LIST
+#if (defined(__aarch64__) || defined(__arm__) || defined(_M_ARM) || defined(__thumb__))
+// In ARM machines, va_list does not define comparison operators
+// nor the compiler allows defining them via operator overloads.
+// Thus, Mimick argument matching code will not compile.
+#undef MOCKING_UTILS_SUPPORT_VA_LIST
+#endif
+
+#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
+#include <cstdarg>
+#endif
+
+#include <functional>
+#include <string>
+#include <type_traits>
+#include <utility>
+
+#include "mimick/mimick.h"
+
+#include "rcutils/error_handling.h"
+#include "rcutils/macros.h"
+
+namespace mocking_utils
+{
+
+/// Mimick specific traits for each mocking_utils::Patch instance.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam SignatureT Type of the symbol to be patched.
+*/
+template<size_t ID, typename SignatureT>
+struct PatchTraits;
+
+/// Traits specialization for ReturnT(void) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ */
+template<size_t ID, typename ReturnT>
+struct PatchTraits<ID, ReturnT(void)>
+{
+  mmk_mock_define(mock_type, ReturnT);
+};
+
+/// Traits specialization for void(void) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ */
+template<size_t ID>
+struct PatchTraits<ID, void(void)>
+{
+  mmk_mock_define(mock_type, void);
+};
+
+/// Traits specialization for ReturnT(ArgT0) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgT0 Argument type.
+ */
+template<size_t ID, typename ReturnT, typename ArgT0>
+struct PatchTraits<ID, ReturnT(ArgT0)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0);
+};
+
+/// Traits specialization for void(ArgT0) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgT0 Argument type.
+ */
+template<size_t ID, typename ArgT0>
+struct PatchTraits<ID, void(ArgT0)>
+{
+  mmk_mock_define(mock_type, void, ArgT0);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ArgT0, typename ArgT1>
+struct PatchTraits<ID, void(ArgT0, ArgT1)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1, typename ArgT2>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ArgT0, typename ArgT1, typename ArgT2>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3, typename ArgT4>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
+/// free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3, typename ArgT4>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+{
+  mmk_mock_define(
+    mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5, typename ArgT6>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6);
+};
+
+/// Generic trampoline to wrap generalized callables in plain functions.
+/**
+ * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
+ * \tparam SignatureT Type of the symbol this trampoline replaces.
+ */
+template<size_t ID, typename SignatureT>
+struct Trampoline;
+
+/// Trampoline specialization for free functions.
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+struct Trampoline<ID, ReturnT(ArgTs...)>
+{
+  static ReturnT base(ArgTs... args)
+  {
+    return target(std::forward<ArgTs>(args)...);
+  }
+
+  static std::function<ReturnT(ArgTs...)> target;
+};
+
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+std::function<ReturnT(ArgTs...)>
+Trampoline<ID, ReturnT(ArgTs...)>::target;
+
+/// Setup trampoline with the given @p target.
+/**
+ * \param[in] target Callable that this trampoline will target.
+ * \return the plain base function of this trampoline.
+ *
+ * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
+ * \tparam SignatureT Type of the symbol this trampoline replaces.
+ */
+template<size_t ID, typename SignatureT>
+auto prepare_trampoline(std::function<SignatureT> target)
+{
+  Trampoline<ID, SignatureT>::target = target;
+  return Trampoline<ID, SignatureT>::base;
+}
+
+/// Patch class for binary API mocking
+/**
+ * Built on top of Mimick, to enable symbol mocking on a per dynamically
+ * linked binary object basis.
+ *
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam SignatureT Type of the symbol to be patched.
+ */
+template<size_t ID, typename SignatureT>
+class Patch;
+
+/// Patch specialization for ReturnT(ArgTs...) free functions.
+/**
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTs Argument types.
+ */
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+class Patch<ID, ReturnT(ArgTs...)>
+{
+public:
+  using mock_type = typename PatchTraits<ID, ReturnT(ArgTs...)>::mock_type;
+
+  /// Construct a patch.
+  /**
+   * \param[in] target Symbol target string, using Mimick syntax
+   *   i.e. "symbol(@scope)?", where scope may be "self" to target the current
+   *   binary, "lib:library_name" to target a given library, "file:path/to/library"
+   *   to target a given file, or "sym:other_symbol" to target the first library
+   *   that defines said symbol.
+   * \param[in] proxy An indirection to call the target function.
+   *   This indirection must ensure this call goes through the function's
+   *   trampoline, as setup by the dynamic linker.
+   * \return a mocking_utils::Patch instance.
+   */
+  explicit Patch(const std::string & target, std::function<ReturnT(ArgTs...)> proxy)
+  : target_(target), proxy_(proxy)
+  {
+  }
+
+  // Copy construction and assignment are disabled.
+  Patch(const Patch &) = delete;
+  Patch & operator=(const Patch &) = delete;
+
+  Patch(Patch && other)
+  {
+    mock_ = other.mock_;
+    other.mock_ = nullptr;
+  }
+
+  Patch & operator=(Patch && other)
+  {
+    if (mock_) {
+      mmk_reset(mock_);
+    }
+    mock_ = other.mock_;
+    other.mock_ = nullptr;
+  }
+
+  ~Patch()
+  {
+    if (mock_) {
+      mmk_reset(mock_);
+    }
+  }
+
+  /// Inject a @p replacement for the patched function.
+  Patch & then_call(std::function<ReturnT(ArgTs...)> replacement) &
+  {
+    replace_with(replacement);
+    return *this;
+  }
+
+  /// Inject a @p replacement for the patched function.
+  Patch && then_call(std::function<ReturnT(ArgTs...)> replacement) &&
+  {
+    replace_with(replacement);
+    return std::move(*this);
+  }
+
+private:
+  // Helper for template parameter pack expansion using `mmk_any`
+  // macro as pattern.
+  template<typename T>
+  T any() {return mmk_any(T);}
+
+  void replace_with(std::function<ReturnT(ArgTs...)> replacement)
+  {
+    if (mock_) {
+      throw std::logic_error("Cannot configure patch more than once");
+    }
+    auto type_erased_trampoline =
+      reinterpret_cast<mmk_fn>(prepare_trampoline<ID>(replacement));
+    auto MMK_MANGLE(mock_type, create) =
+      PatchTraits<ID, ReturnT(ArgTs...)>::MMK_MANGLE(mock_type, create);
+    mock_ = mmk_mock(target_.c_str(), mock_type);
+    mmk_when(proxy_(any<ArgTs>()...), .then_call = type_erased_trampoline);
+  }
+
+  mock_type mock_{nullptr};
+  std::string target_;
+  std::function<ReturnT(ArgTs...)> proxy_;
+};
+
+/// Make a patch for a `target` function.
+/**
+ * Useful for type deduction during \ref mocking_utils::Patch construction.
+ *
+ * \param[in] target Symbol target string, using Mimick syntax.
+ * \param[in] proxy An indirection to call the target function.
+ * \return a mocking_utils::Patch instance.
+ *
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam SignatureT Type of the function to be patched.
+ *
+ * \sa mocking_utils::Patch for further reference.
+ */
+template<size_t ID, typename SignatureT>
+auto make_patch(const std::string & target, std::function<SignatureT> proxy)
+{
+  return Patch<ID, SignatureT>(target, proxy);
+}
+
+/// Define a dummy operator `op` for a given `type`.
+/**
+ * Useful to enable patching functions that take arguments whose types
+ * do not define basic comparison operators, as required by Mimick.
+*/
+#define MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(type_, op) \
+  template<typename T> \
+  typename std::enable_if<std::is_same<T, type_>::value, bool>::type \
+  operator op(const T &, const T &) { \
+    return false; \
+  }
+
+/// Get the exact \ref mocking_utils::Patch type for a given `id` and `function`.
+/**
+ * Useful to avoid ignored attribute warnings when using the \b decltype operator.
+ */
+#define MOCKING_UTILS_PATCH_TYPE(id, function) \
+  decltype(mocking_utils::make_patch<id, decltype(function)>("", nullptr))
+
+/// A transparent forwarding proxy to a given `function`.
+/**
+ * Useful to ensure a call to `function` goes through its trampoline.
+ */
+#define MOCKING_UTILS_PATCH_PROXY(function) \
+  [] (auto && ... args)->decltype(auto) { \
+    return function(std::forward<decltype(args)>(args)...); \
+  }
+
+/// Compute a Mimick symbol target string based on which `function` is to be patched
+/// in which `scope`.
+#define MOCKING_UTILS_PATCH_TARGET(scope, function) \
+  (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope))
+
+/// Prepare a mocking_utils::Patch for patching a `function` in a given `scope`
+/// but defer applying any changes.
+#define prepare_patch(scope, function) \
+  make_patch<__COUNTER__, decltype(function)>( \
+    MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \
+  )
+
+/// Patch a `function` with a used-provided `replacement` in a given `scope`.
+#define patch(scope, function, replacement) \
+  prepare_patch(scope, function).then_call(replacement)
+
+/// Patch a `function` to always yield a given `return_code` in a given `scope`.
+#define patch_and_return(scope, function, return_code) \
+  patch(scope, function, [&](auto && ...) {return return_code;})
+
+/// Patch a `function` to always yield a given `return_code` in a given `scope`.
+#define patch_to_fail(scope, function, error_message, return_code) \
+  patch( \
+    scope, function, [&](auto && ...) { \
+      RCUTILS_SET_ERROR_MSG(error_message); \
+      return return_code; \
+    })
+
+/// Patch a `function` to execute normally but always yield a given `return_code`
+/// in a given `scope`.
+/**
+ * \warning On some Linux distributions (e.g. CentOS), pointers to function
+ *   reference their PLT trampolines. In such cases, it is not possible to
+ *   call `function` from within the mock.
+ */
+#define inject_on_return(scope, function, return_code) \
+  patch( \
+    scope, function, ([&, base = function](auto && ... __args) { \
+      if (base != function) { \
+        static_cast<void>(base(std::forward<decltype(__args)>(__args)...)); \
+      } else { \
+        RCUTILS_SAFE_FWRITE_TO_STDERR( \
+          "[WARNING] mocking_utils::inject_on_return() cannot forward call to " \
+          "original '" RCUTILS_STRINGIFY(function) "' function before injection\n" \
+          "    at " __FILE__ ":" RCUTILS_STRINGIFY(__LINE__) "\n"); \
+      } \
+      return return_code; \
+    }))
+
+}  // namespace mocking_utils
+
+#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
+// Define dummy comparison operators for C standard va_list type
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, >)
+#endif
+
+#endif  // MOCKING_UTILS__PATCH_HPP_

rclcpp/test/rclcpp/test_wait_set.cpp

@@ -15,12 +15,15 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <utility>
 #include <vector>
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "test_msgs/msg/basic_types.hpp"
 
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 class TestWaitSet : public ::testing::Test
 {
 protected:
@@ -262,3 +265,39 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) {
     }, std::runtime_error);
   }
 }
+
+/*
+ * Get wait_set from result.
+ */
+TEST_F(TestWaitSet, get_result_from_wait_result) {
+  rclcpp::WaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+  guard_condition->trigger();
+
+  rclcpp::WaitResult<rclcpp::WaitSet> result = wait_set.wait();
+  ASSERT_EQ(rclcpp::WaitResultKind::Ready, result.kind());
+  EXPECT_EQ(&wait_set, &result.get_wait_set());
+
+  const rclcpp::WaitResult<rclcpp::WaitSet> const_result(std::move(result));
+  ASSERT_EQ(rclcpp::WaitResultKind::Ready, const_result.kind());
+  EXPECT_EQ(&wait_set, &const_result.get_wait_set());
+}
+
+TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) {
+  rclcpp::WaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+
+  rclcpp::WaitResult<rclcpp::WaitSet> result = wait_set.wait(std::chrono::milliseconds(10));
+  ASSERT_EQ(rclcpp::WaitResultKind::Timeout, result.kind());
+  RCLCPP_EXPECT_THROW_EQ(
+    result.get_wait_set(),
+    std::runtime_error("cannot access wait set when the result was not ready"));
+
+  const rclcpp::WaitResult<rclcpp::WaitSet> const_result(std::move(result));
+  ASSERT_EQ(rclcpp::WaitResultKind::Timeout, const_result.kind());
+  RCLCPP_EXPECT_THROW_EQ(
+    const_result.get_wait_set(),
+    std::runtime_error("cannot access wait set when the result was not ready"));
+}

rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp

@@ -0,0 +1,299 @@
+// Copyright 2020 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 <memory>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+class TestDynamicStorage : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+};
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  : is_ready_(false) {}
+
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+
+  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+
+  void execute() override {}
+
+  void set_is_ready(bool value) {is_ready_ = value;}
+
+private:
+  bool is_ready_;
+};
+
+TEST_F(TestDynamicStorage, default_construct_destruct) {
+  rclcpp::WaitSet wait_set;
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+  EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait().kind());
+}
+
+TEST_F(TestDynamicStorage, iterables_construct_destruct) {
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+  auto subscriptions =
+    std::vector<rclcpp::WaitSet::SubscriptionEntry>{{subscription}};
+  auto guard_conditions =
+    std::vector<rclcpp::GuardCondition::SharedPtr>{guard_condition};
+  auto timers =
+    std::vector<rclcpp::TimerBase::SharedPtr>{timer};
+  auto clients =
+    std::vector<rclcpp::ClientBase::SharedPtr>{client};
+  auto services =
+    std::vector<rclcpp::ServiceBase::SharedPtr>{service};
+  auto waitables =
+    std::vector<rclcpp::WaitSet::WaitableEntry>{{waitable}};
+  rclcpp::WaitSet wait_set(subscriptions, guard_conditions, timers, clients, services, waitables);
+
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+}
+
+TEST_F(TestDynamicStorage, add_remove_dynamically) {
+  rclcpp::WaitSet wait_set;
+
+  // Adds more coverage
+  rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options;
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options);
+
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+  wait_set.add_subscription(subscription, mask);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_subscription(subscription, mask),
+    std::runtime_error("subscription already associated with a wait set"));
+  wait_set.remove_subscription(subscription, mask);
+
+  // This is long, so it can stick around and be removed
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  wait_set.add_timer(timer);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_timer(timer),
+    std::runtime_error("timer already in use by another wait set"));
+  wait_set.remove_timer(timer);
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_guard_condition(guard_condition),
+    std::runtime_error("guard condition already in use by another wait set"));
+  wait_set.remove_guard_condition(guard_condition);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  wait_set.add_service(service);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_service(service),
+    std::runtime_error("service already in use by another wait set"));
+  wait_set.remove_service(service);
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  wait_set.add_client(client);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_client(client),
+    std::runtime_error("client already in use by another wait set"));
+  wait_set.remove_client(client);
+
+  auto waitable = std::make_shared<TestWaitable>();
+  wait_set.add_waitable(waitable);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_waitable(waitable),
+    std::runtime_error("waitable already in use by another wait set"));
+
+  wait_set.remove_waitable(waitable);
+  wait_set.prune_deleted_entities();
+  EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait().kind());
+}
+
+TEST_F(TestDynamicStorage, add_remove_nullptr) {
+  rclcpp::WaitSet wait_set;
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_subscription(nullptr), std::invalid_argument("subscription is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_subscription(nullptr), std::invalid_argument("subscription is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_timer(nullptr), std::invalid_argument("timer is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_timer(nullptr), std::invalid_argument("timer is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_client(nullptr), std::invalid_argument("client is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_client(nullptr), std::invalid_argument("client is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_service(nullptr), std::invalid_argument("service is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_service(nullptr), std::invalid_argument("service is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_waitable(nullptr), std::invalid_argument("waitable is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_waitable(nullptr), std::invalid_argument("waitable is nullptr"));
+}
+
+TEST_F(TestDynamicStorage, add_remove_out_of_scope) {
+  rclcpp::WaitSet wait_set;
+
+  {
+    auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+      "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+    wait_set.add_subscription(subscription);
+
+    // This is short, so if it's not cleaned up, it will trigger wait and it won't timeout
+    auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+    wait_set.add_timer(timer);
+
+    auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+    wait_set.add_guard_condition(guard_condition);
+
+    auto service =
+      node->create_service<test_msgs::srv::Empty>(
+      "service",
+      [](
+        const test_msgs::srv::Empty::Request::SharedPtr,
+        test_msgs::srv::Empty::Response::SharedPtr) {});
+    wait_set.add_service(service);
+
+    auto client = node->create_client<test_msgs::srv::Empty>("service");
+    wait_set.add_client(client);
+
+    auto waitable = std::make_shared<TestWaitable>();
+    wait_set.add_waitable(waitable);
+  }
+
+  EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind());
+}
+
+TEST_F(TestDynamicStorage, wait_subscription) {
+  rclcpp::WaitSet wait_set;
+
+  // Not added to wait_set, just used for publishing to the topic
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  wait_set.add_subscription(subscription);
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+
+  publisher->publish(test_msgs::msg::Empty());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestDynamicStorage, wait_timer) {
+  rclcpp::WaitSet wait_set;
+
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+  wait_set.add_timer(timer);
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestDynamicStorage, wait_client_service) {
+  rclcpp::WaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  wait_set.add_service(service);
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  wait_set.add_client(client);
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+  client->async_send_request(std::make_shared<test_msgs::srv::Empty::Request>());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestDynamicStorage, wait_waitable) {
+  rclcpp::WaitSet wait_set;
+  auto waitable = std::make_shared<TestWaitable>();
+  wait_set.add_waitable(waitable);
+  {
+    // This waitable doesn't add itself to the rcl_wait_set_t, so Empty is to be expected
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_result.kind());
+  }
+}

rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp

@@ -0,0 +1,202 @@
+// Copyright 2020 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 <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "rclcpp/wait_set_policies/static_storage.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+class TestStaticStorage : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+};
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  : is_ready_(false) {}
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+
+  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+
+  void execute() override {}
+
+  void set_is_ready(bool value) {is_ready_ = value;}
+
+private:
+  bool is_ready_;
+};
+
+TEST_F(TestStaticStorage, iterables_construct_destruct) {
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  // This is long, so it can stick around and be removed
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+  rclcpp::StaticWaitSet<1, 1, 1, 1, 1, 1> wait_set(
+    {{{subscription}}}, {guard_condition}, {timer}, {client}, {service}, {{{waitable}}});
+
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+}
+
+// Because these StaticWaitSet's have templated sizes larger than the input arguments passed
+// to the constructor, their shared-pointer contents will be default constructed to null. This
+// test just checks the appropriate exception is thrown.
+// std::shared_ptr<StaticWaitSet>::reset() is not required for these exceptions, it just
+// disables the unused return value warning of std::make_shared
+TEST_F(TestStaticStorage, fixed_storage_needs_pruning) {
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 1, 0, 0, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 1, 0, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 0, 1, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 0, 0, 1>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+}
+
+TEST_F(TestStaticStorage, wait_subscription) {
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+  rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{subscription, mask}}});
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+
+  publisher->publish(test_msgs::msg::Empty());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestStaticStorage, wait_timer) {
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+  rclcpp::StaticWaitSet<0, 0, 1, 0, 0, 0> wait_set({}, {}, {timer});
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestStaticStorage, wait_guard_condition) {
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0> wait_set({}, {guard_condition});
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+
+  guard_condition->trigger();
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestStaticStorage, wait_client_service) {
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  rclcpp::StaticWaitSet<0, 0, 0, 1, 1, 0> wait_set({}, {}, {}, {client}, {service});
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+  client->async_send_request(std::make_shared<test_msgs::srv::Empty::Request>());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestStaticStorage, wait_waitable) {
+  auto waitable = std::make_shared<TestWaitable>();
+  rclcpp::StaticWaitSet<0, 0, 0, 0, 0, 1> wait_set({}, {}, {}, {}, {}, {{{waitable}}});
+  {
+    // This waitable doesn't add itself to the rcl_wait_set_t, so Empty is to be expected
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_result.kind());
+  }
+}

rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp

@@ -0,0 +1,170 @@
+// Copyright 2020 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 <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+class TestStoragePolicyCommon : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+};
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  : is_ready_(false), add_to_wait_set_(false) {}
+  bool add_to_wait_set(rcl_wait_set_t *) override {return add_to_wait_set_;}
+
+  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+
+  void execute() override {}
+
+  void set_is_ready(bool value) {is_ready_ = value;}
+
+  void set_add_to_wait_set(bool value) {add_to_wait_set_ = value;}
+
+private:
+  bool is_ready_;
+  bool add_to_wait_set_;
+};
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_fini_error) {
+  auto wait_set = std::make_shared<rclcpp::WaitSet>();
+  auto mock = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR);
+  EXPECT_NO_THROW(wait_set.reset());
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_resize_error) {
+  rclcpp::WaitSet wait_set;
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
+  wait_set.add_subscription(subscription, mask);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_clear_error) {
+  rclcpp::WaitSet wait_set;
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_subscription_error) {
+  rclcpp::WaitSet wait_set;
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_subscription, RCL_RET_ERROR);
+  wait_set.add_subscription(subscription, mask);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_guard_condition_error) {
+  rclcpp::WaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR);
+  wait_set.add_guard_condition(guard_condition);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_timer_error) {
+  rclcpp::WaitSet wait_set;
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_timer, RCL_RET_ERROR);
+  wait_set.add_timer(timer);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_service_error) {
+  rclcpp::WaitSet wait_set;
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_service, RCL_RET_ERROR);
+  wait_set.add_service(service);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_client_error) {
+  rclcpp::WaitSet wait_set;
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_client, RCL_RET_ERROR);
+  wait_set.add_client(client);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, add_waitable_error) {
+  rclcpp::WaitSet wait_set;
+  auto waitable = std::make_shared<TestWaitable>();
+  waitable->set_add_to_wait_set(false);
+  wait_set.add_waitable(waitable);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.wait(),
+    std::runtime_error("waitable unexpectedly failed to be added to wait set"));
+}

rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp

@@ -0,0 +1,303 @@
+// Copyright 2020 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 <memory>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+class TestThreadSafeStorage : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+};
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  : is_ready_(false) {}
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+
+  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+
+  void execute() override {}
+
+  void set_is_ready(bool value) {is_ready_ = value;}
+
+private:
+  bool is_ready_;
+};
+
+TEST_F(TestThreadSafeStorage, default_construct_destruct) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+
+  // Expected behavior of thread-safe is to timeout here
+  EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind());
+}
+
+TEST_F(TestThreadSafeStorage, iterables_construct_destruct) {
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  // This is long, so it can stick around
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+  auto subscriptions =
+    std::vector<rclcpp::ThreadSafeWaitSet::SubscriptionEntry>{{subscription}};
+  auto guard_conditions =
+    std::vector<rclcpp::GuardCondition::SharedPtr>{guard_condition};
+  auto timers =
+    std::vector<rclcpp::TimerBase::SharedPtr>{timer};
+  auto clients =
+    std::vector<rclcpp::ClientBase::SharedPtr>{client};
+  auto services =
+    std::vector<rclcpp::ServiceBase::SharedPtr>{service};
+  auto waitables =
+    std::vector<rclcpp::ThreadSafeWaitSet::WaitableEntry>{{waitable}};
+  rclcpp::ThreadSafeWaitSet wait_set(
+    subscriptions, guard_conditions, timers, clients, services, waitables);
+
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+}
+
+TEST_F(TestThreadSafeStorage, add_remove_dynamically) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  // Adds more coverage
+  rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options;
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options);
+
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+  wait_set.add_subscription(subscription, mask);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_subscription(subscription, mask),
+    std::runtime_error("subscription already associated with a wait set"));
+  wait_set.remove_subscription(subscription, mask);
+
+  // This is long, so it can stick around and be removed
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  wait_set.add_timer(timer);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_timer(timer),
+    std::runtime_error("timer already in use by another wait set"));
+  wait_set.remove_timer(timer);
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_guard_condition(guard_condition),
+    std::runtime_error("guard condition already in use by another wait set"));
+  wait_set.remove_guard_condition(guard_condition);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  wait_set.add_service(service);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_service(service),
+    std::runtime_error("service already in use by another wait set"));
+  wait_set.remove_service(service);
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  wait_set.add_client(client);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_client(client),
+    std::runtime_error("client already in use by another wait set"));
+  wait_set.remove_client(client);
+
+  auto waitable = std::make_shared<TestWaitable>();
+  wait_set.add_waitable(waitable);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_waitable(waitable),
+    std::runtime_error("waitable already in use by another wait set"));
+  wait_set.remove_waitable(waitable);
+  wait_set.prune_deleted_entities();
+
+  // Expected behavior of thread-safe is to timeout here
+  EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind());
+}
+
+TEST_F(TestThreadSafeStorage, add_remove_nullptr) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_subscription(nullptr), std::invalid_argument("subscription is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_subscription(nullptr), std::invalid_argument("subscription is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_timer(nullptr), std::invalid_argument("timer is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_timer(nullptr), std::invalid_argument("timer is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_client(nullptr), std::invalid_argument("client is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_client(nullptr), std::invalid_argument("client is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_service(nullptr), std::invalid_argument("service is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_service(nullptr), std::invalid_argument("service is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_waitable(nullptr), std::invalid_argument("waitable is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_waitable(nullptr), std::invalid_argument("waitable is nullptr"));
+}
+
+TEST_F(TestThreadSafeStorage, add_remove_out_of_scope) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  {
+    auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+      "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+    wait_set.add_subscription(subscription);
+
+    // This is short, so if it's not cleaned up, it will trigger wait
+    auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+    wait_set.add_timer(timer);
+
+    auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+    wait_set.add_guard_condition(guard_condition);
+
+    auto service =
+      node->create_service<test_msgs::srv::Empty>(
+      "service",
+      [](
+        const test_msgs::srv::Empty::Request::SharedPtr,
+        test_msgs::srv::Empty::Response::SharedPtr) {});
+    wait_set.add_service(service);
+
+    auto client = node->create_client<test_msgs::srv::Empty>("service");
+    wait_set.add_client(client);
+
+    auto waitable = std::make_shared<TestWaitable>();
+    wait_set.add_waitable(waitable);
+  }
+
+  EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind());
+}
+
+TEST_F(TestThreadSafeStorage, wait_subscription) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  // Not added to wait_set, just used for publishing to the topic
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  wait_set.add_subscription(subscription);
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+
+  publisher->publish(test_msgs::msg::Empty());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestThreadSafeStorage, wait_timer) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+  wait_set.add_timer(timer);
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestThreadSafeStorage, wait_client_service) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  wait_set.add_service(service);
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  wait_set.add_client(client);
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+  client->async_send_request(std::make_shared<test_msgs::srv::Empty::Request>());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestThreadSafeStorage, wait_waitable) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+  auto waitable = std::make_shared<TestWaitable>();
+  wait_set.add_waitable(waitable);
+  {
+    // This waitable doesn't add itself to the rcl_wait_set_t, so Timeout is to be expected
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+}