diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 0d80942..bf207c2 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -35,6 +35,7 @@ ament_cmake_gtest ament_lint_auto ament_lint_common + mimick_vendor rmw rmw_implementation_cmake rosidl_default_generators diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 598798f..92796b4 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/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}) diff --git a/rclcpp/test/mocking_utils/patch.hpp b/rclcpp/test/mocking_utils/patch.hpp new file mode 100644 index 0000000..b9931d3 --- /dev/null +++ b/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 +#endif + +#include +#include +#include +#include + +#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 +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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct PatchTraits +{ + 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 +struct Trampoline; + +/// Trampoline specialization for free functions. +template +struct Trampoline +{ + static ReturnT base(ArgTs... args) + { + return target(std::forward(args)...); + } + + static std::function target; +}; + +template +std::function +Trampoline::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 +auto prepare_trampoline(std::function target) +{ + Trampoline::target = target; + return Trampoline::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 +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 +class Patch +{ +public: + using mock_type = typename PatchTraits::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 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 replacement) & + { + replace_with(replacement); + return *this; + } + + /// Inject a @p replacement for the patched function. + Patch && then_call(std::function replacement) && + { + replace_with(replacement); + return std::move(*this); + } + +private: + // Helper for template parameter pack expansion using `mmk_any` + // macro as pattern. + template + T any() {return mmk_any(T);} + + void replace_with(std::function replacement) + { + if (mock_) { + throw std::logic_error("Cannot configure patch more than once"); + } + auto type_erased_trampoline = + reinterpret_cast(prepare_trampoline(replacement)); + auto MMK_MANGLE(mock_type, create) = + PatchTraits::MMK_MANGLE(mock_type, create); + mock_ = mmk_mock(target_.c_str(), mock_type); + mmk_when(proxy_(any()...), .then_call = type_erased_trampoline); + } + + mock_type mock_{nullptr}; + std::string target_; + std::function 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 +auto make_patch(const std::string & target, std::function proxy) +{ + return Patch(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 std::enable_if::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("", 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(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(base(std::forward(__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_ diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index 39fe4c1..7a5f7b3 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -15,12 +15,15 @@ #include #include +#include #include #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(); + wait_set.add_guard_condition(guard_condition); + guard_condition->trigger(); + + rclcpp::WaitResult result = wait_set.wait(); + ASSERT_EQ(rclcpp::WaitResultKind::Ready, result.kind()); + EXPECT_EQ(&wait_set, &result.get_wait_set()); + + const rclcpp::WaitResult 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(); + wait_set.add_guard_condition(guard_condition); + + rclcpp::WaitResult 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 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")); +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp new file mode 100644 index 0000000..c361231 --- /dev/null +++ b/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 + +#include +#include + +#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("node", "ns"); + } + + std::shared_ptr 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( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto guard_condition = std::make_shared(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + auto subscriptions = + std::vector{{subscription}}; + auto guard_conditions = + std::vector{guard_condition}; + auto timers = + std::vector{timer}; + auto clients = + std::vector{client}; + auto services = + std::vector{service}; + auto waitables = + std::vector{{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> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto subscription = node->create_subscription( + "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(); + 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( + "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("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(); + 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( + "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(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + + auto waitable = std::make_shared(); + 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("topic", 10); + + auto subscription = node->create_subscription( + "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(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("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()); + { + 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(); + 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()); + } +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp new file mode 100644 index 0000000..2fdb71d --- /dev/null +++ b/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 + +#include + +#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("node", "ns"); + } + + std::shared_ptr 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( + "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(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + 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::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().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().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().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().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().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().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } +} + +TEST_F(TestStaticStorage, wait_subscription) { + auto publisher = node->create_publisher("topic", 10); + auto subscription = node->create_subscription( + "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::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( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + + auto client = node->create_client("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()); + { + 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(); + 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()); + } +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp new file mode 100644 index 0000000..d9b35cd --- /dev/null +++ b/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 + +#include + +#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("node", "ns"); + } + + std::shared_ptr 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(); + 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( + "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( + "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(); + 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( + "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("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(); + 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")); +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp new file mode 100644 index 0000000..ba0ccda --- /dev/null +++ b/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 + +#include +#include + +#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("node", "ns"); + } + + std::shared_ptr 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( + "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(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + auto subscriptions = + std::vector{{subscription}}; + auto guard_conditions = + std::vector{guard_condition}; + auto timers = + std::vector{timer}; + auto clients = + std::vector{client}; + auto services = + std::vector{service}; + auto waitables = + std::vector{{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> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto subscription = node->create_subscription( + "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(); + 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( + "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("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(); + 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( + "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(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + + auto waitable = std::make_shared(); + 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("topic", 10); + + auto subscription = node->create_subscription( + "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(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("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()); + { + 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(); + 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()); + } +}