Skip to content

Commit 8647708

Browse files
bkoensgenbkoensgen
authored
tests(opennav_docking): restore executor-based spinning (#5533)
Signed-off-by: bkoensgen <[email protected]>
1 parent 7468843 commit 8647708

File tree

2 files changed

+54
-25
lines changed

2 files changed

+54
-25
lines changed

‎nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp‎

Lines changed: 29 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,8 @@ TEST(SimpleChargingDockTests, BatteryState)
8686

8787
dock->configure(node, "my_dock", nullptr);
8888
dock->activate();
89+
rclcpp::executors::SingleThreadedExecutor executor;
90+
executor.add_node(node->get_node_base_interface());
8991
geometry_msgs::msg::PoseStamped pose;
9092
EXPECT_TRUE(dock->getRefinedPose(pose, ""));
9193

@@ -95,7 +97,7 @@ TEST(SimpleChargingDockTests, BatteryState)
9597
pub->publish(msg);
9698
rclcpp::Rate r(2);
9799
r.sleep();
98-
rclcpp::spin_some(node->get_node_base_interface());
100+
executor.spin_some();
99101

100102
EXPECT_FALSE(dock->isCharging());
101103
EXPECT_TRUE(dock->hasStoppedCharging());
@@ -106,7 +108,7 @@ TEST(SimpleChargingDockTests, BatteryState)
106108
pub->publish(msg2);
107109
rclcpp::Rate r1(2);
108110
r1.sleep();
109-
rclcpp::spin_some(node->get_node_base_interface());
111+
executor.spin_some();
110112

111113
EXPECT_TRUE(dock->isCharging());
112114
EXPECT_FALSE(dock->hasStoppedCharging());
@@ -139,6 +141,8 @@ TEST(SimpleChargingDockTests, StallDetection)
139141
rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)));
140142
dock->configure(node, "my_dock", nullptr);
141143
dock->activate();
144+
rclcpp::executors::SingleThreadedExecutor executor;
145+
executor.add_node(node->get_node_base_interface());
142146
EXPECT_EQ(dock->getStallJointNames(), names);
143147

144148
// Stopped, but below effort threshold
@@ -149,7 +153,7 @@ TEST(SimpleChargingDockTests, StallDetection)
149153
pub->publish(msg);
150154
rclcpp::Rate r(2);
151155
r.sleep();
152-
rclcpp::spin_some(node->get_node_base_interface());
156+
executor.spin_some();
153157

154158
EXPECT_FALSE(dock->isDocked());
155159

@@ -161,7 +165,7 @@ TEST(SimpleChargingDockTests, StallDetection)
161165
pub->publish(msg2);
162166
rclcpp::Rate r1(2);
163167
r1.sleep();
164-
rclcpp::spin_some(node->get_node_base_interface());
168+
executor.spin_some();
165169

166170
EXPECT_FALSE(dock->isDocked());
167171

@@ -173,7 +177,7 @@ TEST(SimpleChargingDockTests, StallDetection)
173177
pub->publish(msg3);
174178
rclcpp::Rate r2(2);
175179
r2.sleep();
176-
rclcpp::spin_some(node->get_node_base_interface());
180+
executor.spin_some();
177181

178182
EXPECT_TRUE(dock->isDocked());
179183

@@ -245,6 +249,8 @@ TEST(SimpleChargingDockTests, RefinedPoseTest)
245249
dock->configure(node, "my_dock", nullptr);
246250
dock->activate();
247251
dock->startDetectionProcess();
252+
rclcpp::executors::SingleThreadedExecutor executor;
253+
executor.add_node(node->get_node_base_interface());
248254

249255
geometry_msgs::msg::PoseStamped pose;
250256

@@ -258,7 +264,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest)
258264
detected_pose.pose.position.x = 0.1;
259265
detected_pose.pose.position.y = -0.5;
260266
pub->publish(detected_pose);
261-
rclcpp::spin_some(node->get_node_base_interface());
267+
executor.spin_some();
262268
std::this_thread::sleep_for(std::chrono::milliseconds(100));
263269

264270
pose.header.frame_id = "my_frame";
@@ -287,14 +293,16 @@ TEST(SimpleChargingDockTests, RefinedPoseNotTransform)
287293

288294
dock->configure(node, "my_dock", tf_buffer);
289295
dock->activate();
296+
rclcpp::executors::SingleThreadedExecutor executor;
297+
executor.add_node(node->get_node_base_interface());
290298

291299
geometry_msgs::msg::PoseStamped detected_pose;
292300
detected_pose.header.stamp = node->now();
293301
detected_pose.header.frame_id = "my_frame";
294302
detected_pose.pose.position.x = 1.0;
295303
detected_pose.pose.position.y = 1.0;
296304
pub->publish(detected_pose);
297-
rclcpp::spin_some(node->get_node_base_interface());
305+
executor.spin_some();
298306

299307
// Create a pose with a different frame_id
300308
geometry_msgs::msg::PoseStamped pose;
@@ -325,6 +333,8 @@ TEST(SimpleChargingDockTests, IsDockedTransformException)
325333
dock->configure(node, "my_dock", tf_buffer);
326334
dock->activate();
327335
dock->startDetectionProcess();
336+
rclcpp::executors::SingleThreadedExecutor executor;
337+
executor.add_node(node->get_node_base_interface());
328338

329339
// Create a pose with a different frame_id
330340
geometry_msgs::msg::PoseStamped pose;
@@ -347,7 +357,7 @@ TEST(SimpleChargingDockTests, IsDockedTransformException)
347357
detected_pose.pose.position.x = 1.0;
348358
detected_pose.pose.position.y = 1.0;
349359
pub->publish(detected_pose);
350-
rclcpp::spin_some(node->get_node_base_interface());
360+
executor.spin_some();
351361

352362
// Second call should succeed
353363
EXPECT_TRUE(dock->getRefinedPose(pose, ""));
@@ -452,12 +462,14 @@ TEST(SimpleChargingDockTests, DetectorLifecycle)
452462

453463
dock->activate();
454464
dock->startDetectionProcess();
465+
rclcpp::executors::SingleThreadedExecutor executor;
466+
executor.add_node(node->get_node_base_interface());
455467
// Spin to process async service call
456468
auto start_time = std::chrono::steady_clock::now();
457469
while (!service_called &&
458470
std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2))
459471
{
460-
rclcpp::spin_some(node->get_node_base_interface());
472+
executor.spin_some();
461473
std::this_thread::sleep_for(std::chrono::milliseconds(10));
462474
}
463475
EXPECT_TRUE(service_called);
@@ -502,19 +514,21 @@ TEST(SimpleChargingDockTests, SubscriptionCallback)
502514
publisher->on_activate();
503515

504516
dock->startDetectionProcess();
517+
rclcpp::executors::SingleThreadedExecutor executor;
518+
executor.add_node(node->get_node_base_interface());
505519

506520
// Wait for the publisher and subscriber to connect.
507521
int tries = 0;
508522
while (publisher->get_subscription_count() == 0 && tries++ < 10) {
509-
rclcpp::spin_some(node->get_node_base_interface());
523+
executor.spin_some();
510524
std::this_thread::sleep_for(100ms);
511525
}
512526
ASSERT_GT(publisher->get_subscription_count(), 0);
513527

514528
// Publish a message to trigger the subscription callback.
515529
publisher->publish(geometry_msgs::msg::PoseStamped{});
516530
std::this_thread::sleep_for(50ms);
517-
rclcpp::spin_some(node->get_node_base_interface());
531+
executor.spin_some();
518532

519533
// Verify the detector state was updated, proving the callback was executed.
520534
EXPECT_TRUE(dock->isDetectorActive());
@@ -608,17 +622,18 @@ TEST(SimpleChargingDockTests, SubscriptionPersistent)
608622
auto publisher = node->create_publisher<geometry_msgs::msg::PoseStamped>(
609623
"detected_dock_pose", rclcpp::QoS(1));
610624
publisher->on_activate();
611-
625+
rclcpp::executors::SingleThreadedExecutor executor;
626+
executor.add_node(node->get_node_base_interface());
612627
int tries = 0;
613628
while (publisher->get_subscription_count() == 0 && tries++ < 10) {
614-
rclcpp::spin_some(node->get_node_base_interface());
629+
executor.spin_some();
615630
std::this_thread::sleep_for(100ms);
616631
}
617632
ASSERT_GT(publisher->get_subscription_count(), 0);
618633

619634
publisher->publish(geometry_msgs::msg::PoseStamped{});
620635
std::this_thread::sleep_for(50ms);
621-
rclcpp::spin_some(node->get_node_base_interface());
636+
executor.spin_some();
622637

623638
// Verify the detector state changed, proving the callback was executed.
624639
EXPECT_TRUE(dock->isDetectorActive());

‎nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp‎

Lines changed: 25 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,8 @@ TEST(SimpleNonChargingDockTests, StallDetection)
9999
EXPECT_EQ(dock->getStallJointNames(), names);
100100

101101
dock->activate();
102+
rclcpp::executors::SingleThreadedExecutor executor;
103+
executor.add_node(node->get_node_base_interface());
102104
geometry_msgs::msg::PoseStamped pose;
103105
EXPECT_TRUE(dock->getRefinedPose(pose, ""));
104106

@@ -110,7 +112,7 @@ TEST(SimpleNonChargingDockTests, StallDetection)
110112
pub->publish(msg);
111113
rclcpp::Rate r(2);
112114
r.sleep();
113-
rclcpp::spin_some(node->get_node_base_interface());
115+
executor.spin_some();
114116

115117
EXPECT_FALSE(dock->isDocked());
116118

@@ -122,7 +124,7 @@ TEST(SimpleNonChargingDockTests, StallDetection)
122124
pub->publish(msg2);
123125
rclcpp::Rate r1(2);
124126
r1.sleep();
125-
rclcpp::spin_some(node->get_node_base_interface());
127+
executor.spin_some();
126128

127129
EXPECT_FALSE(dock->isDocked());
128130

@@ -134,7 +136,7 @@ TEST(SimpleNonChargingDockTests, StallDetection)
134136
pub->publish(msg3);
135137
rclcpp::Rate r2(2);
136138
r2.sleep();
137-
rclcpp::spin_some(node->get_node_base_interface());
139+
executor.spin_some();
138140

139141
EXPECT_TRUE(dock->isDocked());
140142

@@ -206,6 +208,8 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest)
206208
dock->configure(node, "my_dock", nullptr);
207209
dock->activate();
208210
dock->startDetectionProcess();
211+
rclcpp::executors::SingleThreadedExecutor executor;
212+
executor.add_node(node->get_node_base_interface());
209213

210214
geometry_msgs::msg::PoseStamped pose;
211215

@@ -219,7 +223,7 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest)
219223
detected_pose.pose.position.x = 0.1;
220224
detected_pose.pose.position.y = -0.5;
221225
pub->publish(detected_pose);
222-
rclcpp::spin_some(node->get_node_base_interface());
226+
executor.spin_some();
223227
std::this_thread::sleep_for(std::chrono::milliseconds(100));
224228

225229
pose.header.frame_id = "my_frame";
@@ -248,14 +252,16 @@ TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform)
248252

249253
dock->configure(node, "my_dock", tf_buffer);
250254
dock->activate();
255+
rclcpp::executors::SingleThreadedExecutor executor;
256+
executor.add_node(node->get_node_base_interface());
251257

252258
geometry_msgs::msg::PoseStamped detected_pose;
253259
detected_pose.header.stamp = node->now();
254260
detected_pose.header.frame_id = "my_frame";
255261
detected_pose.pose.position.x = 1.0;
256262
detected_pose.pose.position.y = 1.0;
257263
pub->publish(detected_pose);
258-
rclcpp::spin_some(node->get_node_base_interface());
264+
executor.spin_some();
259265

260266
// Create a pose with a different frame_id
261267
geometry_msgs::msg::PoseStamped pose;
@@ -286,6 +292,8 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException)
286292
dock->configure(node, "my_dock", tf_buffer);
287293
dock->activate();
288294
dock->startDetectionProcess();
295+
rclcpp::executors::SingleThreadedExecutor executor;
296+
executor.add_node(node->get_node_base_interface());
289297

290298
// Create a pose with a different frame_id
291299
geometry_msgs::msg::PoseStamped pose;
@@ -308,7 +316,7 @@ TEST(SimpleNonChargingDockTests, IsDockedTransformException)
308316
detected_pose.pose.position.x = 1.0;
309317
detected_pose.pose.position.y = 1.0;
310318
pub->publish(detected_pose);
311-
rclcpp::spin_some(node->get_node_base_interface());
319+
executor.spin_some();
312320

313321
// Second call should succeed
314322
EXPECT_TRUE(dock->getRefinedPose(pose, ""));
@@ -413,12 +421,14 @@ TEST(SimpleNonChargingDockTests, DetectorLifecycle)
413421

414422
dock->activate();
415423
dock->startDetectionProcess();
424+
rclcpp::executors::SingleThreadedExecutor executor;
425+
executor.add_node(node->get_node_base_interface());
416426
// Spin to process async service call
417427
auto start_time = std::chrono::steady_clock::now();
418428
while (!service_called &&
419429
std::chrono::steady_clock::now() - start_time < std::chrono::seconds(2))
420430
{
421-
rclcpp::spin_some(node->get_node_base_interface());
431+
executor.spin_some();
422432
std::this_thread::sleep_for(std::chrono::milliseconds(10));
423433
}
424434
EXPECT_TRUE(service_called);
@@ -464,19 +474,21 @@ TEST(SimpleNonChargingDockTests, SubscriptionCallback)
464474
publisher->on_activate();
465475

466476
dock->startDetectionProcess();
477+
rclcpp::executors::SingleThreadedExecutor executor;
478+
executor.add_node(node->get_node_base_interface());
467479

468480
// Wait for the publisher and subscriber to connect.
469481
int tries = 0;
470482
while (publisher->get_subscription_count() == 0 && tries++ < 10) {
471-
rclcpp::spin_some(node->get_node_base_interface());
483+
executor.spin_some();
472484
std::this_thread::sleep_for(100ms);
473485
}
474486
ASSERT_GT(publisher->get_subscription_count(), 0);
475487

476488
// Publish a message to trigger the subscription callback.
477489
publisher->publish(geometry_msgs::msg::PoseStamped{});
478490
std::this_thread::sleep_for(50ms);
479-
rclcpp::spin_some(node->get_node_base_interface());
491+
executor.spin_some();
480492

481493
// Verify the detector state was updated, proving the callback was executed.
482494
EXPECT_TRUE(dock->isDetectorActive());
@@ -567,6 +579,8 @@ TEST(SimpleNonChargingDockTests, SubscriptionPersistent)
567579
auto dock = std::make_unique<SimpleNonChargingDockTestable>();
568580
dock->configure(node, "my_dock", nullptr);
569581
dock->activate();
582+
rclcpp::executors::SingleThreadedExecutor executor;
583+
executor.add_node(node->get_node_base_interface());
570584

571585
// The subscription should be active immediately after configuration.
572586
auto publisher = node->create_publisher<geometry_msgs::msg::PoseStamped>(
@@ -575,14 +589,14 @@ TEST(SimpleNonChargingDockTests, SubscriptionPersistent)
575589

576590
int tries = 0;
577591
while (publisher->get_subscription_count() == 0 && tries++ < 10) {
578-
rclcpp::spin_some(node->get_node_base_interface());
592+
executor.spin_some();
579593
std::this_thread::sleep_for(100ms);
580594
}
581595
ASSERT_GT(publisher->get_subscription_count(), 0);
582596

583597
publisher->publish(geometry_msgs::msg::PoseStamped{});
584598
std::this_thread::sleep_for(50ms);
585-
rclcpp::spin_some(node->get_node_base_interface());
599+
executor.spin_some();
586600

587601
// Verify the detector state changed, proving the callback was executed.
588602
EXPECT_TRUE(dock->isDetectorActive());

0 commit comments

Comments
 (0)