#include <unordered_map>

#include <optional> // NOLINT, cpplint doesn't think this is a cpp std header

#include <variant> // NOLINT

using Request = typename ServiceT::Request;

using Response = typename ServiceT::Response;

using Future = std::future<SharedResponse>;

/// A convenient Client::Future and request id pair.

class FutureAndRequestId

{

FutureAndRequestId(Future impl, int64_t req_id)

: impl_(std::move(impl)), req_id_(req_id)

{}

/// Allow implicit conversions to `std::future` by reference.

// TODO(ivanpauno): Maybe, deprecate this in favor of get_future() (?)

operator Future &() {return impl_;}

/// Deprecated, use take_future() instead.

/**

[[deprecated("FutureAndRequestId: use take_future() instead of an implicit conversion")]]

operator Future() {return impl_;}

/// Returns the internal `std::future`, moving it out.

/**

Future

take_future() {return impl_;}

/// Getter for the internal future.

Future &

get_future() {return impl_;}

/// Getter for the internal future.

const Future &

get_future() const {return impl_;}

/// Returns the request id associated with this future.

int64_t get_request_id() const {return req_id_;}

// delegate future like methods in the std::future impl_

/// See std::future::share().

SharedFuture share() noexcept {return impl_.share();}

/// See std::future::get().

SharedResponse get() {return impl_.get();}

/// See std::future::valid().

bool valid() const noexcept {return impl_.valid();}

/// See std::future::wait().

void wait() const {return impl_.wait();}

/// See std::future::wait_for().

template<class Rep, class Period>

std::future_status wait_for(

const std::chrono::duration<Rep, Period> & timeout_duration) const

{

return impl_.wait_for(timeout_duration);

}

/// See std::future::wait_until().

template<class Clock, class Duration>

std::future_status wait_until(

const std::chrono::time_point<Clock, Duration> & timeout_time) const

{

return impl_.wait_until(timeout_time);

}

// Rule of five, we could use the rule of zero here, but better be explicit as some of the

// methods are deleted.

/// Move constructor.

FutureAndRequestId(FutureAndRequestId && other) noexcept = default;

/// Deleted copy constructor, each instance is a unique owner of the future.

FutureAndRequestId(const FutureAndRequestId & other) = delete;

/// Move assignment.

FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default;

/// Deleted copy assignment, each instance is a unique owner of the future.

FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete;

/// Destructor.

~FutureAndRequestId() = default;

Future impl_;

int64_t req_id_;

};

auto opt = this->get_and_erase_pending_request(request_header->sequence_number);

if (!opt) {

auto & value = *opt;

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

std::move(response));

if (std::holds_alternative<Promise>(value)) {

auto & promise = std::get<Promise>(value);

promise.set_value(std::move(typed_response));

} else if (std::holds_alternative<CallbackType>(value)) {

Promise promise;

promise.set_value(std::move(typed_response));

const auto & callback = std::get<CallbackType>(value);

callback(promise.get_future().share());

} else if (std::holds_alternative<std::pair<CallbackWithRequestType, SharedRequest>>(value)) {

PromiseWithRequest promise;

const auto & pair = std::get<std::pair<CallbackWithRequestType, SharedRequest>>(value);

promise.set_value(std::make_pair(std::move(pair.second), std::move(typed_response)));

pair.first(promise.get_future().share());

}

/// Send a request to the service server.

/**

FutureAndRequestId

Promise promise;

auto future = promise.get_future();

auto req_id = async_send_request_impl(

*request,

std::move(promise));

return FutureAndRequestId(std::move(future), req_id);

/// Send a request to the service server and schedule a callback in the executor.

/**

// TODO(ivanpauno): Link to example that shows how to cleanup requests.

int64_t

return async_send_request_impl(

*request,

CallbackType{std::forward<CallbackT>(cb)});

/// Send a request to the service server and schedule a callback in the executor.

/**

// TODO(ivanpauno): Deprecate this.

// If someone wants the request they can capture it in the lambda.

int64_t

auto & req = *request;

return async_send_request_impl(

req,

std::make_pair(CallbackWithRequestType{std::forward<CallbackT>(cb)}, std::move(request)));

}

/// Cleanup a pending request.

/**

bool

remove_pending_request(int64_t request_id)

{

std::lock_guard guard(pending_requests_mutex_);

return pending_requests_.erase(request_id) != 0u;

}

/// Cleanup a pending request.

/**

bool

remove_pending_request(const FutureAndRequestId & future)

{

return this->remove_pending_request(future.get_request_id());

}

/// Clean all pending requests.

/**

size_t

prune_requests()

{

std::lock_guard guard(pending_requests_mutex_);

auto ret = pending_requests_.size();

pending_requests_.clear();

return ret;

}

using PendingRequestsMapValue = std::variant<

std::promise<SharedResponse>,

CallbackType,

std::pair<CallbackWithRequestType, SharedRequest>>;

RCLCPP_PUBLIC

int64_t

async_send_request_impl(const Request & request, PendingRequestsMapValue value)

{

int64_t sequence_number;

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

if (RCL_RET_OK != ret) {

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

}

{

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

pending_requests_.try_emplace(sequence_number, std::move(value));

}

return sequence_number;

}

RCLCPP_PUBLIC

std::optional<PendingRequestsMapValue>

get_and_erase_pending_request(int64_t request_number)

{

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

auto it = this->pending_requests_.find(request_number);

if (it == this->pending_requests_.end()) {

RCUTILS_LOG_DEBUG_NAMED(

"rclcpp",

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

return std::nullopt;

}

auto value = std::move(it->second);

this->pending_requests_.erase(request_number);

return value;

std::unordered_map<int64_t, PendingRequestsMapValue> pending_requests_;