stream.hpp

Go to the documentation of this file.

// nros-cpp: Stream<T> -- multi-shot message receiver
// Freestanding C++ -- no exceptions, no STL required
 
#ifndef NROS_CPP_STREAM_HPP
#define NROS_CPP_STREAM_HPP
 
#include <cstdint>
#include <cstddef>
 
#include "nros/result.hpp"
 
// FFI declarations
extern "C" {
typedef int nros_cpp_ret_t;
nros_cpp_ret_t nros_cpp_spin_once(void* handle, int32_t timeout_ms);
uint64_t nros_cpp_time_ns(void);
}
 
namespace nros {
 
template <typename T> class Stream {
  public:
    Result try_next(T& out) {
        if (!try_recv_fn_) return Result(ErrorCode::NotInitialized);
        uint8_t buf[T::SERIALIZED_SIZE_MAX];
        size_t len = 0;
        nros_cpp_ret_t ret = try_recv_fn_(storage_, buf, sizeof(buf), &len);
        if (ret != 0) return Result(ret);
        if (len == 0) return Result(ErrorCode::TryAgain);
        if (T::ffi_deserialize(buf, len, &out) != 0) return Result(ErrorCode::Error);
        return Result::success();
    }
 
    Result wait_next(void* executor_handle, uint32_t timeout_ms, T& out, uint32_t poll_ms = 10) {
        if (!try_recv_fn_) return Result(ErrorCode::NotInitialized);
        if (poll_ms == 0) poll_ms = 1;
        // Phase 118.C: budget by wall-clock. Accumulating `step` per
        // iteration breaks when `zpico_spin_once` returns early on a
        // signaled condvar (keep-alives, discovery gossip) — the
        // iteration loop collapses into milliseconds and returns
        // Timeout before the message has a chance to land. Mirrors the
        // Phase 89.2 fix on Future::wait().
        const uint64_t start_ns = nros_cpp_time_ns();
        const uint64_t budget_ns = static_cast<uint64_t>(timeout_ms) * 1000000ULL;
        while (true) {
            nros_cpp_ret_t ret = nros_cpp_spin_once(executor_handle, static_cast<int32_t>(poll_ms));
            // Transient conditions: keep polling. Anything else propagates.
            if (ret != 0 && ret != static_cast<nros_cpp_ret_t>(ErrorCode::Timeout) &&
                ret != static_cast<nros_cpp_ret_t>(ErrorCode::TryAgain)) {
                return Result(ret);
            }
            Result rn = try_next(out);
            if (rn.ok()) return Result::success();
            // TryAgain / NotInitialized / Error from try_next: keep polling
            // unless we've hit a hard error that's not "no data yet".
            if (rn.code() != ErrorCode::TryAgain && rn.code() != ErrorCode::NotInitialized) {
                return rn;
            }
            const uint64_t now_ns = nros_cpp_time_ns();
            if (now_ns - start_ns >= budget_ns) break;
        }
        return Result(ErrorCode::Timeout);
    }
 
    bool is_valid() const { return try_recv_fn_ != nullptr; }
 
    // Move semantics (non-copyable)
    Stream(Stream&& other) noexcept : storage_(other.storage_), try_recv_fn_(other.try_recv_fn_) {
        other.storage_ = nullptr;
        other.try_recv_fn_ = nullptr;
    }
 
    Stream& operator=(Stream&& other) noexcept {
        if (this != &other) {
            storage_ = other.storage_;
            try_recv_fn_ = other.try_recv_fn_;
            other.storage_ = nullptr;
            other.try_recv_fn_ = nullptr;
        }
        return *this;
    }
 
    Stream() : storage_(nullptr), try_recv_fn_(nullptr) {}
 
  private:
    Stream(const Stream&) = delete;
    Stream& operator=(const Stream&) = delete;
 
    template <typename M> friend class Subscription;
    template <typename A> friend class ActionClient;
 
    using TryRecvFn = nros_cpp_ret_t (*)(void*, uint8_t*, size_t, size_t*);
 
    Stream(void* storage, TryRecvFn fn) : storage_(storage), try_recv_fn_(fn) {}
 
    void bind(void* storage, TryRecvFn fn) {
        storage_ = storage;
        try_recv_fn_ = fn;
    }
 
    void* storage_;
    TryRecvFn try_recv_fn_;
};
 
} // namespace nros
#endif // NROS_CPP_STREAM_HPP