action_server.hpp

Go to the documentation of this file.

// nros-cpp: Action server class
// Freestanding C++ — no exceptions, no STL required
 
#ifndef NROS_CPP_ACTION_SERVER_HPP
#define NROS_CPP_ACTION_SERVER_HPP
 
#include <cstdint>
#include <cstddef>
#include <string.h>
 
#include "nros/config.hpp"
#include "nros/result.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 "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);
 
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);
} // extern "C"
 
namespace nros {
 
enum class GoalResponse : int32_t {
    Reject = 0,
    AcceptAndExecute = 1,
    AcceptAndDefer = 2,
};
 
enum class CancelResponse : int32_t {
    Reject = 0,
    Accept = 1,
};
 
enum class GoalStatus : int8_t {
    Unknown = 0,
    Accepted = 1,
    Executing = 2,
    Canceling = 3,
    Succeeded = 4,
    Canceled = 5,
    Aborted = 6,
};
 
template <typename A> class ActionServer {
  public:
    using GoalType = typename A::Goal;
    using ResultType = typename A::Result;
    using FeedbackType = typename A::Feedback;
 
    using TypedGoalFn = GoalResponse (*)(const uint8_t uuid[16], const GoalType& goal);
    using TypedGoalFnWithCtx = GoalResponse (*)(const uint8_t uuid[16], const GoalType& goal,
                                                void* ctx);
    using TypedCancelFn = CancelResponse (*)(const uint8_t uuid[16]);
    using TypedCancelFnWithCtx = CancelResponse (*)(const uint8_t uuid[16], void* ctx);
    using TypedVisitorFn = void (*)(const uint8_t uuid[16], GoalStatus status);
 
    template <typename F> Result set_goal_callback(F f) {
        if (!initialized_) return Result(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();
    }
 
    Result set_goal_callback_with_ctx(TypedGoalFnWithCtx f, void* ctx) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        user_goal_fn_ctx_ = f;
        user_goal_ctx_ = ctx;
        user_goal_fn_ = nullptr;
        return install_callbacks();
    }
 
    template <typename F> Result set_cancel_callback(F f) {
        if (!initialized_) return Result(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();
    }
 
    Result set_cancel_callback_with_ctx(TypedCancelFnWithCtx f, void* ctx) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        user_cancel_fn_ctx_ = f;
        user_cancel_ctx_ = ctx;
        user_cancel_fn_ = nullptr;
        return install_callbacks();
    }
 
    Result publish_feedback(const uint8_t goal_id[16], const FeedbackType& feedback) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
 
        uint8_t buf[FeedbackType::SERIALIZED_SIZE_MAX];
        size_t len = 0;
        if (FeedbackType::ffi_serialize(&feedback, buf, sizeof(buf), &len) != 0) {
            return Result(ErrorCode::Error);
        }
        return Result(nros_cpp_action_server_publish_feedback(
            storage_, executor_, reinterpret_cast<const uint8_t(*)[16]>(goal_id), buf, len));
    }
 
    Result complete_goal(const uint8_t goal_id[16], const ResultType& result) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
 
        uint8_t buf[ResultType::SERIALIZED_SIZE_MAX];
        size_t len = 0;
        if (ResultType::ffi_serialize(&result, buf, sizeof(buf), &len) != 0) {
            return Result(ErrorCode::Error);
        }
        return Result(nros_cpp_action_server_complete_goal(
            storage_, executor_, reinterpret_cast<const uint8_t(*)[16]>(goal_id), buf, len));
    }
 
    template <typename F> Result for_each_active_goal(F f) {
        using Fn = void (*)(const uint8_t[16], GoalStatus);
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        user_visitor_fn_ = Fn(f); // compile error if F is not convertible
 
        auto trampoline = [](const uint8_t goal_id[16], int8_t status, void* ctx) {
            auto* self = static_cast<ActionServer*>(ctx);
            if (!self || self->user_visitor_fn_ == nullptr) return;
            self->user_visitor_fn_(goal_id, static_cast<GoalStatus>(status));
        };
        Result ret(nros_cpp_action_server_for_each_active_goal(
            storage_, executor_,
            reinterpret_cast<void (*)(const uint8_t(*)[16], int8_t, void*)>(+trampoline), this));
        user_visitor_fn_ = nullptr; // one-shot — don't leak the function pointer between calls
        return ret;
    }
 
    bool is_valid() const { return initialized_; }
 
    ~ActionServer() {
        if (initialized_) {
            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.
    ActionServer(ActionServer&& 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_visitor_fn_(other.user_visitor_fn_),
          initialized_(other.initialized_) {
        if (other.initialized_) {
            nros_cpp_action_server_relocate(other.storage_, storage_);
            other.initialized_ = false;
            install_callbacks();
        }
    }
 
    ActionServer& operator=(ActionServer&& other) {
        if (this != &other) {
            if (initialized_) {
                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_visitor_fn_ = other.user_visitor_fn_;
            initialized_ = other.initialized_;
            if (other.initialized_) {
                nros_cpp_action_server_relocate(other.storage_, storage_);
                other.initialized_ = false;
                install_callbacks();
            }
        }
        return *this;
    }
 
    ActionServer()
        : 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_visitor_fn_(nullptr), initialized_(false) {}
 
  private:
    ActionServer(const ActionServer&) = delete;
    ActionServer& operator=(const ActionServer&) = delete;
 
    friend class 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<ActionServer*>(ctx);
        if (!self) return static_cast<int32_t>(GoalResponse::Reject);
        GoalType g;
        if (GoalType::ffi_deserialize(data, len, &g) != 0) {
            return static_cast<int32_t>(GoalResponse::Reject);
        }
        if (self->user_goal_fn_ctx_ != nullptr) {
            return static_cast<int32_t>(self->user_goal_fn_ctx_(goal_id, g, self->user_goal_ctx_));
        }
        if (self->user_goal_fn_ != nullptr) {
            return static_cast<int32_t>(self->user_goal_fn_(goal_id, g));
        }
        return static_cast<int32_t>(GoalResponse::Reject);
    }
 
    static int32_t cancel_trampoline(const uint8_t goal_id[16], void* ctx) {
        auto* self = static_cast<ActionServer*>(ctx);
        if (!self) return static_cast<int32_t>(CancelResponse::Accept);
        if (self->user_cancel_fn_ctx_ != nullptr) {
            return static_cast<int32_t>(self->user_cancel_fn_ctx_(goal_id, self->user_cancel_ctx_));
        }
        if (self->user_cancel_fn_ != nullptr) {
            return static_cast<int32_t>(self->user_cancel_fn_(goal_id));
        }
        return static_cast<int32_t>(CancelResponse::Accept);
    }
 
    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);
        nros_cpp_goal_callback_t gcb = goal_set ? &goal_trampoline : nullptr;
        nros_cpp_cancel_callback_t ccb = cancel_set ? &cancel_trampoline : nullptr;
        return Result(nros_cpp_action_server_set_callbacks(storage_, gcb, ccb, 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_;
    TypedVisitorFn user_visitor_fn_;
    bool initialized_;
    // Phase 87.6: action name buffer moved C++-side. 256 matches
    // nros_node::limits::MAX_ACTION_NAME_LEN.
    char action_name_[256] = {};
};
 
} // namespace nros
 
// Phase 84.G8: out-of-line definition of Node::create_action_server<A>().
#include "nros/node.hpp"
 
namespace nros {
 
template <typename A>
Result Node::create_action_server(ActionServer<A>& out, const char* action_name, const QoS& qos,
                                  const ActionServerOptions& options) {
    if (!initialized_) return Result(ErrorCode::NotInitialized);
    nros_cpp_qos_t ffi_qos;
    ffi_qos.reliability = static_cast<nros_cpp_qos_reliability_t>(qos.reliability_raw());
    ffi_qos.durability = static_cast<nros_cpp_qos_durability_t>(qos.durability_raw());
    ffi_qos.history = static_cast<nros_cpp_qos_history_t>(qos.history_raw());
    ffi_qos.liveliness_kind = static_cast<nros_cpp_qos_liveliness_t>(qos.liveliness_raw());
    ffi_qos.depth = qos.depth();
    ffi_qos.deadline_ms = qos.deadline_ms();
    ffi_qos.lifespan_ms = qos.lifespan_ms();
    ffi_qos.liveliness_lease_ms = qos.liveliness_lease_ms();
    ffi_qos.avoid_ros_namespace_conventions = qos.avoid_ros_namespace_conventions() ? 1 : 0;
    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++
    // `nros::ActionServer<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 == SCHED_CONTEXT_UNSET)
                        ? 0u
                        : static_cast<uint8_t>(options.sched_context);
    ret = nros_cpp_action_server_register(out.storage_, executor_handle_, action_name, A::TYPE_NAME,
                                          A::Goal::TYPE_HASH, sched);
    if (ret == 0) {
        // Phase 87.6: copy action_name into the C++-owned buffer for
        // `get_action_name()` accessor.
        size_t name_len = 0;
        while (action_name[name_len] != '\0' && name_len + 1 < sizeof(out.action_name_)) {
            out.action_name_[name_len] = action_name[name_len];
            ++name_len;
        }
        out.action_name_[name_len] = '\0';
        out.executor_ = executor_handle_;
        out.initialized_ = true;
    }
    return Result(ret);
}
 
} // namespace nros
 
#endif // NROS_CPP_ACTION_SERVER_HPP