From 43847f8ee5e92e836f2b243f8d9ca5811d8f95aa Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 26 Jul 2024 11:35:50 -0700 Subject: [PATCH 1/6] add unit-test to verify that spin-all doesn't need warm-up Signed-off-by: Alberto Soragna --- .../test/rclcpp/executors/test_executors.cpp | 41 +++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 72afee8dda..8b1cac222d 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -856,3 +856,44 @@ TYPED_TEST(TestExecutors, release_ownership_entity_after_spinning_cancel) EXPECT_EQ(server.use_count(), 1); } + +// This test verifies that spin_all is correctly collecting work multiple times +// even when one of the items of work is a notifier waitable event and thus results in +// rebuilding the entities collection. +// When spin_all goes back to collect more work, it should see the ready items from +// the new added entities +TYPED_TEST(TestExecutors, spin_all_doesnt_require_warmup) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + // Add node to the executor before creating the entities + executor.add_node(node); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback)); + + ASSERT_EQ(this->callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // This is unfortunately open to flakiness... We need to select a duration that is greater than + // the time taken to refresh the entities collection and rebuild the waitset. + // On my 8-core laptop this reliably works with 1ms, but using 500ms to increase robustness. + // spin-all is expected to process the notifier waitable event, rebuild the collection, + // and then collect more work, finding the subscription message event. + executor.spin_all(std::chrono::milliseconds(500)); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(this->callback_count, 1u); +} From 8591223738c111f75f204889565392e4ed2dc90b Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Sun, 4 Aug 2024 16:35:24 -0700 Subject: [PATCH 2/6] improve comment and add callback group test Signed-off-by: Alberto Soragna --- .../test/rclcpp/executors/test_executors.cpp | 48 ++++++++++++++++++- 1 file changed, 46 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 8b1cac222d..a69c99b1d3 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -887,11 +887,55 @@ TYPED_TEST(TestExecutors, spin_all_doesnt_require_warmup) // Publish a message so that the new entities (i.e. the subscriber) already have work to do publisher->publish(test_msgs::msg::Empty()); - // This is unfortunately open to flakiness... We need to select a duration that is greater than + // We need to select a duration that is greater than // the time taken to refresh the entities collection and rebuild the waitset. - // On my 8-core laptop this reliably works with 1ms, but using 500ms to increase robustness. // spin-all is expected to process the notifier waitable event, rebuild the collection, // and then collect more work, finding the subscription message event. + // This duration has been selected empirically. + executor.spin_all(std::chrono::milliseconds(500)); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(this->callback_count, 1u); +} + +// Same test as `spin_all_doesnt_require_warmup`, but uses a callback group +// This test reproduces the bug reported by https://github.com/ros2/rclcpp/issues/2589 +TYPED_TEST(TestExecutors, spin_all_doesnt_require_warmup_with_cbgroup) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + auto callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + // Add callback group to the executor before creating the entities + executor.add_callback_group(callback_group, node->get_node_base_interface()); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback), sub_options); + + ASSERT_EQ(this->callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // We need to select a duration that is greater than + // the time taken to refresh the entities collection and rebuild the waitset. + // spin-all is expected to process the notifier waitable event, rebuild the collection, + // and then collect more work, finding the subscription message event. + // This duration has been selected empirically. executor.spin_all(std::chrono::milliseconds(500)); // Verify that the callback is called as part of the spin above From 77ddcd18bff9b0de3b62ddff125b1ad21d4d631d Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Sun, 4 Aug 2024 16:57:03 -0700 Subject: [PATCH 3/6] move executor tests to a new file Signed-off-by: Alberto Soragna --- rclcpp/test/rclcpp/CMakeLists.txt | 10 ++ .../test/rclcpp/executors/test_executors.cpp | 85 ----------- .../executors/test_executors_minimal_init.cpp | 140 ++++++++++++++++++ 3 files changed, 150 insertions(+), 85 deletions(-) create mode 100644 rclcpp/test/rclcpp/executors/test_executors_minimal_init.cpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 756e1c67c6..3f2859dd77 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -500,6 +500,16 @@ if(TARGET test_executors) target_link_libraries(test_executors_busy_waiting ${PROJECT_NAME}) endif() +ament_add_gtest( + test_executors_minimal_init + executors/test_executors_minimal_init.cpp + executors/test_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors_minimal_init) + target_link_libraries(test_executors_minimal_init ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") ament_add_test_label(test_static_single_threaded_executor mimick) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index a69c99b1d3..72afee8dda 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -856,88 +856,3 @@ TYPED_TEST(TestExecutors, release_ownership_entity_after_spinning_cancel) EXPECT_EQ(server.use_count(), 1); } - -// This test verifies that spin_all is correctly collecting work multiple times -// even when one of the items of work is a notifier waitable event and thus results in -// rebuilding the entities collection. -// When spin_all goes back to collect more work, it should see the ready items from -// the new added entities -TYPED_TEST(TestExecutors, spin_all_doesnt_require_warmup) -{ - using ExecutorType = TypeParam; - ExecutorType executor; - - // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event - auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); - auto node = std::make_shared("test_node", node_options); - - // Add node to the executor before creating the entities - executor.add_node(node); - - // Create entities, this will produce a notifier waitable event, telling the executor to refresh - // the entities collection - auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); - auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; - auto subscription = - node->create_subscription( - "test_topic", rclcpp::QoS(10), std::move(callback)); - - ASSERT_EQ(this->callback_count, 0u); - - // Publish a message so that the new entities (i.e. the subscriber) already have work to do - publisher->publish(test_msgs::msg::Empty()); - - // We need to select a duration that is greater than - // the time taken to refresh the entities collection and rebuild the waitset. - // spin-all is expected to process the notifier waitable event, rebuild the collection, - // and then collect more work, finding the subscription message event. - // This duration has been selected empirically. - executor.spin_all(std::chrono::milliseconds(500)); - - // Verify that the callback is called as part of the spin above - EXPECT_EQ(this->callback_count, 1u); -} - -// Same test as `spin_all_doesnt_require_warmup`, but uses a callback group -// This test reproduces the bug reported by https://github.com/ros2/rclcpp/issues/2589 -TYPED_TEST(TestExecutors, spin_all_doesnt_require_warmup_with_cbgroup) -{ - using ExecutorType = TypeParam; - ExecutorType executor; - - // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event - auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); - auto node = std::make_shared("test_node", node_options); - - auto callback_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false); - - // Add callback group to the executor before creating the entities - executor.add_callback_group(callback_group, node->get_node_base_interface()); - - // Create entities, this will produce a notifier waitable event, telling the executor to refresh - // the entities collection - auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); - auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; - auto subscription = - node->create_subscription( - "test_topic", rclcpp::QoS(10), std::move(callback), sub_options); - - ASSERT_EQ(this->callback_count, 0u); - - // Publish a message so that the new entities (i.e. the subscriber) already have work to do - publisher->publish(test_msgs::msg::Empty()); - - // We need to select a duration that is greater than - // the time taken to refresh the entities collection and rebuild the waitset. - // spin-all is expected to process the notifier waitable event, rebuild the collection, - // and then collect more work, finding the subscription message event. - // This duration has been selected empirically. - executor.spin_all(std::chrono::milliseconds(500)); - - // Verify that the callback is called as part of the spin above - EXPECT_EQ(this->callback_count, 1u); -} diff --git a/rclcpp/test/rclcpp/executors/test_executors_minimal_init.cpp b/rclcpp/test/rclcpp/executors/test_executors_minimal_init.cpp new file mode 100644 index 0000000000..0eec703284 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_minimal_init.cpp @@ -0,0 +1,140 @@ +// Copyright 2017 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. + +/** + * This test checks all implementations of rclcpp::executor to check they pass they basic API + * tests. Anything specific to any executor in particular should go in a separate test file. + */ + +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/empty.hpp" + +#include "./executor_types.hpp" + +using namespace std::chrono_literals; + +/** + * @brief This test suite is meant to be used by Executors tests that + * require test-specific initialization routines. + */ +template +class TestExecutorsMinimalInitialization : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TYPED_TEST_SUITE(TestExecutorsMinimalInitialization, ExecutorTypes, ExecutorTypeNames); + +// This test verifies that spin_all is correctly collecting work multiple times +// even when one of the items of work is a notifier waitable event and thus results in +// rebuilding the entities collection. +// When spin_all goes back to collect more work, it should see the ready items from +// the new added entities +TYPED_TEST(TestExecutorsMinimalInitialization, spin_all_doesnt_require_warmup) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + // Add node to the executor before creating the entities + executor.add_node(node); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback)); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // We need to select a duration that is greater than + // the time taken to refresh the entities collection and rebuild the waitset. + // spin-all is expected to process the notifier waitable event, rebuild the collection, + // and then collect more work, finding the subscription message event. + // This duration has been selected empirically. + executor.spin_all(std::chrono::milliseconds(500)); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} + +// Same test as `spin_all_doesnt_require_warmup`, but uses a callback group +// This test reproduces the bug reported by https://github.com/ros2/rclcpp/issues/2589 +TYPED_TEST(TestExecutorsMinimalInitialization, spin_all_doesnt_require_warmup_with_cbgroup) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + auto callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + // Add callback group to the executor before creating the entities + executor.add_callback_group(callback_group, node->get_node_base_interface()); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback), sub_options); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // We need to select a duration that is greater than + // the time taken to refresh the entities collection and rebuild the waitset. + // spin-all is expected to process the notifier waitable event, rebuild the collection, + // and then collect more work, finding the subscription message event. + // This duration has been selected empirically. + executor.spin_all(std::chrono::milliseconds(500)); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} From 7a5d7e33f8be9c964168bb0854283404bb4163f4 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Sun, 4 Aug 2024 18:05:08 -0700 Subject: [PATCH 4/6] do not require warm up iteration with events executor spin_some Signed-off-by: Alberto Soragna --- .../events_executor/events_executor.cpp | 22 ++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp index b7dd1487f9..94a6e3a0b8 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -79,7 +79,6 @@ EventsExecutor::setup_notify_waitable() // ---> we need to wake up the executor so that it can terminate // - a node or callback group guard condition is triggered: // ---> the entities collection is changed, we need to update callbacks - entities_need_rebuild_ = false; this->handle_updated_entities(false); }); @@ -168,6 +167,14 @@ EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhau return false; }; + // If this spin is not exhaustive (e.g. spin_some), we need to explicitly check + // if entities need to be rebuilt here rather than letting the notify waitable event do it. + // A non-exhaustive spin would not check for work a second time, thus delaying the execution + // of some entities to the next invocation of spin. + if (!exhaustive) { + this->handle_updated_entities(false); + } + // Get the number of events and timers ready at start const size_t ready_events_at_start = events_queue_->size(); size_t executed_events = 0; @@ -311,9 +318,18 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } void -EventsExecutor::handle_updated_entities(bool notify) +EventsExecutor::handle_updated_entities(bool /*notify*/) { - (void)notify; + // Do not rebuild if we don't need to. + // A rebuild event could be generated, but then + // this function could end up being called from somewhere else + // before that event gets processed, for example if + // a node or callback group is manually added to the executor. + const bool notify_waitable_triggered = entities_need_rebuild_.exchange(false); + if (!notify_waitable_triggered && !this->collector_.has_pending()) { + return; + } + // Build the new collection this->collector_.update_collections(); auto callback_groups = this->collector_.get_all_callback_groups(); From cf2280c16262298390427d69c3b21585a20d5387 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Sat, 10 Aug 2024 10:02:35 -0700 Subject: [PATCH 5/6] add spin_some tests and cleanup Signed-off-by: Alberto Soragna --- .../events_executor/events_executor.cpp | 4 +- rclcpp/test/rclcpp/CMakeLists.txt | 8 +- ...mal_init.cpp => test_executors_warmup.cpp} | 117 ++++++++++++++++-- 3 files changed, 116 insertions(+), 13 deletions(-) rename rclcpp/test/rclcpp/executors/{test_executors_minimal_init.cpp => test_executors_warmup.cpp} (53%) diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp index 94a6e3a0b8..24fc1e8ead 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -318,8 +318,10 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } void -EventsExecutor::handle_updated_entities(bool /*notify*/) +EventsExecutor::handle_updated_entities(bool notify) { + (void)notify; + // Do not rebuild if we don't need to. // A rebuild event could be generated, but then // this function could end up being called from somewhere else diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 3f2859dd77..cc5105688d 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -501,13 +501,13 @@ if(TARGET test_executors) endif() ament_add_gtest( - test_executors_minimal_init - executors/test_executors_minimal_init.cpp + test_executors_warmup + executors/test_executors_warmup.cpp executors/test_waitable.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) -if(TARGET test_executors_minimal_init) - target_link_libraries(test_executors_minimal_init ${PROJECT_NAME} ${test_msgs_TARGETS}) +if(TARGET test_executors_warmup) + target_link_libraries(test_executors_warmup ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp diff --git a/rclcpp/test/rclcpp/executors/test_executors_minimal_init.cpp b/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp similarity index 53% rename from rclcpp/test/rclcpp/executors/test_executors_minimal_init.cpp rename to rclcpp/test/rclcpp/executors/test_executors_warmup.cpp index 0eec703284..524191b885 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_minimal_init.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp @@ -31,12 +31,8 @@ using namespace std::chrono_literals; -/** - * @brief This test suite is meant to be used by Executors tests that - * require test-specific initialization routines. - */ template -class TestExecutorsMinimalInitialization : public ::testing::Test +class TestExecutorsWarmup : public ::testing::Test { public: void SetUp() @@ -50,14 +46,14 @@ class TestExecutorsMinimalInitialization : public ::testing::Test } }; -TYPED_TEST_SUITE(TestExecutorsMinimalInitialization, ExecutorTypes, ExecutorTypeNames); +TYPED_TEST_SUITE(TestExecutorsWarmup, ExecutorTypes, ExecutorTypeNames); // This test verifies that spin_all is correctly collecting work multiple times // even when one of the items of work is a notifier waitable event and thus results in // rebuilding the entities collection. // When spin_all goes back to collect more work, it should see the ready items from // the new added entities -TYPED_TEST(TestExecutorsMinimalInitialization, spin_all_doesnt_require_warmup) +TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup) { using ExecutorType = TypeParam; ExecutorType executor; @@ -96,9 +92,18 @@ TYPED_TEST(TestExecutorsMinimalInitialization, spin_all_doesnt_require_warmup) // Same test as `spin_all_doesnt_require_warmup`, but uses a callback group // This test reproduces the bug reported by https://github.com/ros2/rclcpp/issues/2589 -TYPED_TEST(TestExecutorsMinimalInitialization, spin_all_doesnt_require_warmup_with_cbgroup) +TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup_with_cbgroup) { using ExecutorType = TypeParam; + + // TODO(alsora): Enable when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + std::is_same() || + std::is_same()) + { + GTEST_SKIP(); + } + ExecutorType executor; // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event @@ -138,3 +143,99 @@ TYPED_TEST(TestExecutorsMinimalInitialization, spin_all_doesnt_require_warmup_wi // Verify that the callback is called as part of the spin above EXPECT_EQ(callback_count, 1u); } + +TYPED_TEST(TestExecutorsWarmup, spin_some_doesnt_require_warmup) +{ + using ExecutorType = TypeParam; + + // TODO(alsora): currently only the events-executor passes this test. + // Enable single-threaded and multi-threaded executors + // when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + !std::is_same()) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + // Add node to the executor before creating the entities + executor.add_node(node); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback)); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // NOTE: intra-process communication is enabled, so the subscription will immediately see + // the new message, no risk of race conditions where spin_some gets called before the + // message has been delivered. + executor.spin_some(); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} + +TYPED_TEST(TestExecutorsWarmup, spin_some_doesnt_require_warmup_with_cbgroup) +{ + using ExecutorType = TypeParam; + + // TODO(alsora): currently only the events-executor passes this test. + // Enable single-threaded and multi-threaded executors + // when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + !std::is_same()) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + auto callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + // Add callback group to the executor before creating the entities + executor.add_callback_group(callback_group, node->get_node_base_interface()); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback), sub_options); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // NOTE: intra-process communication is enabled, so the subscription will immediately see + // the new message, no risk of race conditions where spin_some gets called before the + // message has been delivered. + executor.spin_some(); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} From 4b7c4341b4678aebf0afd60cec52247449d0c2c8 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 6 Sep 2024 14:48:09 +0200 Subject: [PATCH 6/6] add missing include directives Signed-off-by: Alberto Soragna --- rclcpp/test/rclcpp/executors/test_executors_warmup.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp b/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp index 524191b885..7ab26a9da9 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// 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. @@ -20,8 +20,11 @@ #include #include +#include #include #include +#include +#include #include "rclcpp/rclcpp.hpp"