publisher.hpp

Go to the documentation of this file.

// nros-cpp: Publisher class
// Freestanding C++ — no exceptions, no STL required

 
#ifndef NROS_RCLCPP_PUBLISHER_HPP
#define NROS_RCLCPP_PUBLISHER_HPP
 
#include <cstdint>
#include <cstddef>
#include <string.h> // memcpy — `<cstring>` isn't in Zephyr's minimal libcpp
 
#include "rclcpp/detail/traits.hpp"
#include "rclcpp/detail/config.hpp"
#include "rclcpp/detail/std_detect.hpp"  // phase-488 — `NROS_CPP_HAS_SHARED_PTR`
#include "rclcpp/detail/entity_name.hpp" // phase-444 — the one entity-name copy
#include "rclcpp/executor.hpp"           // phase-444 — the graph counter these forward to
#include "rclcpp/owned.hpp" // phase-456 W5 — `Publisher<M>::SharedPtr` IS `Owned<Publisher<M>>`
#include "rclcpp/result.hpp"
#if defined(NROS_CPP_STD) // hosted-family: ported-publish
#include <rclc/log.h>     // phase-488 — `detail::report_failed_publish`
#endif
// RFC-0088 D5 — NROS_CPP_ASSERT_MESSAGE_FORMAT, expanded in the creator below.
#include "rclcpp/detail/serialization_format.hpp"
 
#include "rclcpp/detail/nros_cpp_ffi.h"
#include "rclcpp/event.hpp" // issue 1972 — MatchedInfo / IncompatibleTypeInfo
 
// issue 1437 — `get_actual_qos()` returns a `rclcpp::QoS` BY VALUE from an
// inline body, so the complete type must be here, not only by the time
// `rclcpp/node.hpp` is pulled in below.
//
// AFTER `nros_cpp_ffi.h`, never before: `qos.hpp` defines the four
// `nros_cpp_qos_*_t` enums ITSELF under `#ifndef NROS_CPP_FFI_H`, so
// reaching it first makes the cbindgen header a REDEFINITION of all four.
#include "rclcpp/qos.hpp"
 
// phase-456 W5 — THIS HEADER NO LONGER DETECTS ANYTHING, and the include that
// used to sit here is gone with the block it served.
//
// From phase-417 W1.a until now, `Publisher<M>::SharedPtr` and friends were
// `std::shared_ptr` / `std::unique_ptr`, so they needed `<memory>` — which a
// MINIMAL freestanding libcpp does not ship (Zephyr's is `<cstddef>`,
// `<cstdint>`, `<new>`, plus this repo's `zephyr/cxx-compat/`) — and the
// aliases were therefore ABSENT on exactly the targets ported source needs them
// on. The predicate that decided it needed three separate corrections to get
// right (issues 0112, 1187, 1240).
//
// The aliases name `rclcpp::Owned<Publisher<M>>` now, which is ours and exists
// everywhere, so there is no capability left to probe: the alias is present
// unconditionally and means the same thing on every target. That is RFC-0096 D1
// — one API, no shape decided by a toolchain flag — reached by deleting the
// question rather than by answering it more carefully.
 
namespace rclcpp {

static constexpr size_t PUBLISHER_TOPIC_NAME_MAX = 256;
 
} // namespace rclcpp

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

#if defined(NROS_CPP_STD) // hosted-family: ported-publish
namespace detail {
class PublishResult : public ::rclcpp::ResultOf<void> {
  public:
    constexpr PublishResult(::rclcpp::ResultOf<void> r) : ::rclcpp::ResultOf<void>(r) {}
};

template <typename M> inline void report_failed_publish(const char* topic, int32_t code) {
    static unsigned long failures = 0;
    if ((failures++ % 1024UL) == 0) {
        RCLC_LOG_ERROR(rclc_log_default_logger(),
                       "rclcpp::Publisher::publish on \"%.64s\" failed: rclcpp::ErrorCode %d "
                       "(%lu failure(s) so far)",
                       topic, static_cast<int>(code), failures);
    }
}
} // namespace detail
#endif
 
template <typename M> class Publisher {
  public:
    using SharedPtr = ::rclcpp::Owned<Publisher<M>>;
    using ConstSharedPtr = ::rclcpp::Owned<Publisher<M>>;
    using UniquePtr = ::rclcpp::Owned<Publisher<M>>;

    static constexpr size_t GID_SIZE = 24;

#if defined(NROS_CPP_STD) // hosted-family: ported-publish
    ::rclcpp::detail::PublishResult publish(const M& msg) {
        const Result r = initialized_ ? Result(M::ffi_publish(storage_, &msg))
                                      : Result(::rclcpp::ErrorCode::NotInitialized);
        if (!r.ok()) ::rclcpp::detail::report_failed_publish<M>(get_topic_name(), r.raw());
        return r;
    }
#if defined(NROS_CPP_HAS_SHARED_PTR) // hosted-family: shared-ptr-interop
    template <typename D>::rclcpp::detail::PublishResult publish(::std::unique_ptr<M, D> msg) {
        if (!msg) {
            const Result r(::rclcpp::ErrorCode::InvalidArgument);
            ::rclcpp::detail::report_failed_publish<M>(get_topic_name(), r.raw());
            return r;
        }
        return publish(static_cast<const M&>(*msg));
    }
#endif
#else
    Result publish(const M& msg) {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return Result(M::ffi_publish(storage_, &msg));
    }
#endif

    Result publish_serialized(const uint8_t* data, size_t len) {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return Result(nros_cpp_publish_raw(storage_, data, len));
    }

    NROS_CPP_DEPRECATED_MSG("renamed to publish_serialized (phase-515 C8)")
    Result publish_raw(const uint8_t* data, size_t len) { return publish_serialized(data, len); }

    template <typename W> Result publish_streamed(size_t total_len, W&& writer) {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        struct Ctx {
            W writer;
            size_t total;
        } ctx{static_cast<W&&>(writer), total_len};
        auto size_cb = [](size_t* out_total, void* user) {
            auto* c = static_cast<Ctx*>(user);
            *out_total = c->total;
        };
        auto chunk_cb = [](uint8_t* out_buf, size_t cap, size_t* out_written, void* user) {
            auto* c = static_cast<Ctx*>(user);
            *out_written = c->writer(out_buf, cap);
        };
        return Result(nros_cpp_publisher_publish_streamed(storage_, size_cb, chunk_cb,
                                                          static_cast<void*>(&ctx)));
    }
 
    // ====================================================================
    // Phase 124.A.7 — zero-copy publish (loan / commit / discard)
    // ====================================================================

    class Loan {
      public:
        Loan() : pub_(nullptr), buf_(nullptr), cap_(0), token_(nullptr) {}
        Loan(Loan&& o) : pub_(o.pub_), buf_(o.buf_), cap_(o.cap_), token_(o.token_) {
            o.pub_ = nullptr;
            o.token_ = nullptr;
        }
        Loan& operator=(Loan&& o) {
            if (this != &o) {
                release();
                pub_ = o.pub_;
                buf_ = o.buf_;
                cap_ = o.cap_;
                token_ = o.token_;
                o.pub_ = nullptr;
                o.token_ = nullptr;
            }
            return *this;
        }
        Loan(const Loan&) = delete;
        Loan& operator=(const Loan&) = delete;
        ~Loan() { release(); }

        uint8_t* data() { return buf_; }
        const uint8_t* data() const { return buf_; }
        size_t capacity() const { return cap_; }
 
        bool is_valid() const { return token_ != nullptr; }

        Result commit(size_t actual_len) {
            if (!token_) return Result(::rclcpp::ErrorCode::NotInitialized);
            nros_cpp_ret_t ret = nros_cpp_publisher_commit(pub_, token_, actual_len);
            pub_ = nullptr;
            token_ = nullptr;
            buf_ = nullptr;
            cap_ = 0;
            return Result(ret);
        }

        Result discard() {
            if (!token_) return Result::success();
            nros_cpp_ret_t ret = nros_cpp_publisher_discard(pub_, token_);
            pub_ = nullptr;
            token_ = nullptr;
            buf_ = nullptr;
            cap_ = 0;
            return Result(ret);
        }

        Loan(void* pub, uint8_t* buf, size_t cap, void* token)
            : pub_(pub), buf_(buf), cap_(cap), token_(token) {}
 
      private:
        void release() {
            if (token_ && pub_) {
                nros_cpp_publisher_discard(pub_, token_);
                token_ = nullptr;
            }
        }
 
        void* pub_;
        uint8_t* buf_;
        size_t cap_;
        void* token_;
    };

    ::rclcpp::ResultOf<Loan> loan(size_t requested_len) {
        if (!initialized_)
            return ::rclcpp::ResultOf<Loan>::error(Result(::rclcpp::ErrorCode::NotInitialized));
        uint8_t* buf = nullptr;
        size_t cap = 0;
        void* token = nullptr;
        nros_cpp_ret_t ret = nros_cpp_publisher_loan(storage_, requested_len, &buf, &cap, &token);
        if (ret != 0) return ::rclcpp::ResultOf<Loan>::error(Result(ret));
        return ::rclcpp::ResultOf<Loan>::ok(Loan{storage_, buf, cap, token});
    }

    const char* get_topic_name() const { return initialized_ ? topic_name_ : ""; }

    ::rclcpp::QoS get_actual_qos() const {
        nros_cpp_qos_t f{};
        if (!initialized_ || nros_cpp_publisher_get_actual_qos(storage_, &f) != 0) {
            return ::rclcpp::detail::qos_all_unknown();
        }
        return ::rclcpp::detail::qos_from_ffi(f);
    }

    bool is_durability_transient_local() const {
        return get_actual_qos().durability() == ::rclcpp::DurabilityPolicy::TransientLocal;
    }

    Result get_subscription_count(::rclcpp::Executor& executor, size_t* out_count) const {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return executor.count_subscribers(topic_name_, out_count);
    }

    bool is_valid() const { return initialized_; }

    ~Publisher() {
        if (initialized_) {
            nros_cpp_publisher_destroy(storage_);
            initialized_ = false;
        }
    }
 
    // Move semantics (non-copyable). Relocation goes through the
    // `nros_cpp_publisher_relocate` runtime call.
    Publisher(Publisher&& other) : initialized_(other.initialized_) {
        if (other.initialized_) {
            nros_cpp_publisher_relocate(other.storage_, storage_);
            ::memcpy(topic_name_, other.topic_name_, sizeof(topic_name_));
            other.initialized_ = false;
        }
    }
 
    Publisher& operator=(Publisher&& other) {
        if (this != &other) {
            if (initialized_) {
                nros_cpp_publisher_destroy(storage_);
                initialized_ = false;
            }
            if (other.initialized_) {
                nros_cpp_publisher_relocate(other.storage_, storage_);
                ::memcpy(topic_name_, other.topic_name_, sizeof(topic_name_));
                initialized_ = true;
                other.initialized_ = false;
            }
        }
        return *this;
    }

    Publisher() : storage_(), topic_name_{}, initialized_(false) {}
 
    // ====================================================================
    // Phase 108 — status events
    // ====================================================================

    Result on_liveliness_lost(nros_cpp_publisher_count_cb_t cb, void* user_context = nullptr) {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return Result(nros_cpp_publisher_set_liveliness_lost(storage_, cb, user_context));
    }

    Result on_offered_deadline_missed(uint32_t deadline_ms, nros_cpp_publisher_count_cb_t cb,
                                      void* user_context = nullptr) {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return Result(nros_cpp_publisher_set_offered_deadline_missed(storage_, deadline_ms, cb,
                                                                     user_context));
    }

    Result on_matched(PublisherMatchedCallbackType cb, void* user_context = nullptr) {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return Result(nros_cpp_publisher_set_matched(storage_, cb, user_context));
    }

    Result on_incompatible_type(IncompatibleTypeCallbackType cb, void* user_context = nullptr) {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return Result(nros_cpp_publisher_set_incompatible_type(storage_, cb, user_context));
    }

    Result assert_liveliness() {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return Result(nros_cpp_publisher_assert_liveliness(storage_));
    }

    Result get_gid(uint8_t (&out_gid)[GID_SIZE]) const {
        if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
        return Result(nros_cpp_publisher_get_gid(storage_, out_gid, GID_SIZE));
    }
 
  private:
    Publisher(const Publisher&) = delete;
    Publisher& operator=(const Publisher&) = delete;
 
    friend class ::rclcpp::Node;
 
    // `+ sizeof(void*)`: the monitor cell pointer the runtime stores beside
    // the handle (`CppPublisher` in nros-cpp's publisher.rs, whose
    // compile-time assert is the other half of this number).
    alignas(8) uint8_t storage_[NROS_PUBLISHER_SIZE + sizeof(void*)];
    char topic_name_[::rclcpp::PUBLISHER_TOPIC_NAME_MAX];
    bool initialized_;
};
 
} // namespace rclcpp
 
// Phase 84.G8: out-of-line definition of Node::create_publisher<M>().
// Placed here so consumers only pay for this template when they actually
// use publishers — including `rclcpp/node.hpp` alone no longer drags in
// every entity's code path.
#include "rclcpp/node.hpp"
 
namespace rclcpp {} // namespace rclcpp
 
namespace rclcpp {
template <typename M>
Result Node::create_publisher(Publisher<M>& out, const char* topic, const ::rclcpp::QoS& qos) {
    // RFC-0088 D5 — one image, one backend, one encoding. Compile-time, so a
    // message the linked backend cannot encode never reaches the wire.
    NROS_CPP_ASSERT_MESSAGE_FORMAT(M);
    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_publisher_create(&handle_, topic, M::TYPE_NAME, M::TYPE_HASH,
                                                   ffi_qos, out.storage_);
    if (ret == 0) {
        // Topic name lives C++-side; copy + null-terminate into the
        // fixed-size buffer. Truncation is silent.
        ::rclcpp::detail::assign_entity_name(out.topic_name_, topic);
        out.initialized_ = true;
    }
    return Result(ret);
}
} // namespace rclcpp
 
namespace rclcpp {

} // namespace rclcpp
 
namespace rclcpp {
template <typename M>
Result Node::create_publisher(Publisher<M>& out, const char* topic, const ::rclcpp::QoS& qos,
                              const ::rclcpp::PublisherOptions& options) {
    (void)options; // reserved — no live fields today
    return create_publisher<M>(out, topic, qos);
}
} // namespace rclcpp
 
namespace rclcpp {

template <typename M>
inline ResultOf<Publisher<M>> create_publisher(::rclcpp::Node& node, const char* topic,
                                               const QoS& qos = QoS::default_profile()) {
    Publisher<M> p;
    Result r = node.create_publisher<M>(p, topic, qos);
    if (!r.ok()) return ResultOf<Publisher<M>>::error(r);
    return ResultOf<Publisher<M>>::ok(::rclcpp::tr::forward_rvalue(p));
}
 
} // namespace rclcpp
 
#endif // NROS_RCLCPP_PUBLISHER_HPP