Skip to content

Commit 98fba6a

Browse files
Fixed a way of waiting results
Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
1 parent b583132 commit 98fba6a

File tree

3 files changed

+10
-22
lines changed

3 files changed

+10
-22
lines changed

localization/ndt_scan_matcher/test/stub_initialpose_client.hpp

+5-9
Original file line numberDiff line numberDiff line change
@@ -48,16 +48,12 @@ class StubInitialposeClient : public rclcpp::Node
4848
std::shared_ptr<AlignSrv::Request> request = std::make_shared<AlignSrv::Request>();
4949
request->pose_with_covariance = initialpose;
5050
auto result = align_service_client_->async_send_request(request);
51-
52-
// wait for the response
53-
std::future_status status = result.wait_for(std::chrono::seconds(0));
54-
while (status != std::future_status::ready) {
55-
if (!rclcpp::ok()) {
56-
EXIT_FAILURE;
57-
}
58-
status = result.wait_for(std::chrono::seconds(1));
51+
if (
52+
rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
53+
rclcpp::FutureReturnCode::SUCCESS) {
54+
RCLCPP_ERROR(this->get_logger(), "Service call failed");
55+
throw std::runtime_error("Service call failed.");
5956
}
60-
6157
return result.get()->pose_with_covariance;
6258
}
6359

localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp

+5-9
Original file line numberDiff line numberDiff line change
@@ -46,16 +46,12 @@ class StubTriggerNodeClient : public rclcpp::Node
4646
std::shared_ptr<SetBool::Request> request = std::make_shared<SetBool::Request>();
4747
request->data = trigger;
4848
auto result = align_service_client_->async_send_request(request);
49-
50-
// wait for the response
51-
std::future_status status = result.wait_for(std::chrono::seconds(0));
52-
while (status != std::future_status::ready) {
53-
if (!rclcpp::ok()) {
54-
EXIT_FAILURE;
55-
}
56-
status = result.wait_for(std::chrono::seconds(1));
49+
if (
50+
rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
51+
rclcpp::FutureReturnCode::SUCCESS) {
52+
RCLCPP_ERROR(this->get_logger(), "Service call failed");
53+
throw std::runtime_error("Service call failed.");
5754
}
58-
5955
return result.get()->success;
6056
}
6157

localization/ndt_scan_matcher/test/test.cpp

-4
Original file line numberDiff line numberDiff line change
@@ -118,8 +118,6 @@ TEST_F(TestNDTScanMatcher,
118118
exec.spin();
119119
});
120120
std::thread t2([&]() { rclcpp::spin(pcd_loader_); });
121-
std::thread t3([&]() { rclcpp::spin(initialpose_client_); });
122-
std::thread t4([&]() { rclcpp::spin(trigger_node_client_); });
123121

124122
//-----//
125123
// Act //
@@ -144,8 +142,6 @@ TEST_F(TestNDTScanMatcher,
144142
rclcpp::shutdown();
145143
t1.join();
146144
t2.join();
147-
t3.join();
148-
t4.join();
149145
}
150146

151147
int main(int argc, char ** argv)

0 commit comments

Comments
 (0)