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_RCLCPP_STREAM_HPP
#define NROS_RCLCPP_STREAM_HPP
 
#include <cstdint>
#include <cstddef>
 
#include "rclcpp/result.hpp"
#include "rclcpp/size_bound.hpp" // rclcpp::rx_buffer_capacity<T> — the receive-buffer size
 
// 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);
}
 
// phase-428 (RFC-0089): the entity types that reach into this one's private
// constructor are DEFINED in the upstream namespace now, so the friend
// declarations below name them there. A qualified friend cannot introduce a
// name, hence these declarations.
namespace rclcpp {
template <typename S> class Client;
} // namespace rclcpp
 
// phase-456 W2b — the subscription that BINDS a stream is the poll one: a
// stream reads through `nros_cpp_subscription_take_serialized` on a
// caller-owned `RmwSubscriber`, which a dispatch registration does not have.
namespace rclcpp {
template <typename M> class PollSubscription;
} // namespace rclcpp
 
namespace rclcpp_action {
template <typename A> class Client;
} // namespace rclcpp_action
 
namespace rclcpp {

template <typename T> class Stream {
  public:
    Result try_next(T& out) { return try_next_sized<::rclcpp::rx_buffer_capacity<T>::value>(out); }

    template <size_t Cap> Result try_next_sized(T& out) {
        if (!take_fn_) return Result(ErrorCode::NotInitialized);
        uint8_t buf[Cap];
        size_t len = 0;
        nros_cpp_ret_t ret = take_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) {
        return wait_next_sized<::rclcpp::rx_buffer_capacity<T>::value>(executor_handle, timeout_ms,
                                                                       out, poll_ms);
    }

    template <size_t Cap>
    Result wait_next_sized(void* executor_handle, uint32_t timeout_ms, T& out,
                           uint32_t poll_ms = 10) {
        if (!take_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_sized<Cap>(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 take_fn_ != nullptr; }
 
    // Move semantics (non-copyable)
    Stream(Stream&& other) noexcept : storage_(other.storage_), take_fn_(other.take_fn_) {
        other.storage_ = nullptr;
        other.take_fn_ = nullptr;
    }
 
    Stream& operator=(Stream&& other) noexcept {
        if (this != &other) {
            storage_ = other.storage_;
            take_fn_ = other.take_fn_;
            other.storage_ = nullptr;
            other.take_fn_ = nullptr;
        }
        return *this;
    }

    Stream() : storage_(nullptr), take_fn_(nullptr) {}
 
  private:
    Stream(const Stream&) = delete;
    Stream& operator=(const Stream&) = delete;
 
    template <typename M> friend class ::rclcpp::PollSubscription;
    template <typename A> friend class ::rclcpp_action::Client;
 
    using TakeFn = nros_cpp_ret_t (*)(void*, uint8_t*, size_t, size_t*);
 
    Stream(void* storage, TakeFn fn) : storage_(storage), take_fn_(fn) {}
 
    void bind(void* storage, TakeFn fn) {
        storage_ = storage;
        take_fn_ = fn;
    }
 
    void* storage_;
    TakeFn take_fn_;
};
 
} // namespace rclcpp
#endif // NROS_RCLCPP_STREAM_HPP