service.hpp

Go to the documentation of this file.

// nros-cpp: the DISPATCH service server
// Freestanding C++ — no exceptions, no STL required

 
#ifndef NROS_RCLCPP_SERVICE_HPP
#define NROS_RCLCPP_SERVICE_HPP
 
#include <cstdint>
#include <cstddef>
 
#include "rclcpp/detail/callback_context.hpp" // phase-456 W3 — the handler IS the arena context
#include "rclcpp/detail/config.hpp"
#include "rclcpp/detail/entity_name.hpp" // phase-444 — the one entity-name copy
#include "rclcpp/result.hpp"
#include "rclcpp/service_handle.hpp" // phase-456 W5 — what `Service<S>::SharedPtr` IS
#include "rclcpp/size_bound.hpp"     // rclcpp::rx_buffer_capacity<M> — the receive-buffer size
 
#include "rclcpp/detail/nros_cpp_ffi.h"
 
// issue 1437 — the two granted-QoS accessors return a `rclcpp::QoS` BY VALUE from
// an inline body, so the complete type must be here, not only by the time
// `rclcpp/node.hpp` is pulled in below.
//
// AFTER `nros_cpp_ffi.h`, never before: `qos.hpp` defines the four
// `nros_cpp_qos_*_t` enums ITSELF under `#ifndef NROS_CPP_FFI_H`, so
// reaching it first makes the cbindgen header a REDEFINITION of all four.
#include "rclcpp/qos.hpp"
 
// Phase 189.M3.3.e — `nros_cpp_service_server_register` is excluded from
// cbindgen (its Rust signature uses `RawServiceCallback`, an external-crate
// type alias cbindgen names without defining). Declare it locally with a plain
// function-pointer typedef matching the ABI (`bool(req, req_len, resp,
// resp_cap, resp_len, ctx)`).
extern "C" {
typedef bool (*nros_cpp_service_request_callback_t)(const uint8_t* req, size_t req_len,
                                                    uint8_t* resp, size_t resp_cap,
                                                    size_t* resp_len, void* ctx);
 
nros_cpp_ret_t nros_cpp_service_server_register(const nros_cpp_node_t* node,
                                                const char* service_name, const char* type_name,
                                                const char* type_hash, nros_cpp_qos_t qos,
                                                nros_cpp_service_request_callback_t callback,
                                                void* context, uint8_t sched_context,
                                                size_t* out_handle_id);
 
// phase-488 — the capturing twin, excluded from cbindgen for the same reason.
nros_cpp_ret_t nros_cpp_service_server_register_capturing(
    const nros_cpp_node_t* node, const char* service_name, const char* type_name,
    const char* type_hash, nros_cpp_qos_t qos, nros_cpp_service_request_callback_t callback,
    const uint8_t* capture, size_t capture_len, void (*capture_drop)(void*), uint8_t sched_context,
    size_t* out_handle_id);
} // extern "C"
 
// `rmw_request_id_t` — upstream's request header. Upstream's three-argument
// service handler receives it; issue 2081 fills it from the backend's report.
#include "rclcpp/detail/rmw_request_id.hpp"
 
extern "C" {
typedef bool (*nros_cpp_service_request_id_callback_t)(const uint8_t* req, size_t req_len,
                                                       rmw_request_id_t* request_id, uint8_t* resp,
                                                       size_t resp_cap, size_t* resp_len,
                                                       void* ctx);
 
// Issue 2081 — the request-id registration, excluded from cbindgen for the same
// reason. A non-NULL `capture` is copied into the arena (as the capturing
// form); otherwise `context` is handed back.
nros_cpp_ret_t nros_cpp_service_server_register_with_request_id(
    const nros_cpp_node_t* node, const char* service_name, const char* type_name,
    const char* type_hash, nros_cpp_qos_t qos, nros_cpp_service_request_id_callback_t callback,
    void* context, const uint8_t* capture, size_t capture_len, void (*capture_drop)(void*),
    uint8_t sched_context, size_t* out_handle_id);
} // extern "C"

namespace rclcpp {
class Node;
}
 
// ============================================================================
// `rclcpp::Service<S>` -- DEFINED here (RFC-0089: rclcpp:: is the home)
// ============================================================================
//
// phase-428: the definition moved from `nros::` to `rclcpp::` (the `nros::`
// alias went in phase-483 W1). The nested `SharedPtr` / `ConstSharedPtr` / `UniquePtr`
// aliases live on the class itself, so the rclcpp way of indexing types
// (`rclcpp::Service<S>::SharedPtr`) resolves with no wrapper in between.
namespace rclcpp {

template <typename S> class Service {
  public:
    using SharedPtr = ::rclcpp::ServiceHandle<S>;
    using ConstSharedPtr = ::rclcpp::ServiceHandle<S>;
    using UniquePtr = ::rclcpp::ServiceHandle<S>;
 
    using RequestType = typename S::Request;
    using ResponseType = typename S::Response;

    using TypedServiceFn = void (*)(const RequestType& request, ResponseType& response);

    bool is_valid() const { return initialized_; }

    const char* get_service_name() const { return initialized_ ? service_name_ : ""; }

    ::rclcpp::QoS get_request_subscription_actual_qos() const { return actual_qos_half(true); }

    ::rclcpp::QoS get_response_publisher_actual_qos() const { return actual_qos_half(false); }

    ~Service() { initialized_ = false; }
 
    // Move semantics (non-copyable). Bookkeeping only — there is no storage to
    // relocate, so `nros_cpp_service_server_relocate` is not called here.
    //
    // phase-456 W3 — a callback-style service is MOVABLE, and the warning that
    // used to stand here is gone with its subject. It said:
    //
    //     A callback-style service must NOT be moved after register — the arena
    //     holds `this` as the trampoline context (Phase 189.M3.3.e); the move
    //     only transfers bookkeeping and leaves that pointer stale, so don't.
    //
    // The arena holds the user's HANDLER as its context now, not `this`, so
    // after registration nothing of the caller's is referenced and there is no
    // pointer a move could leave stale. What moves is bookkeeping, which is
    // what the warning said it was — the difference is that bookkeeping is now
    // all there is.
    Service(Service&& other)
        : initialized_(other.initialized_), handle_id_(other.handle_id_), service_name_{},
          executor_(other.executor_) {
        ::rclcpp::detail::assign_entity_name(service_name_, other.service_name_);
        other.initialized_ = false;
    }
 
    Service& operator=(Service&& other) {
        if (this != &other) {
            initialized_ = other.initialized_;
            handle_id_ = other.handle_id_;
            ::rclcpp::detail::assign_entity_name(service_name_, other.service_name_);
            // issue 1437 — moves with the index it pairs with.
            executor_ = other.executor_;
            other.initialized_ = false;
        }
        return *this;
    }

    Service() : initialized_(false), service_name_{} {}

    size_t handle_id() const { return handle_id_; }
 
  private:
    Service(const Service&) = delete;
    Service& operator=(const Service&) = delete;
 
    friend class ::rclcpp::Node;

    static bool request_trampoline(const uint8_t* req, size_t req_len, uint8_t* resp,
                                   size_t resp_cap, size_t* resp_len, void* ctx) {
        const TypedServiceFn user_fn = ::rclcpp::detail::fn_from_context<TypedServiceFn>(ctx);
        if (user_fn == nullptr) return false;
        RequestType request;
        if (RequestType::ffi_deserialize(req, req_len, &request) != 0) return false;
        ResponseType response;
        user_fn(request, response);
        size_t len = 0;
        if (ResponseType::ffi_serialize(&response, resp, resp_cap, &len) != 0) return false;
        *resp_len = len;
        return true;
    }

    ::rclcpp::QoS actual_qos_half(bool request) const {
        nros_cpp_qos_t req{};
        nros_cpp_qos_t resp{};
        if (!initialized_) return ::rclcpp::detail::qos_all_unknown();
        if (nros_cpp_service_server_get_actual_qos(nullptr, executor_, handle_id_, &req, &resp) !=
            0) {
            return ::rclcpp::detail::qos_all_unknown();
        }
        return ::rclcpp::detail::qos_from_ffi(request ? req : resp);
    }
 
    bool initialized_;
    // Callback-style BOOKKEEPING (Phase 189.M3.3.e). The handler itself is not
    // here — it lives in the arena (phase-456 W3) — and neither is the server,
    // so what remains is the caller's own record of a registration that refers
    // back to nothing of the caller's.
    size_t handle_id_ = static_cast<size_t>(-1);

    char service_name_[::rclcpp::SERVICE_NAME_MAX];
 
    // issue 1437 — the executor whose arena holds this service, recorded with
    // the index that names it there. `handle_id_` alone points into an arena
    // this object could not otherwise reach.
    void* executor_ = nullptr;
};
 
} // namespace rclcpp
 
// Phase 84.G8: out-of-line definition of Node::create_service<S>().
#include "rclcpp/node.hpp"
 
namespace rclcpp {
 
// Phase 189.M3.3.e — callback-style (arena-registered) service. The arena owns
// the server AND the request handler, and dispatches it during spin_once, so
// the handle is real and `options.sched_context` is functional. `out` receives
// the handle id and nothing the arena refers back to (phase-456 W3).
} // namespace rclcpp
 
namespace rclcpp {
template <typename S, typename F, typename>
Result Node::create_service(Service<S>& out, const char* service_name, F callback,
                            const ::rclcpp::QoS& qos, const ::rclcpp::ServiceOptions& options) {
    // The user handler becomes the ARENA's context (phase-456 W3). The
    // conversion is still the compile error for a non-convertible `F`; what
    // changed is where the resulting pointer is stored — in the registration,
    // not in `out`, so `out` is no longer an object the arena knows by address.
    const typename Service<S>::TypedServiceFn user_fn =
        typename Service<S>::TypedServiceFn(callback);
    return this->template register_service_<S>(out, service_name, &Service<S>::request_trampoline,
                                               ::rclcpp::detail::fn_to_context(user_fn), qos,
                                               options);
}
 
template <typename S>
Result Node::register_service_(Service<S>& out, const char* service_name,
                               bool (*trampoline)(const uint8_t*, size_t, uint8_t*, size_t, size_t*,
                                                  void*),
                               void* context, const ::rclcpp::QoS& qos,
                               const ::rclcpp::ServiceOptions& options, const void* capture,
                               size_t capture_len, void (*capture_drop)(void*)) {
    if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
    nros_cpp_qos_t ffi_qos = ::rclcpp::detail::qos_to_ffi(qos);
 
    uint8_t sched = (options.sched_context == ::rclcpp::SCHED_CONTEXT_UNSET)
                        ? 0u
                        : static_cast<uint8_t>(options.sched_context);
    size_t handle = static_cast<size_t>(-1);
    nros_cpp_ret_t ret =
        (capture != nullptr && capture_len != 0)
            ? nros_cpp_service_server_register_capturing(
                  &handle_, service_name, S::TYPE_NAME, S::Request::TYPE_HASH, ffi_qos, trampoline,
                  static_cast<const uint8_t*>(capture), capture_len, capture_drop, sched, &handle)
            : nros_cpp_service_server_register(&handle_, service_name, S::TYPE_NAME,
                                               S::Request::TYPE_HASH, ffi_qos, trampoline, context,
                                               sched, &handle);
    return this->template finish_register_service_<S>(out, service_name, ret, handle);
}
 
// Issue 2081 — the same registration for a trampoline that also receives the
// request's id (the hosted `shared_ptr` handlers, whose three-argument form
// hands it to the user as the request header).
template <typename S>
Result Node::register_service_(Service<S>& out, const char* service_name,
                               nros_cpp_service_request_id_callback_t trampoline, void* context,
                               const ::rclcpp::QoS& qos, const ::rclcpp::ServiceOptions& options,
                               const void* capture, size_t capture_len,
                               void (*capture_drop)(void*)) {
    if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
    nros_cpp_qos_t ffi_qos = ::rclcpp::detail::qos_to_ffi(qos);
    uint8_t sched = (options.sched_context == ::rclcpp::SCHED_CONTEXT_UNSET)
                        ? 0u
                        : static_cast<uint8_t>(options.sched_context);
    size_t handle = static_cast<size_t>(-1);
    const bool captured = capture != nullptr && capture_len != 0;
    nros_cpp_ret_t ret = nros_cpp_service_server_register_with_request_id(
        &handle_, service_name, S::TYPE_NAME, S::Request::TYPE_HASH, ffi_qos, trampoline,
        captured ? nullptr : context, captured ? static_cast<const uint8_t*>(capture) : nullptr,
        captured ? capture_len : 0, captured ? capture_drop : nullptr, sched, &handle);
    return this->template finish_register_service_<S>(out, service_name, ret, handle);
}
 
template <typename S>
Result Node::finish_register_service_(Service<S>& out, const char* service_name, nros_cpp_ret_t ret,
                                      size_t handle) {
    if (ret == 0) {
        out.handle_id_ = handle;
        // phase-444 — remember the name for `get_service_name()`; the runtime
        // takes `service_name` and drops it.
        ::rclcpp::detail::assign_entity_name(out.service_name_, service_name);
 
        // issue 1437 — the arena that owns the entity, beside its index.
        out.executor_ = executor_handle_;
        out.initialized_ = true;
    }
    return Result(ret);
}
} // namespace rclcpp
 
#include "rclcpp/logging.hpp"    // phase-488 — NROS_RCLCPP_SAY_REFUSED for the shared_ptr handler
#include "rclcpp/inplace_fn.hpp" // phase-488 — a stateful handler's arena capture
 
#if defined(NROS_CPP_HAS_SHARED_PTR) // hosted-family: shared-ptr-interop
namespace rclcpp {
namespace detail {

struct FrameBlock {
    static const size_t CAPACITY = 96;
    alignas(alignof(::std::max_align_t)) unsigned char bytes[CAPACITY];
    bool used = false;
};
 
[[noreturn]] inline void abort_frame_block() {
    RCLC_LOG_ERROR(log_handle(nullptr),
                   "rclcpp: a shared_ptr control block does not fit FrameBlock::CAPACITY");
    ::std::abort();
}
 
template <typename T> struct FrameBlockAllocator {
    using value_type = T;
    FrameBlock* block;
    explicit FrameBlockAllocator(FrameBlock* b) : block(b) {}
    template <typename U> FrameBlockAllocator(const FrameBlockAllocator<U>& o) : block(o.block) {}
    T* allocate(size_t n) {
        if (block->used || n * sizeof(T) > FrameBlock::CAPACITY ||
            alignof(T) > alignof(::std::max_align_t)) {
            abort_frame_block();
        }
        block->used = true;
        return reinterpret_cast<T*>(block->bytes);
    }
    void deallocate(T*, size_t) { block->used = false; }
    template <typename U> bool operator==(const FrameBlockAllocator<U>& o) const {
        return block == o.block;
    }
    template <typename U> bool operator!=(const FrameBlockAllocator<U>& o) const {
        return block != o.block;
    }
};
 
struct LendDeleter {
    template <typename T> void operator()(T*) const {}
};
 
template <typename T> inline ::std::shared_ptr<T> lend_from_frame(T* object, FrameBlock* block) {
    return ::std::shared_ptr<T>(object, LendDeleter(), FrameBlockAllocator<T>(block));
}
 
template <typename S> struct shared_ptr_service_fn {
    using Request = typename S::Request;
    using Response = typename S::Response;
    using two = void (*)(::std::shared_ptr<Request>, ::std::shared_ptr<Response>);
    using three = void (*)(::std::shared_ptr<::rmw_request_id_t>, ::std::shared_ptr<Request>,
                           ::std::shared_ptr<Response>);
};

template <typename S, typename F> struct service_ptr_shape {
    static const int value =
        ::rclcpp::tr::is_convertible<F, typename shared_ptr_service_fn<S>::three>::value ? 3
        : ::rclcpp::tr::is_convertible<F, typename shared_ptr_service_fn<S>::two>::value ? 2
                                                                                         : 0;
};

template <typename S, typename Invoke>
inline bool serve_lent(const uint8_t* req, size_t req_len, const ::rmw_request_id_t* id,
                       uint8_t* resp, size_t resp_cap, size_t* resp_len, const Invoke& invoke) {
    typename S::Request request;
    if (S::Request::ffi_deserialize(req, req_len, &request) != 0) return false;
    typename S::Response response;
    // Issue 2081 — the id the backend reported for this request (all zero
    // when it reported none).
    ::rmw_request_id_t header = {};
    if (id != nullptr) header = *id;
    {
        FrameBlock request_block;
        FrameBlock response_block;
        FrameBlock header_block;
        ::std::shared_ptr<typename S::Request> request_ptr =
            lend_from_frame(&request, &request_block);
        ::std::shared_ptr<typename S::Response> response_ptr =
            lend_from_frame(&response, &response_block);
        ::std::shared_ptr<::rmw_request_id_t> header_ptr = lend_from_frame(&header, &header_block);
        invoke(header_ptr, request_ptr, response_ptr);
        if (request_ptr.use_count() != 1 || response_ptr.use_count() != 1 ||
            header_ptr.use_count() != 1) {
            NROS_RCLCPP_SAY_REFUSED(NROS_RCLCPP_REFUSE_RETAINED_SERVICE_PTR);
            ::std::abort();
        }
    }
    size_t len = 0;
    if (S::Response::ffi_serialize(&response, resp, resp_cap, &len) != 0) return false;
    *resp_len = len;
    return true;
}
 
template <typename S, int Shape> struct shared_ptr_service_trampoline {
    static bool call(const uint8_t* req, size_t req_len, ::rmw_request_id_t* id, uint8_t* resp,
                     size_t resp_cap, size_t* resp_len, void* ctx) {
        using Fn = shared_ptr_service_fn<S>;
        return serve_lent<S>(req, req_len, id, resp, resp_cap, resp_len,
                             [ctx](::std::shared_ptr<::rmw_request_id_t>& h,
                                   ::std::shared_ptr<typename S::Request>& rq,
                                   ::std::shared_ptr<typename S::Response>& rs) {
                                 if (Shape == 3) {
                                     fn_from_context<typename Fn::three>(ctx)(h, rq, rs);
                                 } else {
                                     (void)h;
                                     fn_from_context<typename Fn::two>(ctx)(rq, rs);
                                 }
                             });
    }
};

template <typename S, typename F> class stateful_service_shape {
    using Rq = typename S::Request;
    using Rs = typename S::Response;
    using H = ::std::shared_ptr<::rmw_request_id_t>;
    template <typename G>
    static auto probe3(int)
        -> decltype(::std::declval<G&>()(::std::declval<H&>(),
                                         ::std::declval<::std::shared_ptr<Rq>&>(),
                                         ::std::declval<::std::shared_ptr<Rs>&>()),
                    char(0)) {
        return 0;
    }
    template <typename G> static long probe3(...) { return 0; }
    template <typename G>
    static auto probe2(int)
        -> decltype(::std::declval<G&>()(::std::declval<::std::shared_ptr<Rq>&>(),
                                         ::std::declval<::std::shared_ptr<Rs>&>()),
                    char(0)) {
        return 0;
    }
    template <typename G> static long probe2(...) { return 0; }
    template <typename G>
    static auto probe1(int)
        -> decltype(::std::declval<G&>()(::std::declval<const Rq&>(), ::std::declval<Rs&>()),
                    char(0)) {
        return 0;
    }
    template <typename G> static long probe1(...) { return 0; }
 
  public:
    static const int value = sizeof(probe3<F>(0)) == sizeof(char)   ? 3
                             : sizeof(probe2<F>(0)) == sizeof(char) ? 2
                             : sizeof(probe1<F>(0)) == sizeof(char) ? 1
                                                                    : 0;
};

template <typename S>
using StatefulServiceFn = ::rclcpp::InplaceFn<void(::std::shared_ptr<::rmw_request_id_t>&,
                                                   ::std::shared_ptr<typename S::Request>&,
                                                   ::std::shared_ptr<typename S::Response>&),
                                              8 * sizeof(void*)>;
 
template <typename S, int Shape> struct stateful_service_adapter;
template <typename S> struct stateful_service_adapter<S, 3> {
    template <typename F> static StatefulServiceFn<S> wrap(F f) {
        return StatefulServiceFn<S>(
            [f](::std::shared_ptr<::rmw_request_id_t>& h,
                ::std::shared_ptr<typename S::Request>& rq,
                ::std::shared_ptr<typename S::Response>& rs) mutable { (void)f(h, rq, rs); });
    }
};
template <typename S> struct stateful_service_adapter<S, 2> {
    template <typename F> static StatefulServiceFn<S> wrap(F f) {
        return StatefulServiceFn<S>(
            [f](::std::shared_ptr<::rmw_request_id_t>&, ::std::shared_ptr<typename S::Request>& rq,
                ::std::shared_ptr<typename S::Response>& rs) mutable { (void)f(rq, rs); });
    }
};
template <typename S> struct stateful_service_adapter<S, 1> {
    template <typename F> static StatefulServiceFn<S> wrap(F f) {
        return StatefulServiceFn<S>([f](::std::shared_ptr<::rmw_request_id_t>&,
                                        ::std::shared_ptr<typename S::Request>& rq,
                                        ::std::shared_ptr<typename S::Response>& rs) mutable {
            (void)f(static_cast<const typename S::Request&>(*rq), *rs);
        });
    }
};

template <typename S> struct stateful_service_trampoline {
    static bool call(const uint8_t* req, size_t req_len, ::rmw_request_id_t* id, uint8_t* resp,
                     size_t resp_cap, size_t* resp_len, void* ctx) {
        const StatefulServiceFn<S>* fn = static_cast<const StatefulServiceFn<S>*>(ctx);
        return serve_lent<S>(req, req_len, id, resp, resp_cap, resp_len, *fn);
    }
    static void destroy(void* ctx) { static_cast<StatefulServiceFn<S>*>(ctx)->clear(); }
};

template <typename S, typename F>
inline void* shared_ptr_service_context(F callback, ::rclcpp::tr::integral_constant<int, 3>) {
    return fn_to_context(typename shared_ptr_service_fn<S>::three(callback));
}
template <typename S, typename F>
inline void* shared_ptr_service_context(F callback, ::rclcpp::tr::integral_constant<int, 2>) {
    return fn_to_context(typename shared_ptr_service_fn<S>::two(callback));
}
 
} // namespace detail
} // namespace rclcpp
#endif // NROS_CPP_HAS_SHARED_PTR
 
namespace rclcpp {} // namespace rclcpp
 
#endif // NROS_RCLCPP_SERVICE_HPP