From a1eea43e095cca6d3d5acb51bf500b21291510c7 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 10 Jul 2024 10:06:43 +0800 Subject: [PATCH 1/2] Release ownership of entities after spinning cancelled (#2556) * Release ownership of entities after spinning cancelled Signed-off-by: Barry Xu * Move release action to every exit point in different spin functions Signed-off-by: Barry Xu * Move wait_result_.reset() before setting spinning to false Signed-off-by: Barry Xu * Update test code Signed-off-by: Barry Xu * Move test code to test_executors.cpp Signed-off-by: Barry Xu --------- Signed-off-by: Barry Xu (cherry picked from commit 069a0018935b33a14632a1cdf4074984a1cf80fe) # Conflicts: # rclcpp/test/rclcpp/executors/test_executors.cpp --- rclcpp/src/rclcpp/executor.cpp | 6 ++--- .../executors/multi_threaded_executor.cpp | 2 +- .../executors/single_threaded_executor.cpp | 2 +- rclcpp/test/rclcpp/CMakeLists.txt | 2 +- .../test/rclcpp/executors/test_executors.cpp | 26 +++++++++++++++++++ 5 files changed, 32 insertions(+), 6 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 95b0d7fcc8..181f0c9a6a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -275,7 +275,7 @@ Executor::spin_until_future_complete_impl( if (spinning.exchange(true)) { throw std::runtime_error("spin_until_future_complete() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); while (rclcpp::ok(this->context_) && spinning.load()) { // Do one item of work. spin_once_impl(timeout_left); @@ -364,7 +364,7 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); // clear the wait result and wait for work without blocking to collect the work // for the first time @@ -431,7 +431,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout) if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); spin_once_impl(timeout); } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 2d5c76e817..130b4d953f 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -55,7 +55,7 @@ MultiThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); std::vector threads; size_t thread_id = 0; { diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 975733b497..689bbae398 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -30,7 +30,7 @@ SingleThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); // Clear any previous result and rebuild the waitset this->wait_result_.reset(); diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 844ca630ac..5c607a2d6f 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -598,7 +598,7 @@ ament_add_gtest(test_executor test_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_executor) - target_link_libraries(test_executor ${PROJECT_NAME} mimick) + target_link_libraries(test_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() ament_add_gtest(test_graph_listener test_graph_listener.cpp) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index a82b702db5..959bca8632 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -39,6 +39,7 @@ #include "rclcpp/time_source.hpp" #include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" #include "./executor_types.hpp" @@ -878,6 +879,7 @@ TEST(TestExecutors, testSpinWithNonDefaultContext) rclcpp::shutdown(non_default_context); } +<<<<<<< HEAD template class TestBusyWaiting : public ::testing::Test { @@ -1028,4 +1030,28 @@ TYPED_TEST(TestBusyWaiting, test_spin) this->check_for_busy_waits(start_time); // this should get the initial trigger, and the follow up from in the callback ASSERT_EQ(this->waitable->get_count(), 2u); +======= +TYPED_TEST(TestExecutors, release_ownership_entity_after_spinning_cancel) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto future = std::async(std::launch::async, [&executor] {executor.spin();}); + + auto node = std::make_shared("test_node"); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) { + }; + auto server = node->create_service("test_service", callback); + while (!executor.is_spinning()) { + std::this_thread::sleep_for(50ms); + } + executor.add_node(node); + std::this_thread::sleep_for(50ms); + executor.cancel(); + std::future_status future_status = future.wait_for(1s); + EXPECT_EQ(future_status, std::future_status::ready); + + EXPECT_EQ(server.use_count(), 1); +>>>>>>> 069a0018 (Release ownership of entities after spinning cancelled (#2556)) } From 29c3020f6854613fc560ff037195f8549ac190a7 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 10 Jul 2024 16:02:33 +0800 Subject: [PATCH 2/2] Fix backport issue (#2581) Signed-off-by: Barry Xu --- .../test/rclcpp/executors/test_executors.cpp | 49 +++++++++---------- 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 959bca8632..789843125e 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -879,7 +879,30 @@ TEST(TestExecutors, testSpinWithNonDefaultContext) rclcpp::shutdown(non_default_context); } -<<<<<<< HEAD +TYPED_TEST(TestExecutors, release_ownership_entity_after_spinning_cancel) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto future = std::async(std::launch::async, [&executor] {executor.spin();}); + + auto node = std::make_shared("test_node"); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) { + }; + auto server = node->create_service("test_service", callback); + while (!executor.is_spinning()) { + std::this_thread::sleep_for(50ms); + } + executor.add_node(node); + std::this_thread::sleep_for(50ms); + executor.cancel(); + std::future_status future_status = future.wait_for(1s); + EXPECT_EQ(future_status, std::future_status::ready); + + EXPECT_EQ(server.use_count(), 1); +} + template class TestBusyWaiting : public ::testing::Test { @@ -1030,28 +1053,4 @@ TYPED_TEST(TestBusyWaiting, test_spin) this->check_for_busy_waits(start_time); // this should get the initial trigger, and the follow up from in the callback ASSERT_EQ(this->waitable->get_count(), 2u); -======= -TYPED_TEST(TestExecutors, release_ownership_entity_after_spinning_cancel) -{ - using ExecutorType = TypeParam; - ExecutorType executor; - - auto future = std::async(std::launch::async, [&executor] {executor.spin();}); - - auto node = std::make_shared("test_node"); - auto callback = []( - const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) { - }; - auto server = node->create_service("test_service", callback); - while (!executor.is_spinning()) { - std::this_thread::sleep_for(50ms); - } - executor.add_node(node); - std::this_thread::sleep_for(50ms); - executor.cancel(); - std::future_status future_status = future.wait_for(1s); - EXPECT_EQ(future_status, std::future_status::ready); - - EXPECT_EQ(server.use_count(), 1); ->>>>>>> 069a0018 (Release ownership of entities after spinning cancelled (#2556)) }