From 57a3a7d206184a59b20cd8f22f3fd9150af07913 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Mon, 11 May 2020 20:15:26 +0530 Subject: [PATCH 1/5] Allow spin_until_future_complete to accept std::future Signed-off-by: Sarthak Mittal --- rclcpp/include/rclcpp/executor.hpp | 49 +++++++++++++++++ rclcpp/include/rclcpp/executors.hpp | 54 +++++++++++++++++++ .../static_single_threaded_executor.hpp | 48 +++++++++++++++++ 3 files changed, 151 insertions(+) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 39acba3ec4..3d54ad7359 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -237,6 +237,55 @@ class Executor return FutureReturnCode::INTERRUPTED; } + template + FutureReturnCode + spin_until_future_complete( + const std::future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete + // inside a callback executed by an executor. + + // Check the future before entering the while loop. + // If the future is already complete, don't try to spin. + std::future_status status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + + auto end_time = std::chrono::steady_clock::now(); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; + } + std::chrono::nanoseconds timeout_left = timeout_ns; + + while (rclcpp::ok(this->context_)) { + // Do one item of work. + spin_once(timeout_left); + // Check if the future is set, return SUCCESS if it is. + status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout_ns < std::chrono::nanoseconds::zero()) { + continue; + } + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { + return FutureReturnCode::TIMEOUT; + } + // Subtract the elapsed time from the original timeout. + timeout_left = std::chrono::duration_cast(end_time - now); + } + + // The future did not complete before ok() returned false, return INTERRUPTED. + return FutureReturnCode::INTERRUPTED; + } + /// Cancel any running spin* function, causing it to return. /** * This function can be called asynchonously from any thread. diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index ec55d1309e..371e191fea 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -82,6 +82,22 @@ spin_node_until_future_complete( return retcode; } +template +rclcpp::FutureReturnCode +spin_node_until_future_complete( + rclcpp::Executor & executor, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + const std::future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete + // inside a callback executed by an executor. + executor.add_node(node_ptr); + auto retcode = executor.spin_until_future_complete(future, timeout); + executor.remove_node(node_ptr); + return retcode; +} + template rclcpp::FutureReturnCode @@ -98,6 +114,22 @@ spin_node_until_future_complete( timeout); } +template +rclcpp::FutureReturnCode +spin_node_until_future_complete( + rclcpp::Executor & executor, + std::shared_ptr node_ptr, + const std::future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::executors::spin_node_until_future_complete( + executor, + node_ptr->get_node_base_interface(), + future, + timeout); +} + } // namespace executors template @@ -111,6 +143,17 @@ spin_until_future_complete( return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); } +template +rclcpp::FutureReturnCode +spin_until_future_complete( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + const std::future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + rclcpp::executors::SingleThreadedExecutor executor; + return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); +} + template rclcpp::FutureReturnCode @@ -122,6 +165,17 @@ spin_until_future_complete( return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); } +template +rclcpp::FutureReturnCode +spin_until_future_complete( + std::shared_ptr node_ptr, + const std::future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); +} + } // namespace rclcpp #endif // RCLCPP__EXECUTORS_HPP_ diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 1e1668bcb3..5ed6b71ba9 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -191,6 +191,54 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor return rclcpp::FutureReturnCode::INTERRUPTED; } + template + rclcpp::FutureReturnCode + spin_until_future_complete( + std::future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + std::future_status status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return rclcpp::FutureReturnCode::SUCCESS; + } + + auto end_time = std::chrono::steady_clock::now(); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; + } + std::chrono::nanoseconds timeout_left = timeout_ns; + + entities_collector_ = std::make_shared(); + entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + + while (rclcpp::ok(this->context_)) { + // Do one set of work. + entities_collector_->refresh_wait_set(timeout_left); + execute_ready_executables(); + // Check if the future is set, return SUCCESS if it is. + status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return rclcpp::FutureReturnCode::SUCCESS; + } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout_ns < std::chrono::nanoseconds::zero()) { + continue; + } + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { + return rclcpp::FutureReturnCode::TIMEOUT; + } + // Subtract the elapsed time from the original timeout. + timeout_left = std::chrono::duration_cast(end_time - now); + } + + // The future did not complete before ok() returned false, return INTERRUPTED. + return rclcpp::FutureReturnCode::INTERRUPTED; + } + protected: /// Check which executables in ExecutableList struct are ready from wait_set and execute them. /** From 51399e514a10d691106675ea83139209c76cf851 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 29 May 2020 04:11:25 +0530 Subject: [PATCH 2/5] Add templated function to take std::future and std::shared_future Signed-off-by: Sarthak Mittal --- rclcpp/include/rclcpp/executor.hpp | 53 +-------------- rclcpp/include/rclcpp/executors.hpp | 66 ++----------------- .../static_single_threaded_executor.hpp | 52 +-------------- 3 files changed, 10 insertions(+), 161 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 3d54ad7359..9afdadf9f8 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -188,59 +188,10 @@ class Executor * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template + template FutureReturnCode spin_until_future_complete( - const std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) - { - // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete - // inside a callback executed by an executor. - - // Check the future before entering the while loop. - // If the future is already complete, don't try to spin. - std::future_status status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - - auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( - timeout); - if (timeout_ns > std::chrono::nanoseconds::zero()) { - end_time += timeout_ns; - } - std::chrono::nanoseconds timeout_left = timeout_ns; - - while (rclcpp::ok(this->context_)) { - // Do one item of work. - spin_once(timeout_left); - // Check if the future is set, return SUCCESS if it is. - status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - // If the original timeout is < 0, then this is blocking, never TIMEOUT. - if (timeout_ns < std::chrono::nanoseconds::zero()) { - continue; - } - // Otherwise check if we still have time to wait, return TIMEOUT if not. - auto now = std::chrono::steady_clock::now(); - if (now >= end_time) { - return FutureReturnCode::TIMEOUT; - } - // Subtract the elapsed time from the original timeout. - timeout_left = std::chrono::duration_cast(end_time - now); - } - - // The future did not complete before ok() returned false, return INTERRUPTED. - return FutureReturnCode::INTERRUPTED; - } - - template - FutureReturnCode - spin_until_future_complete( - const std::future & future, + const FutureResponseT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 371e191fea..ea37f662d0 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -66,12 +66,12 @@ using rclcpp::executors::SingleThreadedExecutor; * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ -template +template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const std::shared_future & future, + const FutureResponseT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete @@ -82,45 +82,13 @@ spin_node_until_future_complete( return retcode; } -template -rclcpp::FutureReturnCode -spin_node_until_future_complete( - rclcpp::Executor & executor, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const std::future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) -{ - // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete - // inside a callback executed by an executor. - executor.add_node(node_ptr); - auto retcode = executor.spin_until_future_complete(future, timeout); - executor.remove_node(node_ptr); - return retcode; -} - -template -rclcpp::FutureReturnCode -spin_node_until_future_complete( - rclcpp::Executor & executor, - std::shared_ptr node_ptr, - const std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) -{ - return rclcpp::executors::spin_node_until_future_complete( - executor, - node_ptr->get_node_base_interface(), - future, - timeout); -} - -template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, std::shared_ptr node_ptr, - const std::future & future, + const FutureResponseT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::executors::spin_node_until_future_complete( @@ -136,18 +104,7 @@ template & future, - std::chrono::duration timeout = std::chrono::duration(-1)) -{ - rclcpp::executors::SingleThreadedExecutor executor; - return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); -} - -template -rclcpp::FutureReturnCode -spin_until_future_complete( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const std::future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { rclcpp::executors::SingleThreadedExecutor executor; @@ -159,18 +116,7 @@ template node_ptr, - const std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) -{ - return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); -} - -template -rclcpp::FutureReturnCode -spin_until_future_complete( - std::shared_ptr node_ptr, - const std::future & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 5ed6b71ba9..7ad414178b 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -143,58 +143,10 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor * exec.add_node(node); * exec.spin_until_future_complete(future); */ - template + template rclcpp::FutureReturnCode spin_until_future_complete( - std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) - { - std::future_status status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return rclcpp::FutureReturnCode::SUCCESS; - } - - auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( - timeout); - if (timeout_ns > std::chrono::nanoseconds::zero()) { - end_time += timeout_ns; - } - std::chrono::nanoseconds timeout_left = timeout_ns; - - entities_collector_ = std::make_shared(); - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); - - while (rclcpp::ok(this->context_)) { - // Do one set of work. - entities_collector_->refresh_wait_set(timeout_left); - execute_ready_executables(); - // Check if the future is set, return SUCCESS if it is. - status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return rclcpp::FutureReturnCode::SUCCESS; - } - // If the original timeout is < 0, then this is blocking, never TIMEOUT. - if (timeout_ns < std::chrono::nanoseconds::zero()) { - continue; - } - // Otherwise check if we still have time to wait, return TIMEOUT if not. - auto now = std::chrono::steady_clock::now(); - if (now >= end_time) { - return rclcpp::FutureReturnCode::TIMEOUT; - } - // Subtract the elapsed time from the original timeout. - timeout_left = std::chrono::duration_cast(end_time - now); - } - - // The future did not complete before ok() returned false, return INTERRUPTED. - return rclcpp::FutureReturnCode::INTERRUPTED; - } - - template - rclcpp::FutureReturnCode - spin_until_future_complete( - std::future & future, + FutureResponseT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { std::future_status status = future.wait_for(std::chrono::seconds(0)); From 81aa00af1740c774c64cb76c450014e06c35989c Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 30 May 2020 00:47:06 +0530 Subject: [PATCH 3/5] Add unit tests for spin_until_future_complete Signed-off-by: Sarthak Mittal --- rclcpp/test/test_executor.cpp | 44 +++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/test_executor.cpp index 95371415fc..ea31c567fa 100644 --- a/rclcpp/test/test_executor.cpp +++ b/rclcpp/test/test_executor.cpp @@ -67,3 +67,47 @@ TEST_F(TestExecutors, addTemporaryNode) { executor.add_node(std::make_shared("temporary_node")); EXPECT_NO_THROW(executor.spin_some()); } + +// Make sure that the spin_until_future_complete works correctly with std::future +TEST_F(TestExecutors, testSpinUntilFutureComplete) { + rclcpp::executors::SingleThreadedExecutor executor; + std::future future; + rclcpp::FutureReturnCode ret; + + // test success + future = std::async([]() { + return; + }); + ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + + // test timeout + future = std::async([]() { + std::this_thread::sleep_for(20ms); + return; + }); + ret = executor.spin_until_future_complete(future, 10ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); +} + +// Make sure that the spin_until_future_complete works correctly with std::shared_future +TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) { + rclcpp::executors::SingleThreadedExecutor executor; + std::future future; + rclcpp::FutureReturnCode ret; + + // test success + future = std::async([]() { + return; + }); + ret = executor.spin_until_future_complete(future.share(), 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + + // test timeout + future = std::async([]() { + std::this_thread::sleep_for(20ms); + return; + }); + ret = executor.spin_until_future_complete(future.share(), 10ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); +} From 21388d07d89614f44a3cba9beaa734ac71db5833 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 30 May 2020 00:56:56 +0530 Subject: [PATCH 4/5] Fix uncrustify errors Signed-off-by: Sarthak Mittal --- rclcpp/test/test_executor.cpp | 32 ++++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/test_executor.cpp index ea31c567fa..e2dd5bd174 100644 --- a/rclcpp/test/test_executor.cpp +++ b/rclcpp/test/test_executor.cpp @@ -75,17 +75,19 @@ TEST_F(TestExecutors, testSpinUntilFutureComplete) { rclcpp::FutureReturnCode ret; // test success - future = std::async([]() { - return; - }); + future = std::async( + []() { + return; + }); ret = executor.spin_until_future_complete(future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); // test timeout - future = std::async([]() { - std::this_thread::sleep_for(20ms); - return; - }); + future = std::async( + []() { + std::this_thread::sleep_for(20ms); + return; + }); ret = executor.spin_until_future_complete(future, 10ms); EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); } @@ -97,17 +99,19 @@ TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) { rclcpp::FutureReturnCode ret; // test success - future = std::async([]() { - return; - }); + future = std::async( + []() { + return; + }); ret = executor.spin_until_future_complete(future.share(), 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); // test timeout - future = std::async([]() { - std::this_thread::sleep_for(20ms); - return; - }); + future = std::async( + []() { + std::this_thread::sleep_for(20ms); + return; + }); ret = executor.spin_until_future_complete(future.share(), 10ms); EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); } From 31fbab3e6882700944893ab8a4c02652fdb41440 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 30 May 2020 01:18:22 +0530 Subject: [PATCH 5/5] Rename FutureResponseT to FutureT Signed-off-by: Sarthak Mittal --- rclcpp/include/rclcpp/executor.hpp | 4 ++-- rclcpp/include/rclcpp/executors.hpp | 8 ++++---- .../rclcpp/executors/static_single_threaded_executor.hpp | 4 ++-- rclcpp/test/test_executor.cpp | 8 ++++---- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 9afdadf9f8..77fb35ca2e 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -188,10 +188,10 @@ class Executor * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template + template FutureReturnCode spin_until_future_complete( - const FutureResponseT & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index ea37f662d0..36fb0d63cf 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -66,12 +66,12 @@ using rclcpp::executors::SingleThreadedExecutor; * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ -template +template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const FutureResponseT & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete @@ -82,13 +82,13 @@ spin_node_until_future_complete( return retcode; } -template rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, std::shared_ptr node_ptr, - const FutureResponseT & future, + const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::executors::spin_node_until_future_complete( diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 7ad414178b..25645e23f2 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -143,10 +143,10 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor * exec.add_node(node); * exec.spin_until_future_complete(future); */ - template + template rclcpp::FutureReturnCode spin_until_future_complete( - FutureResponseT & future, + FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { std::future_status status = future.wait_for(std::chrono::seconds(0)); diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/test_executor.cpp index e2dd5bd174..cdefec71e1 100644 --- a/rclcpp/test/test_executor.cpp +++ b/rclcpp/test/test_executor.cpp @@ -85,10 +85,10 @@ TEST_F(TestExecutors, testSpinUntilFutureComplete) { // test timeout future = std::async( []() { - std::this_thread::sleep_for(20ms); + std::this_thread::sleep_for(1s); return; }); - ret = executor.spin_until_future_complete(future, 10ms); + ret = executor.spin_until_future_complete(future, 100ms); EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); } @@ -109,9 +109,9 @@ TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) { // test timeout future = std::async( []() { - std::this_thread::sleep_for(20ms); + std::this_thread::sleep_for(1s); return; }); - ret = executor.spin_until_future_complete(future.share(), 10ms); + ret = executor.spin_until_future_complete(future.share(), 100ms); EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); }