int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("add_two_ints_client"); auto client = node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints"); while (!client->wait_for_service(1_s)) { if (!rclcpp::ok()) { printf("add_two_ints_client was interrupted while waiting for the service. Exiting.\n"); return 0; } printf("service not available, waiting again...\n"); } auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>(); request->a = 2; request->b = 3; auto future_result = client->async_send_request(request); // Wait for the result. if (rclcpp::spin_until_future_complete(node, future_result) == rclcpp::executor::FutureReturnCode::SUCCESS) { printf("Result of add_two_ints: %zd\n", future_result.get()->sum); } else { printf("add_two_ints_client_async was interrupted. Exiting.\n"); } rclcpp::shutdown(); return 0; }
TEST(CLASSNAME(test_services_client, RMW_IMPLEMENTATION), test_add_reqid) { auto node = rclcpp::Node::make_shared("test_services_client_add_reqid"); auto client = node->create_client<test_rclcpp::srv::AddTwoInts>("add_two_ints_reqid"); auto request = std::make_shared<test_rclcpp::srv::AddTwoInts::Request>(); request->a = 4; request->b = 5; auto result = client->async_send_request(request); rclcpp::spin_until_future_complete(node, result); // Wait for the result. EXPECT_EQ(9, result.get()->sum); }
SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT && cb) { SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>(); SharedFutureWithRequest future_with_request(promise->get_future()); auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) { auto response = future.get(); promise->set_value(std::make_pair(request, response)); cb(future_with_request); }; async_send_request(request, wrapping_cb); return future_with_request; }
TEST(CLASSNAME(test_services_client, RMW_IMPLEMENTATION), test_return_request) { auto node = rclcpp::Node::make_shared("test_services_client_return_request"); auto client = node->create_client<test_rclcpp::srv::AddTwoInts>( "add_two_ints_reqid_return_request"); auto request = std::make_shared<test_rclcpp::srv::AddTwoInts::Request>(); request->a = 4; request->b = 5; auto result = client->async_send_request( request, [](rclcpp::client::Client<test_rclcpp::srv::AddTwoInts>::SharedFutureWithRequest future) { EXPECT_EQ(4, future.get().first->a); EXPECT_EQ(5, future.get().first->b); EXPECT_EQ(9, future.get().second->sum); }); rclcpp::spin_until_future_complete(node, result); // Wait for the result. }
int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("add_two_ints_client"); auto client = node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints"); auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>(); request->a = 2; request->b = 3; auto future_result = client->async_send_request(request); // Wait for the result. if (rclcpp::spin_until_future_complete(node, future_result) == rclcpp::executor::FutureReturnCode::SUCCESS) { printf("Result of add_two_ints: %zd\n", future_result.get()->sum); } else { printf("add_two_ints_client_async was interrupted. Exiting.\n"); } return 0; }
SharedFuture async_send_request(SharedRequest request) { return async_send_request(request, [](SharedFuture) {}); }
SharedFuture async_send_request( typename ServiceT::Request::SharedPtr request) { return async_send_request(request, [](SharedFuture) {}); }