From bf03d2f5e306732335da25044611c5e53a781b2c Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 26 Feb 2024 10:42:41 -0800 Subject: [PATCH] add tests for waitables, fix const-ness in some of the APIs Signed-off-by: William Woodall --- rclcpp/include/rclcpp/event_handler.hpp | 4 +- .../executors/executor_notify_waitable.hpp | 4 +- .../static_executor_entities_collector.hpp | 4 +- .../subscription_intra_process.hpp | 8 +- .../subscription_intra_process_base.hpp | 4 +- .../subscription_intra_process_buffer.hpp | 2 +- rclcpp/include/rclcpp/waitable.hpp | 4 +- rclcpp/src/rclcpp/event_handler.cpp | 2 +- .../executors/executor_notify_waitable.cpp | 4 +- .../static_executor_entities_collector.cpp | 4 +- rclcpp/test/rclcpp/CMakeLists.txt | 5 + .../test/rclcpp/executors/test_executors.cpp | 4 +- ...est_static_executor_entities_collector.cpp | 4 +- .../node_interfaces/test_node_waitables.cpp | 4 +- .../test_allocator_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_memory_strategy.cpp | 4 +- .../test_dynamic_storage.cpp | 4 +- .../wait_set_policies/test_static_storage.cpp | 4 +- .../test_storage_policy_common.cpp | 4 +- .../test_thread_safe_synchronization.cpp | 4 +- .../waitables/test_intra_process_waitable.cpp | 46 +++++++ .../waitables/waitable_test_helpers.hpp | 117 ++++++++++++++++++ 22 files changed, 206 insertions(+), 38 deletions(-) create mode 100644 rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp create mode 100644 rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp index f9b75eb7cf..dea1174ac8 100644 --- a/rclcpp/include/rclcpp/event_handler.hpp +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -122,7 +122,7 @@ class EventHandlerBase : public Waitable /// Check if the Waitable is ready. RCLCPP_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t * wait_set) override; /// Set a callback to be called when each new event instance occurs. /** @@ -294,7 +294,7 @@ class EventHandler : public EventHandlerBase /// Execute any entities of the Waitable that are ready. void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr & data) override { if (!data) { throw std::runtime_error("'data' is empty"); diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index 7faa723394..db8283b383 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -68,7 +68,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t * wait_set) override; /// Perform work associated with the waitable. /** @@ -77,7 +77,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// Retrieve data to be used in the next execute call. /** diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index f9fd2ff672..7d6b4b091e 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -77,7 +77,7 @@ class StaticExecutorEntitiesCollector final /// Execute the waitable. RCLCPP_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// Take the data so that it can be consumed with `execute`. /** @@ -196,7 +196,7 @@ class StaticExecutorEntitiesCollector final */ RCLCPP_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t * wait_set) override; /// Return number of timers /** diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 34ba02ef07..25cd123b8c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -150,7 +150,7 @@ class SubscriptionIntraProcess ); } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { execute_impl(data); } @@ -158,7 +158,7 @@ class SubscriptionIntraProcess protected: template typename std::enable_if::value, void>::type - execute_impl(std::shared_ptr & data) + execute_impl(const std::shared_ptr & data) { (void)data; throw std::runtime_error("Subscription intra-process can't handle serialized messages"); @@ -166,9 +166,9 @@ class SubscriptionIntraProcess template typename std::enable_if::value, void>::type - execute_impl(std::shared_ptr & data) + execute_impl(const std::shared_ptr & data) { - if (!data) { + if (nullptr == data) { return; } diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 6a14aac0c7..656f7b9686 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -72,7 +72,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable is_durability_transient_local() const; bool - is_ready(rcl_wait_set_t * wait_set) override = 0; + is_ready(const rcl_wait_set_t * wait_set) override = 0; std::shared_ptr take_data() override = 0; @@ -85,7 +85,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } void - execute(std::shared_ptr & data) override = 0; + execute(const std::shared_ptr & data) override = 0; virtual bool diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index f564cbb047..cdf5b749ef 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -100,7 +100,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff } bool - is_ready(rcl_wait_set_t * wait_set) override + is_ready(const rcl_wait_set_t * wait_set) override { (void) wait_set; return buffer_->has_data(); diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 1a0d8b61f1..bc79f0c940 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -124,7 +124,7 @@ class Waitable RCLCPP_PUBLIC virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; + is_ready(const rcl_wait_set_t * wait_set) = 0; /// Take the data so that it can be consumed with `execute`. /** @@ -203,7 +203,7 @@ class Waitable RCLCPP_PUBLIC virtual void - execute(std::shared_ptr & data) = 0; + execute(const std::shared_ptr & data) = 0; /// Exchange the "in use by wait set" state for this timer. /** diff --git a/rclcpp/src/rclcpp/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp index 3e70904c6f..cb6818b744 100644 --- a/rclcpp/src/rclcpp/event_handler.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -67,7 +67,7 @@ EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) /// Check if the Waitable is ready. bool -EventHandlerBase::is_ready(rcl_wait_set_t * wait_set) +EventHandlerBase::is_ready(const rcl_wait_set_t * wait_set) { return wait_set->events[wait_set_event_index_] == &event_handle_; } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 2e183d7daa..7eac3da102 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -70,7 +70,7 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) } bool -ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) +ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t * wait_set) { std::lock_guard lock(guard_condition_mutex_); @@ -92,7 +92,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) } void -ExecutorNotifyWaitable::execute(std::shared_ptr & data) +ExecutorNotifyWaitable::execute(const std::shared_ptr & data) { (void) data; this->execute_callback_(); diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index 73926b12bc..daab8ff309 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -96,7 +96,7 @@ StaticExecutorEntitiesCollector::take_data() } void -StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) +StaticExecutorEntitiesCollector::execute(const std::shared_ptr & data) { (void) data; // Fill memory strategy with entities coming from weak_nodes_ @@ -434,7 +434,7 @@ StaticExecutorEntitiesCollector::remove_node( } bool -StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set) +StaticExecutorEntitiesCollector::is_ready(const rcl_wait_set_t * p_wait_set) { // Check wait_set guard_conditions for added/removed entities to/from a node for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) { diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 25ca4082e1..780056675e 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -572,6 +572,11 @@ if(TARGET test_thread_safe_synchronization) target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() +ament_add_gtest(test_intra_process_waitable waitables/test_intra_process_waitable.cpp) +if(TARGET test_intra_process_waitable) + target_link_libraries(test_intra_process_waitable ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + ament_add_gtest(test_rosout_qos test_rosout_qos.cpp) if(TARGET test_rosout_qos) target_link_libraries(test_rosout_qos ${PROJECT_NAME} rcl::rcl rmw::rmw) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index da4b087607..1b746b568a 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -350,7 +350,7 @@ class TestWaitable : public rclcpp::Waitable } bool - is_ready(rcl_wait_set_t * wait_set) override + is_ready(const rcl_wait_set_t * wait_set) override { for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) { auto rcl_guard_condition = wait_set->guard_conditions[i]; @@ -375,7 +375,7 @@ class TestWaitable : public rclcpp::Waitable } void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr & data) override { (void) data; count_++; diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index 1ea91029f4..1571fda2be 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -232,7 +232,7 @@ class TestWaitable : public rclcpp::Waitable public: void add_to_wait_set(rcl_wait_set_t *) override {} - bool is_ready(rcl_wait_set_t *) override {return true;} + bool is_ready(const rcl_wait_set_t *) override {return true;} std::shared_ptr take_data() override @@ -240,7 +240,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr & data) override { (void) data; } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index e559e57cd1..18b3e5236a 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -29,7 +29,7 @@ class TestWaitable : public rclcpp::Waitable { public: void add_to_wait_set(rcl_wait_set_t *) override {} - bool is_ready(rcl_wait_set_t *) override {return false;} + bool is_ready(const rcl_wait_set_t *) override {return false;} std::shared_ptr take_data() override @@ -37,7 +37,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { (void) data; } diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index e0a666f6b0..f195824ae4 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -46,7 +46,7 @@ class TestWaitable : public rclcpp::Waitable } } - bool is_ready(rcl_wait_set_t *) override + bool is_ready(const rcl_wait_set_t *) override { return test_waitable_result; } @@ -57,7 +57,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { (void) data; } diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index c08b84230a..6987e43481 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -36,9 +36,9 @@ class TestWaitable : public rclcpp::Waitable { public: void add_to_wait_set(rcl_wait_set_t *) override {} - bool is_ready(rcl_wait_set_t *) override {return true;} + bool is_ready(const rcl_wait_set_t *) override {return true;} std::shared_ptr take_data() override {return nullptr;} - void execute(std::shared_ptr & data) override {(void)data;} + void execute(const std::shared_ptr & data) override {(void)data;} }; class TestMemoryStrategy : public ::testing::Test diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp index 9afb97536a..fd81dca7ac 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -53,12 +53,12 @@ class TestWaitable : public rclcpp::Waitable void add_to_wait_set(rcl_wait_set_t *) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t *) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr & data) override {(void)data;} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp index d3aee83c23..13c6d57a25 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -53,12 +53,12 @@ class TestWaitable : public rclcpp::Waitable void add_to_wait_set(rcl_wait_set_t *) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t *) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr & data) override {(void)data;} void set_is_ready(bool value) {is_ready_ = value;} 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 index 2eeb734383..8f7a0e3900 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp @@ -58,12 +58,12 @@ class TestWaitable : public rclcpp::Waitable } } - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t *) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr & data) override {(void)data;} void set_is_ready(bool value) {is_ready_ = value;} 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 index ec268bb39b..6575bf28c2 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp @@ -53,12 +53,12 @@ class TestWaitable : public rclcpp::Waitable void add_to_wait_set(rcl_wait_set_t *) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t *) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr & data) override {(void)data;} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp b/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp new file mode 100644 index 0000000000..367adf705a --- /dev/null +++ b/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp @@ -0,0 +1,46 @@ +// Copyright 2024 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 "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +#include "./waitable_test_helpers.hpp" + +class TestIntraProcessWaitable : public ::testing::Test +{ +protected: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + static void TearDownTestCase() { rclcpp::shutdown(); } +}; + +TEST_F(TestIntraProcessWaitable, test_that_waitable_stays_ready_after_second_wait) { + auto node = std::make_shared( + "test_node", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + using test_msgs::msg::Empty; + auto sub = node->create_subscription("test_topic", 10, [](const Empty &) {}); + auto pub = node->create_publisher("test_topic", 10); + + auto make_sub_intra_process_waitable_ready = [pub]() { + pub->publish(Empty()); + }; + + rclcpp::test::waitables::do_test_that_waitable_stays_ready_after_second_wait( + sub->get_intra_process_waitable(), + make_sub_intra_process_waitable_ready, + true /* expected_to_stay_ready */); +} diff --git a/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp b/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp new file mode 100644 index 0000000000..ceab259c78 --- /dev/null +++ b/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp @@ -0,0 +1,117 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TEST__RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_ +#define RCLCPP__TEST__RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_ + +#include + +#include +#include +#include + +#include + +namespace rclcpp +{ +namespace test +{ +namespace waitables +{ + +/// Test that a given waitable is ready after a second wait. +/** + * The purpose of this test is to check that a waitable will remain ready + * on subsequent wait calls, if that is the expected behavior. + * Not all waitables should remain ready after a wait call, which can be + * expressed in the expected_to_stay_ready argument which defaults to true. + * If set to false, it will check that it is not ready after a second wait, as + * well as some other parts of the test. + * + * The given waitable should: + * + * - not be ready initially + * - not be ready after being waited on (and timing out) + * - should become ready after the make_waitable_ready method is called + * - may or may not be ready at this point + * - should be ready after waiting on it, within the wait_timeout + * - should be ready still after a second wait (unless expected_to_stay_ready = false) + * - if expected_to_stay_ready, should become not ready after a take_data/execute + */ +template +void +do_test_that_waitable_stays_ready_after_second_wait( + const std::shared_ptr & waitable, + std::function make_waitable_ready, + bool expected_to_stay_ready = true, + std::chrono::nanoseconds wait_timeout = std::chrono::seconds(5)) +{ + rclcpp::WaitSet wait_set; + wait_set.add_waitable(waitable); + + // not ready initially + EXPECT_FALSE(waitable->is_ready(&wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly ready before waiting"; + + // not ready after a wait that timesout + { + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not timeout as expected"; + EXPECT_FALSE(waitable->is_ready(&wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly ready after waiting, but before making ready"; + } + + // make it ready and wait on it + make_waitable_ready(); + { + auto wait_result = wait_set.wait(wait_timeout); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready) + << "wait set was not ready after the waitable should have been made ready"; + EXPECT_TRUE(waitable->is_ready(&wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly not ready after making it ready and waiting"; + } + + // wait again, and see that it is ready as expected or not expected + { + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + if (expected_to_stay_ready) { + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready) + << "wait set was not ready on a second wait on the waitable"; + EXPECT_TRUE(waitable->is_ready(&wait_set.get_rcl_wait_set())) + << "waitable unexpectedly not ready after second wait"; + } else { + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not time out after the waitable should have no longer been ready"; + EXPECT_FALSE(waitable->is_ready(&wait_set.get_rcl_wait_set())) + << "waitable was ready after waiting a second time, which was not expected"; + } + } + + // if expected_to_stay_ready, check that take_data/execute makes it not ready + if (expected_to_stay_ready) { + waitable->execute(waitable->take_data()); + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not time out after the waitable should have no longer been ready"; + EXPECT_FALSE(waitable->is_ready(&wait_set.get_rcl_wait_set())) + << "waitable was unexpectedly ready after a take_data and execute"; + } +} + +} // namespace waitables +} // namespace test +} // namespace rclcpp + +#endif // RCLCPP__TEST__RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_