From 6132f81c13d083b79ee761a102b5bae1496a5918 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 27 Jul 2023 17:43:33 +0200 Subject: [PATCH 1/4] Add spin_all shortcut Signed-off-by: Tony Najjar --- rclcpp/include/rclcpp/executor.hpp | 15 +++++++++++++++ rclcpp/include/rclcpp/executors.hpp | 12 ++++++++++++ rclcpp/src/rclcpp/executor.cpp | 16 ++++++++++++++++ rclcpp/src/rclcpp/executors.cpp | 15 +++++++++++++++ 4 files changed, 58 insertions(+) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 2d5ca2149a..b4f1b9d8f9 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -297,6 +297,21 @@ class Executor virtual void spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)); + /// Add a node, complete all immediately available work exhaustively, and remove the node. + /** + * \param[in] node Shared pointer to the node to add. + */ + RCLCPP_PUBLIC + void + spin_node_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds max_duration); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + void + spin_node_all(std::shared_ptr node, std::chrono::nanoseconds max_duration); + /// Collect and execute work repeatedly within a duration or until no more work is available. /** * This function can be overridden. The default implementation is suitable for a diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index f7ff06055f..55a38709a7 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -29,6 +29,18 @@ namespace rclcpp { +/// Create a default single-threaded executor and execute all available work exhaustively. +/** \param[in] node_ptr Shared pointer to the node to spin. */ +RCLCPP_PUBLIC +void +spin_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::nanoseconds max_duration); + +RCLCPP_PUBLIC +void +spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration); + /// Create a default single-threaded executor and execute any immediately available work. /** \param[in] node_ptr Shared pointer to the node to spin. */ RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 3deb2d1706..9de5883a34 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -431,6 +431,22 @@ void Executor::spin_some(std::chrono::nanoseconds max_duration) return this->spin_some_impl(max_duration, false); } +void +Executor::spin_node_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds max_duration) +{ + this->add_node(node, false); + spin_all(max_duration); + this->remove_node(node, false); +} + +void +Executor::spin_node_all(std::shared_ptr node, std::chrono::nanoseconds max_duration) +{ + this->spin_node_all(node->get_node_base_interface(), max_duration); +} + void Executor::spin_all(std::chrono::nanoseconds max_duration) { if (max_duration < 0ns) { diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 0a900c07da..3ea8d658ad 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -14,6 +14,21 @@ #include "rclcpp/executors.hpp" +void +rclcpp::spin_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::nanoseconds max_duration) +{ + rclcpp::executors::SingleThreadedExecutor exec; + exec.spin_node_all(node_ptr, max_duration); +} + +void +rclcpp::spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration) +{ + rclcpp::spin_all(node_ptr->get_node_base_interface(), max_duration); +} + void rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { From 6fb8370fb9343456470e7f75d75178597b665cc3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 7 Aug 2023 12:52:33 +0200 Subject: [PATCH 2/4] Add tests spin Signed-off-by: Tony Najjar --- rclcpp/test/rclcpp/test_executor.cpp | 34 ++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index bdbb0a1079..021e6da79d 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -546,6 +546,40 @@ TEST_F(TestExecutor, spin_node_once_node) { EXPECT_TRUE(spin_called); } +TEST_F(TestExecutor, spin_node_all_base_interface) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_all(node->get_node_base_interface()); + EXPECT_TRUE(spin_called); +} + +TEST_F(TestExecutor, spin_node_all_node) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_all(node); + EXPECT_TRUE(spin_called); +} + TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); From bd8e8d292690e4ab3ce6f13225a2f4a67092f6a2 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 7 Aug 2023 13:18:44 +0200 Subject: [PATCH 3/4] fix test Signed-off-by: Tony Najjar --- rclcpp/test/rclcpp/test_executor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 021e6da79d..a62cd08dff 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -559,7 +559,7 @@ TEST_F(TestExecutor, spin_node_all_base_interface) { // Wait for the wall timer to have expired. std::this_thread::sleep_for(std::chrono::milliseconds(50)); EXPECT_FALSE(spin_called); - dummy.spin_node_all(node->get_node_base_interface()); + dummy.spin_node_all(node->get_node_base_interface(), std::chrono::nanoseconds(1000)); EXPECT_TRUE(spin_called); } @@ -576,7 +576,7 @@ TEST_F(TestExecutor, spin_node_all_node) { // Wait for the wall timer to have expired. std::this_thread::sleep_for(std::chrono::milliseconds(50)); EXPECT_FALSE(spin_called); - dummy.spin_node_all(node); + dummy.spin_node_all(node, std::chrono::nanoseconds(1000)); EXPECT_TRUE(spin_called); } From 9fb218d15d0c388af9778c657d3bb5f214f18ea9 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 8 Aug 2023 16:34:06 +0200 Subject: [PATCH 4/4] fix too little timeout Signed-off-by: Tony Najjar --- rclcpp/test/rclcpp/test_executor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index a62cd08dff..706b80aef1 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -559,7 +559,7 @@ TEST_F(TestExecutor, spin_node_all_base_interface) { // Wait for the wall timer to have expired. std::this_thread::sleep_for(std::chrono::milliseconds(50)); EXPECT_FALSE(spin_called); - dummy.spin_node_all(node->get_node_base_interface(), std::chrono::nanoseconds(1000)); + dummy.spin_node_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); EXPECT_TRUE(spin_called); } @@ -576,7 +576,7 @@ TEST_F(TestExecutor, spin_node_all_node) { // Wait for the wall timer to have expired. std::this_thread::sleep_for(std::chrono::milliseconds(50)); EXPECT_FALSE(spin_called); - dummy.spin_node_all(node, std::chrono::nanoseconds(1000)); + dummy.spin_node_all(node, std::chrono::milliseconds(50)); EXPECT_TRUE(spin_called); }