server.hpp

Go to the documentation of this file.

// nros-cpp: Action server class
// Freestanding C++ — no exceptions, no STL required
 
#ifndef NROS_RCLCPP_ACTION_SERVER_HPP
#define NROS_RCLCPP_ACTION_SERVER_HPP
 
#include <cstdint>
#include <cstddef>
#include <string.h>
 
#include "rclcpp/detail/callback_context.hpp" // phase-456 W3's one-word context carrier
#include "rclcpp/detail/config.hpp"
#include "rclcpp/detail/entity_name.hpp" // phase-444 — the one entity-name copy
#include "rclcpp/result.hpp"
// issue 0964 — the feedback/result TX buffers below size through
// `detail::buffer_bounds`, so this header needs it directly rather than by
// luck through a transitive include.
#include "rclcpp/size_bound.hpp"
 
// Phase 118.D — most action_server FFI symbols come from
// `nros_cpp_ffi.h`; cbindgen renders Rust `*const [u8; 16]` as
// pointer-to-array (`const uint8_t (*goal_id)[16]`), so callsites
// below `reinterpret_cast` from the array-decay shape used in C++
// member-function signatures.
//
// `nros_cpp_action_server_set_callbacks` is excluded from cbindgen
// output (its Rust signature uses `Option<extern "C" fn(...)>`,
// which cbindgen renders as opaque structs). Declare locally below
// with plain function-pointer typedefs that match the actual ABI.
#include "rclcpp/detail/nros_cpp_ffi.h"
 
extern "C" {
typedef int32_t (*nros_cpp_goal_callback_t)(const uint8_t goal_id[16], const uint8_t* data,
                                            size_t len, void* ctx);
typedef int32_t (*nros_cpp_cancel_callback_t)(const uint8_t goal_id[16], void* ctx);
/* Issue 0796 — post-accept hook. Excluded from cbindgen for the same reason as
 * the two above (`Option<extern "C" fn>` renders as an opaque struct), so this
 * declaration is MIRRORED in `rclc/component.h` as well; `just check c`'s
 * cross-include TU is the gate that catches a half-updated mirror. */
typedef void (*nros_cpp_accepted_callback_t)(const uint8_t goal_id[16], void* ctx);
 
nros_cpp_ret_t nros_cpp_action_server_set_callbacks(void* handle, nros_cpp_goal_callback_t goal_cb,
                                                    nros_cpp_cancel_callback_t cancel_cb,
                                                    void* ctx);
nros_cpp_ret_t
nros_cpp_action_server_set_accepted_callback(void* handle, nros_cpp_accepted_callback_t accepted_cb,
                                             void* ctx);
} // extern "C"
 
namespace rclcpp {
namespace detail {
 
// Issue 1913 — the hash each of an action's five endpoints is keyed on. An
// Iron+ peer keys every endpoint on its own type, so the one `Goal::TYPE_HASH`
// these entry points used to pass matched none of them. Generated action types
// carry the five as `<Endpoint>_HASH` members; a type generated by an older
// `nros` does not, and gets all-NULL, which the runtime reads as "use the type
// hash" (the old behaviour). C++14, so detection is spelled by hand.
template <class...> struct nros_void_ {
    typedef void type;
};
 
template <class A, class = void> struct action_endpoint_hashes_of {
    static nros_cpp_action_hashes_t get() {
        nros_cpp_action_hashes_t h = {nullptr, nullptr, nullptr, nullptr, nullptr};
        return h;
    }
};
 
template <class A>
struct action_endpoint_hashes_of<A,
                                 typename nros_void_<decltype(A::SEND_GOAL_SERVICE_HASH)>::type> {
    static nros_cpp_action_hashes_t get() {
        nros_cpp_action_hashes_t h = {A::SEND_GOAL_SERVICE_HASH, A::CANCEL_GOAL_SERVICE_HASH,
                                      A::GET_RESULT_SERVICE_HASH, A::FEEDBACK_MESSAGE_HASH,
                                      A::STATUS_MESSAGE_HASH};
        return h;
    }
};
 
template <class A> inline nros_cpp_action_hashes_t action_endpoint_hashes() {
    return action_endpoint_hashes_of<A>::get();
}
 
} // namespace detail
} // namespace rclcpp
 
// phase-483 W1: the goal vocabulary lives in `rclcpp_action::`, where upstream
// keeps it. It was `nros::` until then, and `rclcpp_action::` re-exported two
// of the five names.
namespace rclcpp_action {
 
struct GoalUUID {
    static const size_t SIZE = 16;
 
    uint8_t uuid[16];
 
    GoalUUID() : uuid{} {}
 
    explicit GoalUUID(const uint8_t* src) : uuid{} {
        for (size_t i = 0; i < SIZE; ++i) {
            uuid[i] = src[i];
        }
    }
 
    uint8_t* data() { return uuid; }
    const uint8_t* data() const { return uuid; }
 
    bool is_zero() const {
        for (size_t i = 0; i < SIZE; ++i) {
            if (uuid[i] != 0) return false;
        }
        return true;
    }
 
    bool operator==(const GoalUUID& other) const {
        for (size_t i = 0; i < SIZE; ++i) {
            if (uuid[i] != other.uuid[i]) return false;
        }
        return true;
    }
 
    bool operator!=(const GoalUUID& other) const { return !(*this == other); }
 
    bool operator<(const GoalUUID& other) const {
        for (size_t i = 0; i < SIZE; ++i) {
            if (uuid[i] != other.uuid[i]) return uuid[i] < other.uuid[i];
        }
        return false;
    }
};
 
static_assert(sizeof(GoalUUID) == 16,
              "rclcpp_action::GoalUUID must be exactly the 16 bytes the FFI passes as uint8_t[16]");
static_assert(alignof(GoalUUID) == alignof(uint8_t),
              "rclcpp_action::GoalUUID must not add alignment over its uint8_t[16]");
 
static const size_t ACTION_NAME_MAX = 256;
 
enum class GoalResponse : int8_t {
    REJECT = 1,
    ACCEPT_AND_EXECUTE = 2,
    ACCEPT_AND_DEFER = 3,
};
 
enum class CancelResponse : int8_t {
    REJECT = 1,
    ACCEPT = 2,
};
 
constexpr int32_t to_ffi(GoalResponse r) {
    return (r == GoalResponse::ACCEPT_AND_EXECUTE) ? 1
           : (r == GoalResponse::ACCEPT_AND_DEFER) ? 2
                                                   : 0;
}
 
constexpr int32_t to_ffi(CancelResponse r) {
    return (r == CancelResponse::ACCEPT) ? 1 : 0;
}
 
enum class CancelReturnCode : int8_t {
    Ok = 0,
    Rejected = 1,
    UnknownGoal = 2,
    GoalTerminated = 3,
};
 
enum class GoalStatus : int8_t {
    Unknown = 0,
    Accepted = 1,
    Executing = 2,
    Canceling = 3,
    Succeeded = 4,
    Canceled = 5,
    Aborted = 6,
};
 
} // namespace rclcpp_action
 
namespace rclcpp {
class Node;
}
 
// ============================================================================
// `rclcpp_action::Server<A>` -- DEFINED here (RFC-0089: rclcpp_action:: is the home)
// ============================================================================
//
// phase-428: the definition moved from `nros::ActionServer<A>` to
// `rclcpp_action::Server<A>` (the `nros::` alias went in phase-483 W1). This one
// is a RENAME as well as a move -- upstream's namespace already says "action",
// so the type is `Server`, not `ActionServer`.
//
// The vocabulary the signatures use (`GoalUUID`, `GoalResponse`,
// `CancelResponse`, `GoalStatus`, `CancelReturnCode`) is declared above, in
// `rclcpp_action::` where upstream keeps it (phase-483 W1; it was `nros::`
// before). `GoalResponse` and `CancelResponse` carry upstream's values since
// issue 1637, so a ported `rclcpp_action::GoalResponse::REJECT` compiles AND
// means what it meant upstream. Before that they were 0-based, and exposing
// them under the upstream name would have turned a compile error into a silent
// renumbering.
namespace rclcpp_action {
 
template <typename A> class Server {
  public:
    using GoalType = typename A::Goal;
    using ResultType = typename A::Result;
    using FeedbackType = typename A::Feedback;
 
    using TypedGoalFn = ::rclcpp_action::GoalResponse (*)(const uint8_t uuid[16],
                                                          const GoalType& goal);
    using TypedGoalFnWithCtx = ::rclcpp_action::GoalResponse (*)(const uint8_t uuid[16],
                                                                 const GoalType& goal, void* ctx);
    using TypedCancelFn = ::rclcpp_action::CancelResponse (*)(const uint8_t uuid[16]);
    using TypedCancelFnWithCtx = ::rclcpp_action::CancelResponse (*)(const uint8_t uuid[16],
                                                                     void* ctx);
    using TypedAcceptedFn = void (*)(const uint8_t uuid[16]);
    using TypedAcceptedFnWithCtx = void (*)(const uint8_t uuid[16], void* ctx);
    using TypedVisitorFn = void (*)(const uint8_t uuid[16], ::rclcpp_action::GoalStatus status);
 
    template <typename F>::rclcpp::Result set_goal_callback(F f) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        user_goal_fn_ = TypedGoalFn(f); // compile error if F is not convertible
        user_goal_fn_ctx_ = nullptr;    // mutually exclusive with _with_ctx
        user_goal_ctx_ = nullptr;
        return install_callbacks();
    }
 
    ::rclcpp::Result set_goal_callback_with_ctx(TypedGoalFnWithCtx f, void* ctx) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        user_goal_fn_ctx_ = f;
        user_goal_ctx_ = ctx;
        user_goal_fn_ = nullptr;
        return install_callbacks();
    }
 
    template <typename F>::rclcpp::Result set_cancel_callback(F f) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        user_cancel_fn_ = TypedCancelFn(f); // compile error if F is not convertible
        user_cancel_fn_ctx_ = nullptr;      // mutually exclusive with _with_ctx
        user_cancel_ctx_ = nullptr;
        return install_callbacks();
    }
 
    ::rclcpp::Result set_cancel_callback_with_ctx(TypedCancelFnWithCtx f, void* ctx) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        user_cancel_fn_ctx_ = f;
        user_cancel_ctx_ = ctx;
        user_cancel_fn_ = nullptr;
        return install_callbacks();
    }
 
    template <typename F>::rclcpp::Result set_accepted_callback(F f) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        user_accepted_fn_ = TypedAcceptedFn(f); // compile error if F is not convertible
        user_accepted_fn_ctx_ = nullptr;        // mutually exclusive with _with_ctx
        user_accepted_ctx_ = nullptr;
        return install_callbacks();
    }
 
    ::rclcpp::Result set_accepted_callback_with_ctx(TypedAcceptedFnWithCtx f, void* ctx) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        user_accepted_fn_ctx_ = f;
        user_accepted_ctx_ = ctx;
        user_accepted_fn_ = nullptr;
        return install_callbacks();
    }
 
    ::rclcpp::Result publish_feedback(const uint8_t goal_id[16], const FeedbackType& feedback) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
 
        uint8_t buf[::rclcpp::detail::buffer_bounds<FeedbackType>::tx];
        size_t len = 0;
        if (FeedbackType::ffi_serialize(&feedback, buf, sizeof(buf), &len) != 0) {
            return ::rclcpp::Result(::rclcpp::ErrorCode::Error);
        }
        return ::rclcpp::Result(nros_cpp_action_server_publish_feedback(
            storage_, executor_, reinterpret_cast<const uint8_t(*)[16]>(goal_id), buf, len));
    }
 
    ::rclcpp::Result complete_goal(const uint8_t goal_id[16], ::rclcpp_action::GoalStatus status,
                                   const ResultType& result) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
 
        uint8_t buf[::rclcpp::detail::buffer_bounds<ResultType>::tx];
        size_t len = 0;
        if (ResultType::ffi_serialize(&result, buf, sizeof(buf), &len) != 0) {
            return ::rclcpp::Result(::rclcpp::ErrorCode::Error);
        }
        return ::rclcpp::Result(nros_cpp_action_server_complete_goal(
            storage_, executor_, reinterpret_cast<const uint8_t(*)[16]>(goal_id),
            static_cast<int32_t>(status), buf, len));
    }
 
    ::rclcpp::Result complete_goal(const uint8_t goal_id[16], const ResultType& result) {
        return complete_goal(goal_id, ::rclcpp_action::GoalStatus::Succeeded, result);
    }
 
    ::rclcpp::Result publish_feedback(const ::rclcpp_action::GoalUUID& goal_id,
                                      const FeedbackType& feedback) {
        return publish_feedback(goal_id.data(), feedback);
    }
 
    ::rclcpp::Result complete_goal(const ::rclcpp_action::GoalUUID& goal_id,
                                   ::rclcpp_action::GoalStatus status, const ResultType& result) {
        return complete_goal(goal_id.data(), status, result);
    }
 
    ::rclcpp::Result complete_goal(const ::rclcpp_action::GoalUUID& goal_id,
                                   const ResultType& result) {
        return complete_goal(goal_id.data(), ::rclcpp_action::GoalStatus::Succeeded, result);
    }
 
    ::rclcpp::Result succeed(const uint8_t goal_id[16], const ResultType& result) {
        return complete_goal(goal_id, ::rclcpp_action::GoalStatus::Succeeded, result);
    }
 
    ::rclcpp::Result abort(const uint8_t goal_id[16], const ResultType& result) {
        return complete_goal(goal_id, ::rclcpp_action::GoalStatus::Aborted, result);
    }
 
    ::rclcpp::Result canceled(const uint8_t goal_id[16], const ResultType& result) {
        return complete_goal(goal_id, ::rclcpp_action::GoalStatus::Canceled, result);
    }
 
    ::rclcpp::Result succeed(const ::rclcpp_action::GoalUUID& goal_id, const ResultType& result) {
        return complete_goal(goal_id.data(), ::rclcpp_action::GoalStatus::Succeeded, result);
    }
 
    ::rclcpp::Result abort(const ::rclcpp_action::GoalUUID& goal_id, const ResultType& result) {
        return complete_goal(goal_id.data(), ::rclcpp_action::GoalStatus::Aborted, result);
    }
 
    ::rclcpp::Result canceled(const ::rclcpp_action::GoalUUID& goal_id, const ResultType& result) {
        return complete_goal(goal_id.data(), ::rclcpp_action::GoalStatus::Canceled, result);
    }
 
    template <typename F>::rclcpp::Result for_each_active_goal(F f) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        TypedVisitorFn visitor = TypedVisitorFn(f); // compile error if F is not convertible
 
        auto trampoline = [](const uint8_t goal_id[16], int8_t status, void* ctx) {
            TypedVisitorFn fn = ::rclcpp::detail::fn_from_context<TypedVisitorFn>(ctx);
            if (fn == nullptr) return;
            fn(goal_id, static_cast<::rclcpp_action::GoalStatus>(status));
        };
        return ::rclcpp::Result(nros_cpp_action_server_for_each_active_goal(
            storage_, executor_,
            reinterpret_cast<void (*)(const uint8_t(*)[16], int8_t, void*)>(+trampoline),
            ::rclcpp::detail::fn_to_context(visitor)));
    }
 
    const char* get_action_name() const { return initialized_ ? action_name_ : ""; }
 
    bool goal_exists(const uint8_t goal_id[16]) const {
        if (!initialized_) return false;
        return nros_cpp_action_server_goal_exists(storage_, executor_,
                                                  reinterpret_cast<const uint8_t(*)[16]>(goal_id));
    }
 
    bool goal_exists(const ::rclcpp_action::GoalUUID& goal_id) const {
        return goal_exists(goal_id.data());
    }
 
    bool is_valid() const { return initialized_; }
 
    ~Server() {
        if (initialized_) {
            nros_cpp_action_server_detach(storage_, executor_);
            nros_cpp_action_server_destroy(storage_);
            initialized_ = false;
        }
    }
 
    // Move semantics (non-copyable). Relocation goes through the
    // `nros_cpp_action_server_relocate` runtime call (Phase 84.C1) and
    // then `install_callbacks()` re-registers the goal/cancel trampolines
    // with the new `this` as the arena callback context — this is the one
    // type in nros-cpp that registers its storage address externally.
    Server(Server&& other)
        : executor_(other.executor_), user_goal_fn_(other.user_goal_fn_),
          user_goal_fn_ctx_(other.user_goal_fn_ctx_), user_goal_ctx_(other.user_goal_ctx_),
          user_cancel_fn_(other.user_cancel_fn_), user_cancel_fn_ctx_(other.user_cancel_fn_ctx_),
          user_cancel_ctx_(other.user_cancel_ctx_), user_accepted_fn_(other.user_accepted_fn_),
          user_accepted_fn_ctx_(other.user_accepted_fn_ctx_),
          user_accepted_ctx_(other.user_accepted_ctx_), action_name_{},
          initialized_(other.initialized_) {
        ::memcpy(action_name_, other.action_name_, sizeof(action_name_));
        if (other.initialized_) {
            nros_cpp_action_server_relocate(other.storage_, storage_);
            other.initialized_ = false;
            install_callbacks();
        }
    }
 
    Server& operator=(Server&& other) {
        if (this != &other) {
            if (initialized_) {
                // issue 1496 — same pairing as `~Server()`: release the arena
                // entry this storage owns before the storage is overwritten by
                // the relocate below.
                nros_cpp_action_server_detach(storage_, executor_);
                nros_cpp_action_server_destroy(storage_);
            }
            executor_ = other.executor_;
            user_goal_fn_ = other.user_goal_fn_;
            user_goal_fn_ctx_ = other.user_goal_fn_ctx_;
            user_goal_ctx_ = other.user_goal_ctx_;
            user_cancel_fn_ = other.user_cancel_fn_;
            user_cancel_fn_ctx_ = other.user_cancel_fn_ctx_;
            user_cancel_ctx_ = other.user_cancel_ctx_;
            user_accepted_fn_ = other.user_accepted_fn_;
            user_accepted_fn_ctx_ = other.user_accepted_fn_ctx_;
            user_accepted_ctx_ = other.user_accepted_ctx_;
            ::memcpy(action_name_, other.action_name_, sizeof(action_name_));
            initialized_ = other.initialized_;
            if (other.initialized_) {
                nros_cpp_action_server_relocate(other.storage_, storage_);
                other.initialized_ = false;
                install_callbacks();
            }
        }
        return *this;
    }
 
    Server()
        : executor_(nullptr), user_goal_fn_(nullptr), user_goal_fn_ctx_(nullptr),
          user_goal_ctx_(nullptr), user_cancel_fn_(nullptr), user_cancel_fn_ctx_(nullptr),
          user_cancel_ctx_(nullptr), user_accepted_fn_(nullptr), user_accepted_fn_ctx_(nullptr),
          user_accepted_ctx_(nullptr), action_name_{}, initialized_(false) {}
 
  private:
    Server(const Server&) = delete;
    Server& operator=(const Server&) = delete;
 
    friend class ::rclcpp::Node;
 
    // ── C trampolines ───────────────────────────────────────────────
    //
    // `ctx` is a pointer to this `ActionServer<A>` instance, so the
    // trampoline reads the user's stored function pointer via the
    // instance's own fields — no shared mutable statics.
 
    static int32_t goal_trampoline(const uint8_t goal_id[16], const uint8_t* data, size_t len,
                                   void* ctx) {
        auto* self = static_cast<Server*>(ctx);
        if (!self) return ::rclcpp_action::to_ffi(::rclcpp_action::GoalResponse::REJECT);
        GoalType g;
        if (GoalType::ffi_deserialize(data, len, &g) != 0) {
            return ::rclcpp_action::to_ffi(::rclcpp_action::GoalResponse::REJECT);
        }
        if (self->user_goal_fn_ctx_ != nullptr) {
            return ::rclcpp_action::to_ffi(
                self->user_goal_fn_ctx_(goal_id, g, self->user_goal_ctx_));
        }
        if (self->user_goal_fn_ != nullptr) {
            return ::rclcpp_action::to_ffi(self->user_goal_fn_(goal_id, g));
        }
        return ::rclcpp_action::to_ffi(::rclcpp_action::GoalResponse::REJECT);
    }
 
    static int32_t cancel_trampoline(const uint8_t goal_id[16], void* ctx) {
        auto* self = static_cast<Server*>(ctx);
        if (!self) return ::rclcpp_action::to_ffi(::rclcpp_action::CancelResponse::ACCEPT);
        if (self->user_cancel_fn_ctx_ != nullptr) {
            return ::rclcpp_action::to_ffi(
                self->user_cancel_fn_ctx_(goal_id, self->user_cancel_ctx_));
        }
        if (self->user_cancel_fn_ != nullptr) {
            return ::rclcpp_action::to_ffi(self->user_cancel_fn_(goal_id));
        }
        return ::rclcpp_action::to_ffi(::rclcpp_action::CancelResponse::ACCEPT);
    }
 
    static void accepted_trampoline(const uint8_t goal_id[16], void* ctx) {
        auto* self = static_cast<Server*>(ctx);
        if (!self) return;
        if (self->user_accepted_fn_ctx_ != nullptr) {
            self->user_accepted_fn_ctx_(goal_id, self->user_accepted_ctx_);
            return;
        }
        if (self->user_accepted_fn_ != nullptr) {
            self->user_accepted_fn_(goal_id);
        }
    }
 
    ::rclcpp::Result install_callbacks() {
        bool goal_set = (user_goal_fn_ != nullptr) || (user_goal_fn_ctx_ != nullptr);
        bool cancel_set = (user_cancel_fn_ != nullptr) || (user_cancel_fn_ctx_ != nullptr);
        bool accepted_set = (user_accepted_fn_ != nullptr) || (user_accepted_fn_ctx_ != nullptr);
        nros_cpp_goal_callback_t gcb = goal_set ? &goal_trampoline : nullptr;
        nros_cpp_cancel_callback_t ccb = cancel_set ? &cancel_trampoline : nullptr;
        nros_cpp_ret_t ret = nros_cpp_action_server_set_callbacks(storage_, gcb, ccb, this);
        if (ret != 0) return ::rclcpp::Result(ret);
        // Issue 0796 — a SECOND install call rather than a fourth parameter on
        // the first: `nros_cpp_action_server_set_callbacks` is declared in
        // three places and called directly by C components, so growing it
        // would have broken every one of them. Both calls pass the same
        // `this`, which is the shared context slot the runtime keeps.
        nros_cpp_accepted_callback_t acb = accepted_set ? &accepted_trampoline : nullptr;
        return ::rclcpp::Result(nros_cpp_action_server_set_accepted_callback(storage_, acb, this));
    }
 
    alignas(8) uint8_t storage_[NROS_CPP_ACTION_SERVER_STORAGE_SIZE];
    void* executor_; // Executor context needed for feedback/result operations
    TypedGoalFn user_goal_fn_;
    TypedGoalFnWithCtx user_goal_fn_ctx_;
    void* user_goal_ctx_;
    TypedCancelFn user_cancel_fn_;
    TypedCancelFnWithCtx user_cancel_fn_ctx_;
    void* user_cancel_ctx_;
    TypedAcceptedFn user_accepted_fn_;
    TypedAcceptedFnWithCtx user_accepted_fn_ctx_;
    void* user_accepted_ctx_;
    // phase-456 W3b — a `TypedVisitorFn user_visitor_fn_` sat here, written at
    // the head of `for_each_active_goal` and cleared at its tail. The visitor
    // travels in the FFI's own `void* ctx` now; see that function.
    // Phase 87.6 put a `char action_name_[256]` here for a `get_action_name()`
    // that was never written, and an earlier phase-417 W4.b wave deleted it as
    // dead state. W4.b's second half writes the accessor, so it is back and it
    // is read — see `get_action_name()` for why the FFI cannot answer instead.
    char action_name_[::rclcpp_action::ACTION_NAME_MAX];
    bool initialized_;
};
 
} // namespace rclcpp_action
 
// Phase 84.G8: out-of-line definition of Node::create_action_server<A>().
#include "rclcpp/node.hpp"
 
namespace rclcpp {} // namespace rclcpp
 
namespace rclcpp {
template <typename A>
Result Node::create_action_server(::rclcpp_action::Server<A>& out, const char* action_name,
                                  const ::rclcpp::QoS& qos,
                                  const ::rclcpp::ActionServerOptions& options) {
    if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
    nros_cpp_qos_t ffi_qos = ::rclcpp::detail::qos_to_ffi(qos);
    nros_cpp_ret_t ret = nros_cpp_action_server_create(&handle_, action_name, A::TYPE_NAME,
                                                       A::Goal::TYPE_HASH, ffi_qos, out.storage_);
    if (ret != 0) return Result(ret);
    // Register with executor — creates transport handles (3 queryables + 2 publishers).
    // Deferred from create to avoid FreeRTOS QEMU deadlocks. Phase 87.6:
    // names are passed at register-time (buffers live on the C++
    // `rclcpp_action::Server<A>` class, not in the runtime struct).
    // Phase 189.M3.3.c — `sched_context` binds the action's (arena-registered)
    // goal-service handle to a scheduling context. UNSET ⇒ 0 (inherit, no-op).
    uint8_t sched = (options.sched_context == ::rclcpp::SCHED_CONTEXT_UNSET)
                        ? 0u
                        : static_cast<uint8_t>(options.sched_context);
    const nros_cpp_action_hashes_t hashes = ::rclcpp::detail::action_endpoint_hashes<A>();
    ret = nros_cpp_action_server_register(out.storage_, executor_handle_, action_name, A::TYPE_NAME,
                                          A::Goal::TYPE_HASH, &hashes, sched);
    if (ret == 0) {
        out.executor_ = executor_handle_;
        // phase-417 W4.b — remember the name for `get_action_name()`. Truncating
        // copy, as `Subscription`'s and the two polling tiers' do: a name longer
        // than `rclcpp_action::ACTION_NAME_MAX` is not a reason to fail a create that the
        // runtime already accepted.
        ::rclcpp::detail::assign_entity_name(out.action_name_, action_name);
        out.initialized_ = true;
    }
    return Result(ret);
}
} // namespace rclcpp
 
namespace rclcpp {} // namespace rclcpp
 
#endif // NROS_RCLCPP_ACTION_SERVER_HPP