@@ -41,28 +41,38 @@ template<typename T>
4141class TestExecutors : public ::testing::Test
4242{
4343public:
44- void SetUp ()
44+ static void SetUpTestCase ()
4545 {
4646 rclcpp::init (0 , nullptr );
47- node = std::make_shared<rclcpp::Node>( " node " , " ns " );
47+ }
4848
49- callback_count = 0 ;
50- std::stringstream topic_name;
49+ static void TearDownTestCase ()
50+ {
51+ rclcpp::shutdown ();
52+ }
53+
54+ void SetUp ()
55+ {
5156 const auto test_info = ::testing::UnitTest::GetInstance ()->current_test_info ();
52- topic_name << " topic_" << test_info->test_case_name () << " _" << test_info->name ();
57+ std::stringstream test_name;
58+ test_name << test_info->test_case_name () << " _" << test_info->name ();
59+ node = std::make_shared<rclcpp::Node>(" node" , test_name.str ());
5360
54- publisher = node->create_publisher <std_msgs::msg::Empty>(topic_name.str (), rclcpp::QoS (10 ));
61+ callback_count = 0 ;
62+
63+ const std::string topic_name = std::string (" topic_" ) + test_name.str ();
64+ publisher = node->create_publisher <std_msgs::msg::Empty>(topic_name, rclcpp::QoS (10 ));
5565 auto callback = [this ](std_msgs::msg::Empty::SharedPtr) {this ->callback_count ++;};
5666 subscription =
5767 node->create_subscription <std_msgs::msg::Empty>(
58- topic_name. str () , rclcpp::QoS (10 ), std::move (callback));
68+ topic_name, rclcpp::QoS (10 ), std::move (callback));
5969 }
6070
6171 void TearDown ()
6272 {
63- if ( rclcpp::ok ()) {
64- rclcpp::shutdown ();
65- }
73+ publisher. reset ();
74+ subscription. reset ();
75+ node. reset ();
6676 }
6777
6878 rclcpp::Node::SharedPtr node;
@@ -147,7 +157,7 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) {
147157 std::thread spinner ([&]() {EXPECT_NO_THROW (executor.spin ());});
148158
149159 std::this_thread::sleep_for (50ms);
150- rclcpp::shutdown ();
160+ executor. cancel ();
151161 spinner.join ();
152162}
153163
@@ -158,6 +168,7 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) {
158168 ExecutorType executor2;
159169 EXPECT_NO_THROW (executor1.add_node (this ->node ));
160170 EXPECT_THROW (executor2.add_node (this ->node ), std::runtime_error);
171+ executor1.remove_node (this ->node , true );
161172}
162173
163174// Check simple spin example
@@ -172,15 +183,15 @@ TYPED_TEST(TestExecutors, spinWithTimer) {
172183 std::thread spinner ([&]() {executor.spin ();});
173184
174185 auto start = std::chrono::steady_clock::now ();
175- while (!timer_completed && (std::chrono::steady_clock::now () - start) < 1s ) {
186+ while (!timer_completed && (std::chrono::steady_clock::now () - start) < 10s ) {
176187 std::this_thread::sleep_for (1ms);
177188 }
178189
179190 EXPECT_TRUE (timer_completed);
180-
181- // Shutdown needs to be called before join, so that executor.spin() returns.
182- rclcpp::shutdown ();
191+ // Cancel needs to be called before join, so that executor.spin() returns.
192+ executor.cancel ();
183193 spinner.join ();
194+ executor.remove_node (this ->node , true );
184195}
185196
186197TYPED_TEST (TestExecutors, spinWhileAlreadySpinning) {
@@ -195,16 +206,17 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) {
195206 // Sleep for a short time to verify executor.spin() is going, and didn't throw.
196207
197208 auto start = std::chrono::steady_clock::now ();
198- while (!timer_completed && (std::chrono::steady_clock::now () - start) < 1s ) {
209+ while (!timer_completed && (std::chrono::steady_clock::now () - start) < 10s ) {
199210 std::this_thread::sleep_for (1ms);
200211 }
201212
202213 EXPECT_TRUE (timer_completed);
203214 EXPECT_THROW (executor.spin (), std::runtime_error);
204215
205216 // Shutdown needs to be called before join, so that executor.spin() returns.
206- rclcpp::shutdown ();
217+ executor. cancel ();
207218 spinner.join ();
219+ executor.remove_node (this ->node , true );
208220}
209221
210222// Check executor exits immediately if future is complete.
@@ -223,7 +235,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
223235 auto start = std::chrono::steady_clock::now ();
224236 auto shared_future = future.share ();
225237 auto ret = executor.spin_until_future_complete (shared_future, 1s);
226-
238+ executor. remove_node ( this -> node , true );
227239 // Check it didn't reach timeout
228240 EXPECT_GT (500ms, (std::chrono::steady_clock::now () - start));
229241 EXPECT_EQ (rclcpp::FutureReturnCode::SUCCESS, ret);
@@ -245,6 +257,7 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) {
245257 auto shared_future = future.share ();
246258 auto start = std::chrono::steady_clock::now ();
247259 auto ret = executor.spin_until_future_complete (shared_future, 1s);
260+ executor.remove_node (this ->node , true );
248261
249262 // Check it didn't reach timeout
250263 EXPECT_GT (500ms, (std::chrono::steady_clock::now () - start));
@@ -273,6 +286,8 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) {
273286 std::thread spinner ([&]() {
274287 auto ret = executor.spin_until_future_complete (future, -1s);
275288 EXPECT_EQ (rclcpp::FutureReturnCode::SUCCESS, ret);
289+ executor.remove_node (this ->node , true );
290+ executor.cancel ();
276291 spin_exited = true ;
277292 });
278293
@@ -291,6 +306,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) {
291306 // If this fails, the test will probably crash because spinner goes out of scope while the thread
292307 // is active. However, it beats letting this run until the gtest timeout.
293308 ASSERT_TRUE (spin_exited);
309+ executor.cancel ();
294310 spinner.join ();
295311}
296312
@@ -316,6 +332,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) {
316332 std::thread spinner ([&]() {
317333 auto ret = executor.spin_until_future_complete (future, 1ms);
318334 EXPECT_EQ (rclcpp::FutureReturnCode::TIMEOUT, ret);
335+ executor.remove_node (this ->node , true );
319336 spin_exited = true ;
320337 });
321338
@@ -445,7 +462,7 @@ TYPED_TEST(TestExecutorsStable, spinAll) {
445462 bool spin_exited = false ;
446463 std::thread spinner ([&spin_exited, &executor, this ]() {
447464 executor.spin_all (1s);
448- executor.remove_node (this ->node );
465+ executor.remove_node (this ->node , true );
449466 spin_exited = true ;
450467 });
451468
@@ -485,7 +502,7 @@ TYPED_TEST(TestExecutorsStable, spinSome) {
485502 bool spin_exited = false ;
486503 std::thread spinner ([&spin_exited, &executor, this ]() {
487504 executor.spin_some (1s);
488- executor.remove_node (this ->node );
505+ executor.remove_node (this ->node , true );
489506 spin_exited = true ;
490507 });
491508
0 commit comments