subscription.hpp

Go to the documentation of this file.

// nros-cpp: Subscription class
// Freestanding C++ — no exceptions, no STL required
 
#ifndef NROS_CPP_SUBSCRIPTION_HPP
#define NROS_CPP_SUBSCRIPTION_HPP
 
#include <cstdint>
#include <cstddef>
#include <string.h> // memcpy — `<cstring>` isn't in Zephyr's minimal libcpp
 
#include "nros/config.hpp"
#include "nros/result.hpp"
#include "nros/stream.hpp"
 
#include "nros_cpp_ffi.h"
 
// Phase 189.M3.x — `nros_cpp_subscription_register` is excluded from cbindgen
// (its Rust signature uses `RawSubscriptionCallback`, an external-crate type
// alias cbindgen names without defining). Declare it locally with a plain
// function-pointer typedef matching the ABI (`void(data, len, ctx)`), mirroring
// the service.hpp callback-register treatment.
extern "C" {
typedef void (*nros_cpp_subscription_message_callback_t)(const uint8_t* data, size_t len,
                                                         void* ctx);
 
nros_cpp_ret_t nros_cpp_subscription_register(const nros_cpp_node_t* node, const char* topic,
                                              const char* type_name, const char* type_hash,
                                              nros_cpp_qos_t qos,
                                              nros_cpp_subscription_message_callback_t callback,
                                              void* context, uint8_t sched_context,
                                              size_t* out_handle_id);
 
// Phase 189.M3.4 — callback-style register that also delivers the sample's wire
// attachment (5-arg trampoline). Same cbindgen-exclusion reason as above.
typedef void (*nros_cpp_subscription_message_info_callback_t)(const uint8_t* data, size_t len,
                                                              const uint8_t* attachment,
                                                              size_t attachment_len, void* ctx);
 
nros_cpp_ret_t nros_cpp_subscription_register_with_info(
    const nros_cpp_node_t* node, const char* topic, const char* type_name, const char* type_hash,
    nros_cpp_qos_t qos, nros_cpp_subscription_message_info_callback_t callback, void* context,
    uint8_t sched_context, size_t* out_handle_id);
} // extern "C"
 
namespace nros {
 
static constexpr size_t SUBSCRIPTION_TOPIC_NAME_MAX = 256;
 
template <typename M> class Subscription {
  public:
    using TypedSubscriptionFn = void (*)(const M& msg);
    using TypedSubscriptionFnWithCtx = void (*)(const M& msg, void* ctx);
    // Phase 189.M3.4 — callback-with-attachment handler (`bridge_origin` etc.).
    using TypedSubscriptionInfoFn = void (*)(const M& msg, const uint8_t* attachment,
                                             size_t attachment_len);
 
    Result try_recv(M& msg) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        uint8_t buf[M::SERIALIZED_SIZE_MAX];
        size_t len = 0;
        nros_cpp_ret_t ret = nros_cpp_subscription_try_recv_raw(storage_, buf, sizeof(buf), &len);
        if (ret != 0) return Result(ret);
        if (len == 0) return Result(ErrorCode::TryAgain);
        if (M::ffi_deserialize(buf, len, &msg) != 0) return Result(ErrorCode::Error);
        return Result::success();
    }
 
    Result try_recv_validated(M& msg, nros_cpp_integrity_status_t& status) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        uint8_t buf[M::SERIALIZED_SIZE_MAX];
        size_t len = 0;
        nros_cpp_ret_t ret =
            nros_cpp_subscription_try_recv_validated(storage_, buf, sizeof(buf), &len, &status);
        if (ret != 0) return Result(ret);
        if (len == 0) return Result(ErrorCode::TryAgain);
        if (M::ffi_deserialize(buf, len, &msg) != 0) return Result(ErrorCode::Error);
        return Result::success();
    }
 
    Result try_recv_raw(uint8_t* buf, size_t capacity, size_t& out_len) {
        if (!initialized_) {
            out_len = 0;
            return Result(ErrorCode::NotInitialized);
        }
        nros_cpp_ret_t ret = nros_cpp_subscription_try_recv_raw(storage_, buf, capacity, &out_len);
        if (ret != 0) return Result(ret);
        if (out_len == 0) return Result(ErrorCode::TryAgain);
        return Result::success();
    }
 
    Result try_recv_raw_with_attachment(uint8_t* buf, size_t capacity, size_t& out_len,
                                        uint8_t* att, size_t att_capacity, size_t& out_att_len) {
        if (!initialized_) {
            out_len = 0;
            out_att_len = 0;
            return Result(ErrorCode::NotInitialized);
        }
        nros_cpp_ret_t ret = nros_cpp_subscription_try_recv_raw_with_attachment(
            storage_, buf, capacity, &out_len, att, att_capacity, &out_att_len);
        if (ret != 0) return Result(ret);
        if (out_len == 0) return Result(ErrorCode::TryAgain);
        return Result::success();
    }
 
    // ====================================================================
    // Phase 124.A.7 — zero-copy receive (borrow / release)
    // ====================================================================
 
    class View {
      public:
        View() : sub_(nullptr), buf_(nullptr), len_(0), token_(nullptr) {}
        View(View&& o) : sub_(o.sub_), buf_(o.buf_), len_(o.len_), token_(o.token_) {
            o.sub_ = nullptr;
            o.token_ = nullptr;
        }
        View& operator=(View&& o) {
            if (this != &o) {
                release();
                sub_ = o.sub_;
                buf_ = o.buf_;
                len_ = o.len_;
                token_ = o.token_;
                o.sub_ = nullptr;
                o.token_ = nullptr;
            }
            return *this;
        }
        View(const View&) = delete;
        View& operator=(const View&) = delete;
        ~View() { release(); }
 
        const uint8_t* data() const { return buf_; }
        size_t size() const { return len_; }
        bool empty() const { return token_ == nullptr; }
 
        View(void* sub, const uint8_t* buf, size_t len, void* token)
            : sub_(sub), buf_(buf), len_(len), token_(token) {}
 
      private:
        void release() {
            if (token_ && sub_) {
                nros_cpp_subscription_release(sub_, token_);
                token_ = nullptr;
            }
        }
 
        void* sub_;
        const uint8_t* buf_;
        size_t len_;
        void* token_;
    };
 
    Expected<View> try_borrow() {
        if (!initialized_) return Expected<View>::error(Result(ErrorCode::NotInitialized));
        const uint8_t* buf = nullptr;
        size_t len = 0;
        void* token = nullptr;
        int32_t rc = nros_cpp_subscription_borrow(storage_, &buf, &len, &token);
        if (rc < 0) return Expected<View>::error(Result(rc));
        if (rc == 0) return Expected<View>::ok(View{});
        return Expected<View>::ok(View{storage_, buf, len, token});
    }
 
    Result try_recv_sequence(uint8_t* buf, size_t per_msg_cap, size_t max_msgs, size_t* out_lens,
                             size_t& out_count) {
        if (!initialized_) {
            out_count = 0;
            return Result(ErrorCode::NotInitialized);
        }
        nros_cpp_ret_t ret = nros_cpp_subscription_try_recv_sequence(
            storage_, buf, per_msg_cap, max_msgs, out_lens, &out_count);
        if (ret != 0) return Result(ret);
        return Result::success();
    }
 
    const char* get_topic_name() const { return initialized_ ? topic_name_ : ""; }
 
    Stream<M>& stream() {
        if (initialized_ && !stream_.is_valid()) {
            stream_.bind(storage_, &nros_cpp_subscription_try_recv_raw);
        }
        return stream_;
    }
 
    const Stream<M>& stream() const { return stream_; }
 
    bool is_valid() const { return initialized_; }
 
    ~Subscription() {
        if (initialized_ && !callback_mode_) {
            nros_cpp_subscription_destroy(storage_);
        }
        initialized_ = false;
    }
 
    // Move semantics (non-copyable). Relocation goes through the
    // `nros_cpp_subscription_relocate` runtime call (Phase 84.C1);
    // the `stream_` is rebound to the new storage afterwards.
    // A callback-style subscription must NOT be moved after register — the
    // executor arena holds `this` as the trampoline context (Phase 189.M3.x);
    // the move only transfers bookkeeping and leaves that pointer stale. The
    // poll-style relocation path is unchanged.
    Subscription(Subscription&& other) : initialized_(other.initialized_) {
        user_fn_ = other.user_fn_;
        user_fn_ctx_ = other.user_fn_ctx_;
        user_ctx_ = other.user_ctx_;
        callback_mode_ = other.callback_mode_;
        sched_handle_id_ = other.sched_handle_id_;
        if (other.initialized_ && !other.callback_mode_) {
            nros_cpp_subscription_relocate(other.storage_, storage_);
            ::memcpy(topic_name_, other.topic_name_, sizeof(topic_name_));
            stream_.bind(storage_, &nros_cpp_subscription_try_recv_raw);
        }
        other.initialized_ = false;
        other.stream_ = Stream<M>();
    }
 
    Subscription& operator=(Subscription&& other) {
        if (this != &other) {
            if (initialized_ && !callback_mode_) {
                nros_cpp_subscription_destroy(storage_);
                stream_ = Stream<M>();
            }
            initialized_ = other.initialized_;
            user_fn_ = other.user_fn_;
            user_fn_ctx_ = other.user_fn_ctx_;
            user_ctx_ = other.user_ctx_;
            callback_mode_ = other.callback_mode_;
            sched_handle_id_ = other.sched_handle_id_;
            if (other.initialized_ && !other.callback_mode_) {
                nros_cpp_subscription_relocate(other.storage_, storage_);
                ::memcpy(topic_name_, other.topic_name_, sizeof(topic_name_));
                stream_.bind(storage_, &nros_cpp_subscription_try_recv_raw);
            }
            other.initialized_ = false;
            other.stream_ = Stream<M>();
        }
        return *this;
    }
 
    Subscription() : storage_(), topic_name_{}, initialized_(false), stream_() {}
 
    bool has_sched_handle() const { return sched_handle_id_ != static_cast<size_t>(-1); }
    size_t sched_handle_id() const { return sched_handle_id_; }
 
    // ====================================================================
    // Phase 108 — status events
    // ====================================================================
 
    Result on_liveliness_changed(nros_cpp_liveliness_changed_cb_t cb,
                                 void* user_context = nullptr) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        return Result(nros_cpp_subscription_set_liveliness_changed(storage_, cb, user_context));
    }
 
    Result on_requested_deadline_missed(uint32_t deadline_ms, nros_cpp_subscriber_count_cb_t cb,
                                        void* user_context = nullptr) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        return Result(nros_cpp_subscription_set_requested_deadline_missed(storage_, deadline_ms, cb,
                                                                          user_context));
    }
 
    Result on_message_lost(nros_cpp_subscriber_count_cb_t cb, void* user_context = nullptr) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        return Result(nros_cpp_subscription_set_message_lost(storage_, cb, user_context));
    }
 
  private:
    Subscription(const Subscription&) = delete;
    Subscription& operator=(const Subscription&) = delete;
 
    friend class Node;
 
    static void message_trampoline(const uint8_t* data, size_t len, void* ctx) {
        auto* self = static_cast<Subscription*>(ctx);
        if (self == nullptr) return;
        M msg;
        if (M::ffi_deserialize(data, len, &msg) != 0) return;
        if (self->user_fn_ != nullptr) {
            self->user_fn_(msg);
        } else if (self->user_fn_ctx_ != nullptr) {
            self->user_fn_ctx_(msg, self->user_ctx_);
        }
    }
 
    static void message_info_trampoline(const uint8_t* data, size_t len, const uint8_t* attachment,
                                        size_t attachment_len, void* ctx) {
        auto* self = static_cast<Subscription*>(ctx);
        if (self == nullptr) return;
        M msg;
        if (M::ffi_deserialize(data, len, &msg) != 0) return;
        if (self->user_fn_info_ != nullptr) {
            self->user_fn_info_(msg, attachment, attachment_len);
        }
    }
 
    alignas(8) uint8_t storage_[NROS_SUBSCRIBER_SIZE];
    char topic_name_[SUBSCRIPTION_TOPIC_NAME_MAX];
    bool initialized_;
    Stream<M> stream_;
    // Phase 189.M3.1 — executor HandleId for sched-context binding, or
    // SIZE_MAX (the default) when no bindable handle exists. The poll-style
    // thin-wrapper create path leaves this unset (see has_sched_handle());
    // the callback-style create (Phase 189.M3.x) stores the real arena handle.
    size_t sched_handle_id_ = static_cast<size_t>(-1);
    // Callback-style state (Phase 189.M3.x); unused in poll mode. The executor
    // arena owns the subscriber + dispatches `message_trampoline` during spin.
    TypedSubscriptionFn user_fn_ = nullptr;
    TypedSubscriptionFnWithCtx user_fn_ctx_ = nullptr;
    TypedSubscriptionInfoFn user_fn_info_ = nullptr;
    void* user_ctx_ = nullptr;
    bool callback_mode_ = false;
};
 
} // namespace nros
 
// Phase 84.G8: out-of-line definition of Node::create_subscription<M>().
#include "nros/node.hpp"
 
namespace nros {
 
template <typename M>
Result Node::create_subscription(Subscription<M>& out, const char* topic, const QoS& qos) {
    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_subscription_create(&handle_, topic, M::TYPE_NAME, M::TYPE_HASH,
                                                      ffi_qos, out.storage_);
    if (ret == 0) {
        // Phase 87.6: topic name lives C++-side now.
        size_t topic_len = 0;
        while (topic[topic_len] != '\0' && topic_len + 1 < sizeof(out.topic_name_)) {
            out.topic_name_[topic_len] = topic[topic_len];
            ++topic_len;
        }
        out.topic_name_[topic_len] = '\0';
        out.initialized_ = true;
    }
    return Result(ret);
}
 
template <typename M>
Result Node::create_subscription(Subscription<M>& out, const char* topic, const QoS& qos,
                                 const SubscriptionOptions& options) {
    Result r = create_subscription<M>(out, topic, qos);
    if (!r.ok()) return r;
 
    // TODO(M3.4): honour options.message_info via the with-info arena path.
 
    if (options.sched_context != SCHED_CONTEXT_UNSET && out.has_sched_handle()) {
        nros_cpp_ret_t bind = nros_cpp_bind_handle_to_sched_context(
            executor_handle_, out.sched_handle_id(), static_cast<uint8_t>(options.sched_context));
        if (bind != 0) {
            // Roll back so the caller doesn't observe a half-configured
            // entity. Destructor-on-out would also fire, but explicit
            // teardown keeps the returned error authoritative.
            nros_cpp_subscription_destroy(out.storage_);
            out.initialized_ = false;
            return Result(bind);
        }
    }
    return Result::success();
}
 
// Phase 189.M3.x — callback-style (arena-registered) subscription. The arena
// owns the subscriber + dispatches `out`'s message handler during spin_once, so
// the handle is real and `options.sched_context` is functional. Mirrors the
// callback-style `create_service` one entity over.
template <typename M, typename F, typename>
Result Node::create_subscription(Subscription<M>& out, const char* topic, F callback,
                                 const QoS& qos, const SubscriptionOptions& 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;
 
    // Store the user handler (compile error if F isn't convertible to the
    // plain-fn-ptr handler type).
    out.user_fn_ = typename Subscription<M>::TypedSubscriptionFn(callback);
    out.user_fn_ctx_ = nullptr;
    out.user_ctx_ = nullptr;
 
    uint8_t sched = (options.sched_context == SCHED_CONTEXT_UNSET)
                        ? 0u
                        : static_cast<uint8_t>(options.sched_context);
    size_t handle = static_cast<size_t>(-1);
    nros_cpp_ret_t ret =
        nros_cpp_subscription_register(&handle_, topic, M::TYPE_NAME, M::TYPE_HASH, ffi_qos,
                                       &Subscription<M>::message_trampoline, &out, sched, &handle);
    if (ret == 0) {
        out.sched_handle_id_ = handle;
        out.callback_mode_ = true;
        out.initialized_ = true;
    }
    return Result(ret);
}
 
// Phase 189.M3.4 — callback-style subscription that delivers the wire attachment.
// Mirrors the callback `create_subscription` one step over, but stores the
// `(const M&, attachment, att_len)` handler + registers via the with-info arena
// path so the trampoline receives the attachment.
template <typename M, typename F, typename>
Result Node::create_subscription_with_info(Subscription<M>& out, const char* topic, F callback,
                                           const QoS& qos, const SubscriptionOptions& 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;
 
    out.user_fn_info_ = typename Subscription<M>::TypedSubscriptionInfoFn(callback);
    out.user_fn_ = nullptr;
    out.user_fn_ctx_ = nullptr;
    out.user_ctx_ = nullptr;
 
    uint8_t sched = (options.sched_context == SCHED_CONTEXT_UNSET)
                        ? 0u
                        : static_cast<uint8_t>(options.sched_context);
    size_t handle = static_cast<size_t>(-1);
    nros_cpp_ret_t ret = nros_cpp_subscription_register_with_info(
        &handle_, topic, M::TYPE_NAME, M::TYPE_HASH, ffi_qos,
        &Subscription<M>::message_info_trampoline, &out, sched, &handle);
    if (ret == 0) {
        out.sched_handle_id_ = handle;
        out.callback_mode_ = true;
        out.initialized_ = true;
    }
    return Result(ret);
}
 
template <typename M>
inline Expected<Subscription<M>> make_subscription(Node& node, const char* topic,
                                                   const QoS& qos = QoS::default_profile()) {
    Subscription<M> s;
    Result r = node.create_subscription<M>(s, topic, qos);
    if (!r.ok()) return Expected<Subscription<M>>::error(r);
    return Expected<Subscription<M>>::ok(std::move(s));
}
 
} // namespace nros
 
#endif // NROS_CPP_SUBSCRIPTION_HPP