From 5d83c114da2f39c014456b343648c3c2104171fd Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Tue, 17 Dec 2024 17:09:20 +0100 Subject: [PATCH 1/5] test: Added test for broken executor behavior Signed-off-by: Janosch Machowinski --- .../test/rclcpp/executors/test_executors.cpp | 118 ++++++++++++++++++ 1 file changed, 118 insertions(+) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 011e776aa5..98b7f59c4d 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -889,3 +889,121 @@ TYPED_TEST(TestExecutors, testRaceDropCallbackGroupFromSecondThread) executor.cancel(); executor_thread.join(); } + +TYPED_TEST(TestExecutors, dropSomeTimer) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto node = std::make_shared("test_node"); + + bool timer1_works = false; + bool timer2_works = false; + + auto timer1 = node->create_timer(std::chrono::milliseconds(10), [&timer1_works]() { + timer1_works = true; + }); + auto timer2 = node->create_timer(std::chrono::milliseconds(10), [&timer2_works]() { + timer2_works = true; + }); + + executor.add_node(node); + + // first let's make sure that both timers work + auto max_end_time = std::chrono::steady_clock::now() + std::chrono::milliseconds(100); + while(!timer1_works || !timer2_works) { + // let the executor pick up the node and the timers + executor.spin_all(std::chrono::milliseconds(10)); + + const auto cur_time = std::chrono::steady_clock::now(); + ASSERT_LT(cur_time, max_end_time); + if(cur_time > max_end_time) { + return; + } + } + + // delete timer 2. Note, the executor uses an unordered map internally, to order + // the entities added to the rcl waitset therefore the order is kind of undefined, + // and this test may be flaky. In case it triggers, something is most likely + // really broken. + timer2.reset(); + + timer1_works = false; + timer2_works = false; + max_end_time = std::chrono::steady_clock::now() + std::chrono::milliseconds(100); + while(!timer1_works && !timer2_works) { + // let the executor pick up the node and the timers + executor.spin_all(std::chrono::milliseconds(10)); + + const auto cur_time = std::chrono::steady_clock::now(); + ASSERT_LT(cur_time, max_end_time); + if(cur_time > max_end_time) { + return; + } + } + + ASSERT_TRUE(timer1_works || timer2_works); +} + +TYPED_TEST(TestExecutors, dropSomeSubscription) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto node = std::make_shared("test_node"); + + bool sub1_works = false; + bool sub2_works = false; + + auto sub1 = node->create_subscription("/test_drop", 10, + [&sub1_works](const test_msgs::msg::Empty & msg) { + sub1_works = true; + }); + auto sub2 = node->create_subscription("/test_drop", 10, + [&sub2_works](const test_msgs::msg::Empty & msg) { + sub2_works = true; + }); + + auto pub = node->create_publisher("/test_drop", 10); + + executor.add_node(node); + + // first let's make sure that both timers work + auto max_end_time = std::chrono::steady_clock::now() + std::chrono::milliseconds(100); + while(!sub1_works || !sub2_works) { + pub->publish(test_msgs::msg::Empty()); + + // let the executor pick up the node and the timers + executor.spin_all(std::chrono::milliseconds(10)); + + const auto cur_time = std::chrono::steady_clock::now(); + ASSERT_LT(cur_time, max_end_time); + if(cur_time > max_end_time) { + return; + } + } + + // delete subscription 2. Note, the executor uses an unordered map internally, to order + // the entities added to the rcl waitset therefore the order is kind of undefined, + // and this test may be flaky. In case it triggers, something is most likely + // really broken. + sub2.reset(); + + sub1_works = false; + sub2_works = false; + max_end_time = std::chrono::steady_clock::now() + std::chrono::milliseconds(100); + while(!sub1_works && !sub2_works) { + pub->publish(test_msgs::msg::Empty()); + + // let the executor pick up the node and the timers + executor.spin_all(std::chrono::milliseconds(10)); + + const auto cur_time = std::chrono::steady_clock::now(); + ASSERT_LT(cur_time, max_end_time); + if(cur_time > max_end_time) { + return; + } + } + + ASSERT_TRUE(sub1_works || sub2_works); +} From 959b86e05cf2402e3bba8891e24b9471559ef1c3 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Tue, 17 Dec 2024 17:09:41 +0100 Subject: [PATCH 2/5] fix: Prune dynamic storage if needed Signed-off-by: Janosch Machowinski --- rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 8f97596218..f977025438 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -216,6 +216,11 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo shared_waitables_ ); + if(this->needs_pruning_) { + this->storage_prune_deleted_entities(); + this->needs_pruning_ = false; + } + this->storage_release_ownerships(); } From b7c45f03a2d749c1d68e050893a0d8e2a9e654cf Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Tue, 17 Dec 2024 18:18:09 +0100 Subject: [PATCH 3/5] fix(static_storage): Throw if an invalid entry was detected Signed-off-by: Janosch Machowinski --- .../include/rclcpp/wait_set_policies/static_storage.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 7f5cad74ad..9d9058bbc8 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -160,6 +160,15 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom services_, waitables_ ); + + if(this->needs_pruning_) { + // we need to throw here, as the indexing of the rcl_waitset is broken, + // in case of invalid entries + + throw std::runtime_error( + "StaticStorage : storage_rebuild_rcl_wait_set: Detected" + " invalid entity in static entity storage"); + } } // storage_add_subscription() explicitly not declared here From dc62e9e9977a1619672bf7fefa7f8801ba54cd9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 19 Dec 2024 07:48:24 +0100 Subject: [PATCH 4/5] Add executor tests for testing destroying nodes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../test/rclcpp/executors/test_executors.cpp | 116 ++++++++++++++++++ 1 file changed, 116 insertions(+) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 98b7f59c4d..863f9c442c 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -945,6 +945,59 @@ TYPED_TEST(TestExecutors, dropSomeTimer) ASSERT_TRUE(timer1_works || timer2_works); } +TYPED_TEST(TestExecutors, dropSomeNodeWithTimer) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto node1 = std::make_shared("test_node_1"); + auto node2 = std::make_shared("test_node_2"); + + bool timer1_works = false; + bool timer2_works = false; + + auto timer1 = node1->create_timer(std::chrono::milliseconds(10), [&timer1_works]() { + timer1_works = true; + }); + auto timer2 = node2->create_timer(std::chrono::milliseconds(10), [&timer2_works]() { + timer2_works = true; + }); + + executor.add_node(node1); + executor.add_node(node2); + + // first let's make sure that both timers work + auto max_end_time = std::chrono::steady_clock::now() + std::chrono::milliseconds(100); + while(!timer1_works || !timer2_works) { + // let the executor pick up the node and the timers + executor.spin_all(std::chrono::milliseconds(10)); + + const auto cur_time = std::chrono::steady_clock::now(); + ASSERT_LT(cur_time, max_end_time); + if(cur_time > max_end_time) { + return; + } + } + + // delete node 1. + node1 = nullptr; + + timer2_works = false; + max_end_time = std::chrono::steady_clock::now() + std::chrono::milliseconds(100); + while(!timer2_works) { + // let the executor pick up the node and the timer + executor.spin_all(std::chrono::milliseconds(10)); + + const auto cur_time = std::chrono::steady_clock::now(); + ASSERT_LT(cur_time, max_end_time); + if(cur_time > max_end_time) { + return; + } + } + + ASSERT_TRUE(timer2_works); +} + TYPED_TEST(TestExecutors, dropSomeSubscription) { using ExecutorType = TypeParam; @@ -1007,3 +1060,66 @@ TYPED_TEST(TestExecutors, dropSomeSubscription) ASSERT_TRUE(sub1_works || sub2_works); } + +TYPED_TEST(TestExecutors, dropSomeNodesWithSubscription) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto node = std::make_shared("test_node"); + auto node1 = std::make_shared("test_node_1"); + auto node2 = std::make_shared("test_node_2"); + + bool sub1_works = false; + bool sub2_works = false; + + auto sub1 = node1->create_subscription("/test_drop", 10, + [&sub1_works](const test_msgs::msg::Empty & msg) { + sub1_works = true; + }); + auto sub2 = node2->create_subscription("/test_drop", 10, + [&sub2_works](const test_msgs::msg::Empty & msg) { + sub2_works = true; + }); + + auto pub = node->create_publisher("/test_drop", 10); + + executor.add_node(node); + executor.add_node(node1); + executor.add_node(node2); + + // first let's make sure that both subscribers work + auto max_end_time = std::chrono::steady_clock::now() + std::chrono::milliseconds(100); + while(!sub1_works || !sub2_works) { + pub->publish(test_msgs::msg::Empty()); + + // let the executor pick up the node and the timers + executor.spin_all(std::chrono::milliseconds(10)); + + const auto cur_time = std::chrono::steady_clock::now(); + ASSERT_LT(cur_time, max_end_time); + if(cur_time > max_end_time) { + return; + } + } + + // delete node 2. + node2 = nullptr; + + sub1_works = false; + max_end_time = std::chrono::steady_clock::now() + std::chrono::milliseconds(100); + while(!sub1_works) { + pub->publish(test_msgs::msg::Empty()); + + // let the executor pick up the node and the timers + executor.spin_all(std::chrono::milliseconds(10)); + + const auto cur_time = std::chrono::steady_clock::now(); + ASSERT_LT(cur_time, max_end_time); + if(cur_time > max_end_time) { + return; + } + } + + ASSERT_TRUE(sub1_works); +} From 0c429459c6ac3685cf58545e25bb028957aea272 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 9 Jan 2025 18:29:58 +0100 Subject: [PATCH 5/5] chore: style and warning fixes Signed-off-by: Janosch Machowinski --- .../wait_set_policies/dynamic_storage.hpp | 2 +- .../wait_set_policies/static_storage.hpp | 4 +-- .../test/rclcpp/executors/test_executors.cpp | 32 +++---------------- 3 files changed, 7 insertions(+), 31 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index f977025438..3b823144ec 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -216,7 +216,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo shared_waitables_ ); - if(this->needs_pruning_) { + if (this->needs_pruning_) { this->storage_prune_deleted_entities(); this->needs_pruning_ = false; } diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 9d9058bbc8..d07aa3fb57 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -166,8 +166,8 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom // in case of invalid entries throw std::runtime_error( - "StaticStorage : storage_rebuild_rcl_wait_set: Detected" - " invalid entity in static entity storage"); + "StaticStorage::storage_rebuild_rcl_wait_set(): entity weak_ptr " + "unexpectedly expired in static entity storage"); } } diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 863f9c442c..14c7627631 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -917,9 +917,6 @@ TYPED_TEST(TestExecutors, dropSomeTimer) const auto cur_time = std::chrono::steady_clock::now(); ASSERT_LT(cur_time, max_end_time); - if(cur_time > max_end_time) { - return; - } } // delete timer 2. Note, the executor uses an unordered map internally, to order @@ -937,9 +934,6 @@ TYPED_TEST(TestExecutors, dropSomeTimer) const auto cur_time = std::chrono::steady_clock::now(); ASSERT_LT(cur_time, max_end_time); - if(cur_time > max_end_time) { - return; - } } ASSERT_TRUE(timer1_works || timer2_works); @@ -974,9 +968,6 @@ TYPED_TEST(TestExecutors, dropSomeNodeWithTimer) const auto cur_time = std::chrono::steady_clock::now(); ASSERT_LT(cur_time, max_end_time); - if(cur_time > max_end_time) { - return; - } } // delete node 1. @@ -990,9 +981,6 @@ TYPED_TEST(TestExecutors, dropSomeNodeWithTimer) const auto cur_time = std::chrono::steady_clock::now(); ASSERT_LT(cur_time, max_end_time); - if(cur_time > max_end_time) { - return; - } } ASSERT_TRUE(timer2_works); @@ -1009,11 +997,11 @@ TYPED_TEST(TestExecutors, dropSomeSubscription) bool sub2_works = false; auto sub1 = node->create_subscription("/test_drop", 10, - [&sub1_works](const test_msgs::msg::Empty & msg) { + [&sub1_works](const test_msgs::msg::Empty &) { sub1_works = true; }); auto sub2 = node->create_subscription("/test_drop", 10, - [&sub2_works](const test_msgs::msg::Empty & msg) { + [&sub2_works](const test_msgs::msg::Empty &) { sub2_works = true; }); @@ -1031,9 +1019,6 @@ TYPED_TEST(TestExecutors, dropSomeSubscription) const auto cur_time = std::chrono::steady_clock::now(); ASSERT_LT(cur_time, max_end_time); - if(cur_time > max_end_time) { - return; - } } // delete subscription 2. Note, the executor uses an unordered map internally, to order @@ -1053,9 +1038,6 @@ TYPED_TEST(TestExecutors, dropSomeSubscription) const auto cur_time = std::chrono::steady_clock::now(); ASSERT_LT(cur_time, max_end_time); - if(cur_time > max_end_time) { - return; - } } ASSERT_TRUE(sub1_works || sub2_works); @@ -1074,11 +1056,11 @@ TYPED_TEST(TestExecutors, dropSomeNodesWithSubscription) bool sub2_works = false; auto sub1 = node1->create_subscription("/test_drop", 10, - [&sub1_works](const test_msgs::msg::Empty & msg) { + [&sub1_works](const test_msgs::msg::Empty &) { sub1_works = true; }); auto sub2 = node2->create_subscription("/test_drop", 10, - [&sub2_works](const test_msgs::msg::Empty & msg) { + [&sub2_works](const test_msgs::msg::Empty &) { sub2_works = true; }); @@ -1098,9 +1080,6 @@ TYPED_TEST(TestExecutors, dropSomeNodesWithSubscription) const auto cur_time = std::chrono::steady_clock::now(); ASSERT_LT(cur_time, max_end_time); - if(cur_time > max_end_time) { - return; - } } // delete node 2. @@ -1116,9 +1095,6 @@ TYPED_TEST(TestExecutors, dropSomeNodesWithSubscription) const auto cur_time = std::chrono::steady_clock::now(); ASSERT_LT(cur_time, max_end_time); - if(cur_time > max_end_time) { - return; - } } ASSERT_TRUE(sub1_works);