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/owned.hpp"              // issue 2015 — `Server<A>::SharedPtr`
#include "rclcpp/result.hpp"
#include "rclcpp_action/detail/upstream.hpp" // issue 2015 — hosted goal-handle plumbing
// 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 ServerGoalHandle;
namespace detail {
template <typename A> struct ServerUpstream;
} // namespace detail

template <typename A> class Server {
  public:
    using GoalType = typename A::Goal;
    using ResultType = typename A::Result;
    using FeedbackType = typename A::Feedback;

    using SharedPtr = ::rclcpp::Owned<Server>;
    using GoalHandle = ::rclcpp_action::ServerGoalHandle<A>;

    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 execute(const uint8_t goal_id[16]) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        return ::rclcpp::Result(nros_cpp_action_server_execute(
            storage_, executor_, reinterpret_cast<const uint8_t(*)[16]>(goal_id)));
    }

    ::rclcpp::Result execute(const ::rclcpp_action::GoalUUID& goal_id) {
        return execute(goal_id.data());
    }

    ::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); then
    // `nros_cpp_action_server_retarget` re-points the executor's arena entry
    // (whose callback context is the STORAGE address) at the new storage —
    // issue 2015: without it a moved server dispatched through the moved-from
    // bytes — and `install_callbacks()` re-registers the goal/cancel
    // trampolines with the new `this` as THEIR context.
    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_),
          upstream_(static_cast<detail::HostedBox&&>(other.upstream_)) {
        ::memcpy(action_name_, other.action_name_, sizeof(action_name_));
        if (other.initialized_) {
            nros_cpp_action_server_relocate(other.storage_, storage_);
            // issue 2015 — the arena entry follows the storage.
            (void)nros_cpp_action_server_retarget(storage_, executor_);
            other.initialized_ = false;
            (void)install_callbacks();
        }
        detail::ServerUpstream<A>::rehome(*this);
    }
 
    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_;
            upstream_ = static_cast<detail::HostedBox&&>(other.upstream_);
            if (other.initialized_) {
                nros_cpp_action_server_relocate(other.storage_, storage_);
                // issue 2015 — the arena entry follows the storage.
                (void)nros_cpp_action_server_retarget(storage_, executor_);
                other.initialized_ = false;
                (void)install_callbacks();
            }
            detail::ServerUpstream<A>::rehome(*this);
        }
        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;
    // Issue 2015 — the hosted goal handle asks this server for its goal's
    // status, and `create_server` binds upstream's callbacks onto it.
    friend class ::rclcpp_action::ServerGoalHandle<A>;
    friend struct detail::ServerUpstream<A>;

    bool goal_status(const uint8_t goal_id[16], int8_t& out) {
        if (!initialized_) return false;
        struct Query {
            const uint8_t* id;
            int8_t status;
            bool found;
        } q = {goal_id, 0, false};
        auto visit = [](const uint8_t(*id)[16], int8_t status, void* ctx) {
            Query* qq = static_cast<Query*>(ctx);
            if (::memcmp(*id, qq->id, 16) == 0) {
                qq->status = status;
                qq->found = true;
            }
        };
        if (nros_cpp_action_server_for_each_active_goal(storage_, executor_, +visit, &q) != 0)
            return false;
        out = q.status;
        return q.found;
    }
 
    // ── 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_;
    detail::HostedBox upstream_;
};

template <typename A> class ServerGoalHandle {
  public:
    using Goal = typename A::Goal;
    using Feedback = typename A::Feedback;
    using Result = typename A::Result;
#ifdef NROS_CPP_HAS_SHARED_PTR // hosted-family: action-interop
    using SharedPtr = ::std::shared_ptr<ServerGoalHandle>;
    using ConstSharedPtr = ::std::shared_ptr<const ServerGoalHandle>;
#else
    using SharedPtr = detail::Refused<ServerGoalHandle>;
    using ConstSharedPtr = detail::Refused<ServerGoalHandle>;
#endif
 
    ServerGoalHandle(Server<A>* server, const GoalUUID& uuid)
        : server_(server), uuid_(uuid), goal_() {}

    const GoalUUID& get_goal_id() const { return uuid_; }
    bool is_canceling() const { return status() == GoalStatus::Canceling; }
    bool is_executing() const { return status() == GoalStatus::Executing; }
    void execute() {
        if (server_ != nullptr) (void)server_->execute(uuid_.data());
    }

    bool is_active() const {
        const GoalStatus s = status();
        return s == GoalStatus::Accepted || s == GoalStatus::Executing ||
               s == GoalStatus::Canceling;
    }
 
#ifdef NROS_CPP_HAS_SHARED_PTR // hosted-family: action-interop
    ::std::shared_ptr<const Goal> get_goal() const {
        auto* g = goal_.template as<::std::shared_ptr<const Goal>>();
        return g != nullptr ? *g : ::std::shared_ptr<const Goal>();
    }
    void publish_feedback(::std::shared_ptr<Feedback> feedback) {
        if (feedback && server_ != nullptr)
            (void)server_->publish_feedback(uuid_.data(), *feedback);
    }
    void succeed(::std::shared_ptr<Result> result) { terminate(GoalStatus::Succeeded, result); }
    void abort(::std::shared_ptr<Result> result) { terminate(GoalStatus::Aborted, result); }
    void canceled(::std::shared_ptr<Result> result) { terminate(GoalStatus::Canceled, result); }
#endif
 
  private:
    friend struct detail::ServerUpstream<A>;
 
    GoalStatus status() const {
        int8_t s = 0;
        if (server_ == nullptr || !server_->goal_status(uuid_.data(), s))
            return GoalStatus::Unknown;
        return static_cast<GoalStatus>(s);
    }
 
#ifdef NROS_CPP_HAS_SHARED_PTR // hosted-family: action-interop
    void terminate(GoalStatus status, const ::std::shared_ptr<Result>& result) {
        if (server_ == nullptr) return;
        const Result empty{};
        (void)server_->complete_goal(uuid_.data(), status, result ? *result : empty);
        detail::ServerUpstream<A>::forget(*server_, uuid_);
    }
#endif
 
    Server<A>* server_;
    GoalUUID uuid_;
    detail::HostedBox goal_;
};
 
namespace detail {

template <typename A> struct ServerUpstream {
    using Goal = typename A::Goal;
    using Handle = ServerGoalHandle<A>;

    static void rehome(Server<A>& s) {
#ifdef NROS_CPP_HAS_SHARED_PTR // hosted-family: action-interop
        if (State* st = s.upstream_.template as<State>()) st->server = &s;
#else
        (void)s;
#endif
    }
 
#ifdef NROS_CPP_HAS_SHARED_PTR // hosted-family: action-interop
    using GoalFn = ::std::function<GoalResponse(const GoalUUID&, ::std::shared_ptr<const Goal>)>;
    using CancelFn = ::std::function<CancelResponse(::std::shared_ptr<Handle>)>;
    using AcceptedFn = ::std::function<void(::std::shared_ptr<Handle>)>;
 
    struct Entry {
        GoalUUID id;
        ::std::shared_ptr<const Goal> goal;
        bool execute;
    };
    struct State {
        GoalFn goal;
        CancelFn cancel;
        AcceptedFn accepted;
        Server<A>* server;
        ::std::vector<Entry> goals;
    };
 
    static void destroy(void* p) { delete static_cast<State*>(p); }
 
    template <typename G, typename C, typename Acc>
    static ::rclcpp::Result bind(Server<A>& s, G&& goal, C&& cancel, Acc&& accepted) {
        State* st = new State{GoalFn(static_cast<G&&>(goal)),
                              CancelFn(static_cast<C&&>(cancel)),
                              AcceptedFn(static_cast<Acc&&>(accepted)),
                              &s,
                              {}};
        s.upstream_.reset(st, &destroy);
        ::rclcpp::Result r = s.set_goal_callback_with_ctx(&on_goal, st);
        if (r.ok()) r = s.set_cancel_callback_with_ctx(&on_cancel, st);
        if (r.ok()) r = s.set_accepted_callback_with_ctx(&on_accepted, st);
        return r;
    }

    static void forget(Server<A>& s, const GoalUUID& id) {
        State* st = s.upstream_.template as<State>();
        if (st == nullptr) return;
        for (size_t i = 0; i < st->goals.size(); ++i) {
            if (st->goals[i].id == id) {
                st->goals.erase(st->goals.begin() + static_cast<long>(i));
                return;
            }
        }
    }
 
  private:
    static ::std::shared_ptr<Handle> make_handle(State* st, const GoalUUID& id) {
        ::std::shared_ptr<Handle> h = ::std::make_shared<Handle>(st->server, id);
        for (const Entry& e : st->goals) {
            if (e.id == id) {
                h->goal_.reset(new ::std::shared_ptr<const Goal>(e.goal), [](void* p) {
                    delete static_cast<::std::shared_ptr<const Goal>*>(p);
                });
                break;
            }
        }
        return h;
    }
 
    static GoalResponse on_goal(const uint8_t uuid[16], const Goal& goal, void* ctx) {
        State* st = static_cast<State*>(ctx);
        if (!st->goal) return GoalResponse::REJECT;
        const GoalUUID id(uuid);
        ::std::shared_ptr<const Goal> msg = ::std::make_shared<const Goal>(goal);
        const GoalResponse r = st->goal(id, msg);
        if (r != GoalResponse::REJECT) {
            forget(*st->server, id);
            st->goals.push_back(Entry{id, msg, r == GoalResponse::ACCEPT_AND_EXECUTE});
        }
        return r;
    }
 
    static CancelResponse on_cancel(const uint8_t uuid[16], void* ctx) {
        State* st = static_cast<State*>(ctx);
        if (!st->cancel) return CancelResponse::REJECT;
        return st->cancel(make_handle(st, GoalUUID(uuid)));
    }
 
    static void on_accepted(const uint8_t uuid[16], void* ctx) {
        State* st = static_cast<State*>(ctx);
        const GoalUUID id(uuid);
        // Issue 2027 — upstream's server calls `execute()` itself for
        // ACCEPT_AND_EXECUTE, so the goal is EXECUTING (and the status array
        // says so) by the time `handle_accepted` sees it; ACCEPT_AND_DEFER
        // stays ACCEPTED until the handle's `execute()`.
        for (const Entry& e : st->goals) {
            if (e.id == id) {
                if (e.execute) (void)st->server->execute(uuid);
                break;
            }
        }
        if (st->accepted) st->accepted(make_handle(st, id));
    }
#endif
};
 
} // namespace detail
 
} // 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