{

RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase)

RCLCPP_PUBLIC

ClientBase(

rclcpp::node_interfaces::NodeBaseInterface * node_base,

rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);

RCLCPP_PUBLIC

virtual ~ClientBase();

RCLCPP_PUBLIC

const char *

get_service_name() const;

RCLCPP_PUBLIC

std::shared_ptr<rcl_client_t>

get_client_handle();

RCLCPP_PUBLIC

std::shared_ptr<const rcl_client_t>

get_client_handle() const;

RCLCPP_PUBLIC

bool

service_is_ready() const;

template<typename RepT = int64_t, typename RatioT = std::milli>

bool

wait_for_service(

std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))

{

return wait_for_service_nanoseconds(

std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)

);

}

virtual std::shared_ptr<void> create_response() = 0;

virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;

virtual void handle_response(

std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;

RCLCPP_DISABLE_COPY(ClientBase)

RCLCPP_PUBLIC

bool

wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);

RCLCPP_PUBLIC

rcl_node_t *

get_rcl_node_handle();

RCLCPP_PUBLIC

const rcl_node_t *

get_rcl_node_handle() const;

rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;

std::shared_ptr<rcl_node_t> node_handle_;

std::shared_ptr<rclcpp::Context> context_;

std::shared_ptr<rcl_client_t> client_handle_;

{

using SharedRequest = typename ServiceT::Request::SharedPtr;

using SharedResponse = typename ServiceT::Response::SharedPtr;

using Promise = std::promise<SharedResponse>;

using PromiseWithRequest = std::promise<std::pair<SharedRequest, SharedResponse>>;

using SharedPromise = std::shared_ptr<Promise>;

using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;

using SharedFuture = std::shared_future<SharedResponse>;

using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;

using CallbackType = std::function<void (SharedFuture)>;

using CallbackWithRequestType = std::function<void (SharedFutureWithRequest)>;

RCLCPP_SMART_PTR_DEFINITIONS(Client)

Client(

rclcpp::node_interfaces::NodeBaseInterface * node_base,

rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,

const std::string & service_name,

rcl_client_options_t & client_options)

: ClientBase(node_base, node_graph)

{

using rosidl_typesupport_cpp::get_service_type_support_handle;

auto service_type_support_handle =

get_service_type_support_handle<ServiceT>();

rcl_ret_t ret = rcl_client_init(

this->get_client_handle().get(),

this->get_rcl_node_handle(),

service_type_support_handle,

service_name.c_str(),

&client_options);

if (ret != RCL_RET_OK) {

if (ret == RCL_RET_SERVICE_NAME_INVALID) {

auto rcl_node_handle = this->get_rcl_node_handle();

// this will throw on any validation problem

rcl_reset_error();

expand_topic_or_service_name(

service_name,

rcl_node_get_name(rcl_node_handle),

rcl_node_get_namespace(rcl_node_handle),

true);

}

rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");

}

}

virtual ~Client()

{

}

std::shared_ptr<void>

create_response() override

{

return std::shared_ptr<void>(new typename ServiceT::Response());

}

std::shared_ptr<rmw_request_id_t>

create_request_header() override

{

// TODO(wjwwood): This should probably use rmw_request_id's allocator.

// (since it is a C type)

return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);

}

void

handle_response(

std::shared_ptr<rmw_request_id_t> request_header,

std::shared_ptr<void> response) override

{

std::unique_lock<std::mutex> lock(pending_requests_mutex_);

auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);

int64_t sequence_number = request_header->sequence_number;

// TODO(esteve) this should throw instead since it is not expected to happen in the first place

if (this->pending_requests_.count(sequence_number) == 0) {

RCUTILS_LOG_ERROR_NAMED(

"rclcpp",

"Received invalid sequence number. Ignoring...");

return;

}

auto tuple = this->pending_requests_[sequence_number];

auto call_promise = std::get<0>(tuple);

auto callback = std::get<1>(tuple);

auto future = std::get<2>(tuple);

this->pending_requests_.erase(sequence_number);

// Unlock here to allow the service to be called recursively from one of its callbacks.

lock.unlock();

call_promise->set_value(typed_response);

callback(future);

}

SharedFuture

async_send_request(SharedRequest request)

{

return async_send_request(request, [](SharedFuture) {});

}

template<

typename CallbackT,

typename std::enable_if<

rclcpp::function_traits::same_arguments<

CallbackT,

CallbackType

>::value

>::type * = nullptr

>

SharedFuture

async_send_request(SharedRequest request, CallbackT && cb)

{

std::lock_guard<std::mutex> lock(pending_requests_mutex_);

int64_t sequence_number;

rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);

if (RCL_RET_OK != ret) {

rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");

}

SharedPromise call_promise = std::make_shared<Promise>();

SharedFuture f(call_promise->get_future());

pending_requests_[sequence_number] =

std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);

return f;

}

template<

typename CallbackT,

typename std::enable_if<

rclcpp::function_traits::same_arguments<

CallbackT,

CallbackWithRequestType

>::value

>::type * = nullptr

>

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;

}

RCLCPP_DISABLE_COPY(Client)

std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;

std::mutex pending_requests_mutex_;