future.hpp

Go to the documentation of this file.

// nros-cpp: Future<T> -- single-shot deferred result
// Freestanding C++ -- no exceptions, no STL required

 
#ifndef NROS_RCLCPP_FUTURE_HPP
#define NROS_RCLCPP_FUTURE_HPP
 
#include <cstdint>
#include <cstddef>
 
#include "rclcpp/result.hpp"
#include "rclcpp/size_bound.hpp" // rclcpp::rx_buffer_capacity<T> — the receive-buffer size
// issue 1996 — `refuse<T>`, `say_refused` and the `NROS_RCLCPP_REFUSE_FUTURE_GET*`
// messages; it also brings `std_detect.hpp`, which defines the capability
// macros `get()` and `wait_for()` are gated on.
#include "rclcpp/logging.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);
}
 
// 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 M> class Subscription;
} // namespace rclcpp
 
namespace rclcpp_action {
template <typename A> class Client;
} // namespace rclcpp_action
 
namespace rclcpp {
 
// phase-456 W9 — `send_request` and the `Future` it hands back moved off
// `rclcpp::Client<S>` onto `rclcpp::PollClient<S>`, the half that owns the
// `RmwServiceClient` this future takes from. The friend list FOLLOWS the method:
// the poll client is declared here, and `rclcpp::Client<S>` left both this
// declaration and the friend list below, because a friendship nothing uses is a
// claim about the code that is not true.
template <typename S> class PollClient;
 
namespace detail {

constexpr int64_t SEQUENCE_ID_UNKNOWN = (-9223372036854775807LL - 1);

constexpr int MAX_STALE_DRAIN_PER_POLL = 8;

using TakeSeqFn = nros_cpp_ret_t (*)(void*, uint8_t*, size_t, size_t*, int64_t*);

using PumpFn = nros_cpp_ret_t (*)(void*);

struct PendingList;
struct PendingLink {
    PendingLink* next = nullptr;
    PendingList* list = nullptr;
    void* storage = nullptr;
    int64_t seq = SEQUENCE_ID_UNKNOWN;
    bool by_goal = false;
    uint8_t goal_id[16] = {};
    uint8_t* buf = nullptr; 
    size_t cap = 0;
    size_t len = 0;
    bool ready = false;
    bool orphaned = false;
};

struct PendingList {
    PendingLink* head = nullptr;
    uint8_t orphans = 0;
    void (*forget)(void* ctx, const uint8_t goal[16]) = nullptr;
    void* forget_ctx = nullptr;
 
    size_t count() const {
        size_t n = 0;
        for (PendingLink* l = head; l != nullptr; l = l->next)
            ++n;
        return n;
    }
 
    bool can_send() const {
        for (PendingLink* l = head; l != nullptr; l = l->next)
            if (l->seq == SEQUENCE_ID_UNKNOWN) return false;
        return true;
    }
 
    void link(PendingLink* l) {
        l->next = head;
        head = l;
    }

    bool unlink(PendingLink* l, bool abandoned) {
        for (PendingLink** pp = &head; *pp != nullptr; pp = &(*pp)->next) {
            if (*pp == l) {
                *pp = l->next;
                l->next = nullptr;
                // A goal-keyed (action) future names its reply exactly, so its
                // removal costs no later reply (issue 2011).
                if (abandoned && !l->by_goal && l->seq == SEQUENCE_ID_UNKNOWN && orphans < 255)
                    ++orphans;
                // Issue 2027 — an abandoned goal-keyed future was the owner of
                // one request in the Rust core's bounded table; tell the list's
                // owner, so the slot does not outlive the last owner.
                if (abandoned && l->by_goal && forget != nullptr) forget(forget_ctx, l->goal_id);
                return true;
            }
        }
        return false;
    }

    void relink(PendingLink* from, PendingLink* to) {
        for (PendingLink** pp = &head; *pp != nullptr; pp = &(*pp)->next) {
            if (*pp == from) {
                to->next = from->next;
                *pp = to;
                from->next = nullptr;
                return;
            }
        }
    }

    PendingLink* resolve(int64_t seq) {
        if (seq == SEQUENCE_ID_UNKNOWN) {
            if (orphans > 0) {
                --orphans;
                return nullptr;
            }
            return (head != nullptr && head->next == nullptr) ? head : nullptr;
        }
        for (PendingLink* l = head; l != nullptr; l = l->next)
            if (l->seq == seq) return l;
        // The backend gave the REPLY an id but not the request: the sole
        // pending request is the only candidate.
        if (head != nullptr && head->next == nullptr && head->seq == SEQUENCE_ID_UNKNOWN)
            return head;
        return nullptr;
    }

    PendingLink* resolve_goal(const uint8_t goal[16]) {
        for (PendingLink* l = head; l != nullptr; l = l->next) {
            if (!l->by_goal) continue;
            bool same = true;
            for (int i = 0; i < 16; ++i) {
                if (l->goal_id[i] != goal[i]) {
                    same = false;
                    break;
                }
            }
            if (same) return l;
        }
        return nullptr;
    }

    void move_to(PendingList& to, void* storage) {
        to.head = head;
        to.orphans = orphans;
        for (PendingLink* l = head; l != nullptr; l = l->next) {
            l->list = &to;
            l->storage = storage;
        }
        head = nullptr;
        orphans = 0;
    }

    size_t prune() {
        size_t n = 0;
        while (head != nullptr) {
            PendingLink* l = head;
            (void)unlink(l, true);
            l->list = nullptr;
            l->orphaned = true;
            ++n;
        }
        return n;
    }

    bool remove_seq(int64_t seq) {
        if (seq == SEQUENCE_ID_UNKNOWN) return false;
        for (PendingLink* l = head; l != nullptr; l = l->next) {
            if (l->by_goal || l->seq != seq) continue;
            (void)unlink(l, true);
            l->list = nullptr;
            l->orphaned = true;
            return true;
        }
        return false;
    }

    void orphan_all() {
        PendingLink* l = head;
        while (l != nullptr) {
            PendingLink* next = l->next;
            l->next = nullptr;
            l->list = nullptr;
            l->storage = nullptr;
            l->orphaned = true;
            l = next;
        }
        head = nullptr;
        orphans = 0;
    }

    void deliver(PendingLink* owner, const uint8_t* data, size_t len) {
        const size_t n = len < owner->cap ? len : owner->cap;
        if (owner->buf != data) {
            for (size_t i = 0; i < n; ++i)
                owner->buf[i] = data[i];
        }
        owner->len = n;
        owner->ready = true;
        (void)unlink(owner, false);
    }
};
 
} // namespace detail

template <typename T, size_t Cap = ::rclcpp::rx_buffer_capacity<T>::value> class Future {
  public:
    bool is_ready() {
        if (!live()) return false;
        if (link_.ready) return true;
        if (take_seq_fn_ != nullptr) return poll_keyed();
        if (pump_fn_ != nullptr) return poll_pumped();
        if (!take_fn_) return false;
        size_t len = 0;
        nros_cpp_ret_t ret = take_fn_(link_.storage, cached_buf_, sizeof(cached_buf_), &len);
        if (ret == 0 && len > 0) {
            link_.ready = true;
            link_.len = len < sizeof(cached_buf_) ? len : sizeof(cached_buf_);
            return true;
        }
        return false;
    }

    Result try_take(T& out) {
        if (!live()) return Result(ErrorCode::Error);
        if (!link_.ready && !is_ready()) return Result(ErrorCode::Error);
        slot_ = -1; // consume
        if (T::ffi_deserialize(cached_buf_, link_.len, &out) != 0) {
            return Result(ErrorCode::Error);
        }
        return Result::success();
    }

    Result wait(void* executor_handle, uint32_t timeout_ms, T& out, uint32_t poll_ms = 10) {
        if (!live()) return Result(ErrorCode::Error);
        if (poll_ms == 0) poll_ms = 1;
        // Phase 89.2: budget by wall-clock. Accumulating `step` per iteration
        // breaks when the underlying `zpico_spin_once` returns early on a
        // signaled condvar (keep-alives, discovery gossip) — the 500-step
        // default-timeout loop can then collapse into milliseconds and
        // return ErrorCode::Timeout before the reply has a chance to land.
        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.
            // - Ok (0): nothing to dispatch this round.
            // - Timeout (-2): spin_once returned after its timeout — normal.
            // - TryAgain (-6): transport hint to retry.
            if (ret != 0 && ret != static_cast<nros_cpp_ret_t>(ErrorCode::Timeout) &&
                ret != static_cast<nros_cpp_ret_t>(ErrorCode::TryAgain)) {
                return Result(ret);
            }
            if (is_ready()) return try_take(out);
            const uint64_t now_ns = nros_cpp_time_ns();
            if (now_ns - start_ns >= budget_ns) break;
        }
        return Result(ErrorCode::Timeout);
    }

    void cancel() {
        detach(true);
        slot_ = -1;
    }

    bool valid() const { return live(); }
 
#if defined(NROS_CPP_HAS_SHARED_PTR) // hosted-family: shared-ptr-interop
    ::std::shared_ptr<T> get() {
        if (!live() || (!link_.ready && !is_ready())) {
            NROS_RCLCPP_SAY_REFUSED(NROS_RCLCPP_REFUSE_FUTURE_GET_NOT_READY_RUNTIME);
            return ::std::shared_ptr<T>();
        }
        ::std::shared_ptr<T> out = ::std::make_shared<T>();
        if (!try_take(*out).ok()) return ::std::shared_ptr<T>();
        return out;
    }
#else
    template <typename U = void> void get() {
        static_assert(::rclcpp::detail::refuse<U>::value, NROS_RCLCPP_REFUSE_FUTURE_GET);
    }
#endif // NROS_CPP_HAS_SHARED_PTR
 
#if defined(NROS_CPP_HAS_STD_FUTURE) // hosted-family: future-interop
    template <typename Rep, typename Period>
    ::std::future_status wait_for(::std::chrono::duration<Rep, Period> timeout) {
        if (!live()) return ::std::future_status::timeout;
        if (link_.ready || is_ready()) return ::std::future_status::ready;
        const int64_t ms = ::rclcpp::detail::chrono_wait_budget_ms(timeout);
        if (ms <= 0 || executor_ == nullptr) {
            if (executor_ != nullptr) (void)nros_cpp_spin_once(executor_, 0);
            return is_ready() ? ::std::future_status::ready : ::std::future_status::timeout;
        }
        const uint64_t start_ns = nros_cpp_time_ns();
        const uint64_t budget_ns = static_cast<uint64_t>(ms) * 1000000ULL;
        while (true) {
            const uint64_t elapsed_ns = nros_cpp_time_ns() - start_ns;
            if (elapsed_ns >= budget_ns) break;
            const uint64_t left_ms = (budget_ns - elapsed_ns + 999999ULL) / 1000000ULL;
            const int32_t slice = left_ms < 10ULL ? static_cast<int32_t>(left_ms) : 10;
            (void)nros_cpp_spin_once(executor_, slice);
            if (is_ready()) return ::std::future_status::ready;
        }
        return is_ready() ? ::std::future_status::ready : ::std::future_status::timeout;
    }
#endif // NROS_CPP_HAS_STD_FUTURE

    bool is_consumed() const { return !live(); }
 
    // Move semantics (non-copyable, single-shot). A linked future's list entry
    // FOLLOWS it to the new address (issue 2009), so a future may be returned
    // by value and stored wherever the caller likes while it is in flight.
    Future(Future&& other) noexcept
        : take_fn_(other.take_fn_), take_seq_fn_(other.take_seq_fn_), pump_fn_(other.pump_fn_),
          executor_(other.executor_), slot_(other.slot_) {
        adopt(other);
    }
 
    Future& operator=(Future&& other) noexcept {
        if (this != &other) {
            cancel();
            take_fn_ = other.take_fn_;
            take_seq_fn_ = other.take_seq_fn_;
            pump_fn_ = other.pump_fn_;
            executor_ = other.executor_;
            slot_ = other.slot_;
            adopt(other);
        }
        return *this;
    }
 
    ~Future() { cancel(); }

    Future()
        : take_fn_(nullptr), take_seq_fn_(nullptr), pump_fn_(nullptr), executor_(nullptr),
          slot_(-1) {
        link_.buf = cached_buf_;
        link_.cap = sizeof(cached_buf_);
    }
 
  private:
    Future(const Future&) = delete;
    Future& operator=(const Future&) = delete;
 
    template <typename S> friend class ::rclcpp::PollClient;
    template <typename A> friend class ::rclcpp_action::Client;
 
    using TryRecvFn = nros_cpp_ret_t (*)(void*, uint8_t*, size_t, size_t*);
 
    // `executor` is what `wait_for` drives (issue 1996); nullptr = no
    // executor known, and `wait_for` then checks once instead of waiting.
    // An UNKEYED future: the take hook carries no request id, so the future
    // trusts whatever reply it takes. Nothing in-tree builds one any more —
    // the action futures are goal-keyed since issue 2011 — but the shape stays
    // for a caller with exactly one request in flight.
    Future(void* storage, TryRecvFn fn, int slot, void* executor = nullptr)
        : take_fn_(fn), take_seq_fn_(nullptr), pump_fn_(nullptr), executor_(executor), slot_(slot) {
        link_.buf = cached_buf_;
        link_.cap = sizeof(cached_buf_);
        link_.storage = storage;
    }
 
    // Issue 2009 — a KEYED future: the request went out as `seq`, and the
    // future joins `list`, the pending list of the client it was sent on.
    Future(void* storage, ::rclcpp::detail::TakeSeqFn fn, int64_t seq,
           ::rclcpp::detail::PendingList* list, void* executor)
        : take_fn_(nullptr), take_seq_fn_(fn), pump_fn_(nullptr), executor_(executor), slot_(0) {
        link_.buf = cached_buf_;
        link_.cap = sizeof(cached_buf_);
        link_.storage = storage;
        link_.seq = seq;
        link_.list = list;
        if (list != nullptr) list->link(&link_);
    }
 
    // Issue 2011 — a GOAL-KEYED action future: it joins `list` (the action
    // client's acceptances or results) under `goal`. The client's callbacks
    // deliver into it (`PendingList::resolve_goal` + `deliver`); `pump` is the
    // client's own dispatch, for a reply no `spin_once` has taken yet.
    Future(void* storage, ::rclcpp::detail::PumpFn pump, const uint8_t goal[16],
           ::rclcpp::detail::PendingList* list, void* executor)
        : take_fn_(nullptr), take_seq_fn_(nullptr), pump_fn_(pump), executor_(executor), slot_(0) {
        link_.buf = cached_buf_;
        link_.cap = sizeof(cached_buf_);
        link_.storage = storage;
        link_.by_goal = true;
        for (int i = 0; i < 16; ++i)
            link_.goal_id[i] = goal[i];
        link_.list = list;
        if (list != nullptr) list->link(&link_);
    }
 
    bool live() const { return slot_ >= 0 && !link_.orphaned; }

    bool poll_keyed() {
        ::rclcpp::detail::PendingList* list = link_.list;
        if (list == nullptr) return false;
        for (int i = 0; i < ::rclcpp::detail::MAX_STALE_DRAIN_PER_POLL; ++i) {
            size_t len = 0;
            int64_t seq = ::rclcpp::detail::SEQUENCE_ID_UNKNOWN;
            nros_cpp_ret_t ret =
                take_seq_fn_(link_.storage, cached_buf_, sizeof(cached_buf_), &len, &seq);
            if (ret != 0) break; // TRY_AGAIN (nothing yet) or a transport error
            ::rclcpp::detail::PendingLink* owner = list->resolve(seq);
            if (owner != nullptr) list->deliver(owner, cached_buf_, len);
            if (link_.ready) return true;
        }
        return link_.ready;
    }

    bool poll_pumped() {
        if (link_.list == nullptr || link_.storage == nullptr) return link_.ready;
        (void)pump_fn_(link_.storage);
        return link_.ready;
    }

    void detach(bool abandoned) {
        if (link_.list != nullptr && !link_.ready) (void)link_.list->unlink(&link_, abandoned);
        link_.list = nullptr;
    }

    void adopt(Future& other) {
        link_.buf = cached_buf_;
        link_.cap = sizeof(cached_buf_);
        link_.storage = other.link_.storage;
        link_.seq = other.link_.seq;
        link_.by_goal = other.link_.by_goal;
        for (int i = 0; i < 16; ++i)
            link_.goal_id[i] = other.link_.goal_id[i];
        link_.ready = other.link_.ready;
        link_.orphaned = other.link_.orphaned;
        link_.len = other.link_.len;
        link_.list = other.link_.list;
        link_.next = nullptr;
        if (link_.ready) {
            for (size_t i = 0; i < link_.len; ++i)
                cached_buf_[i] = other.cached_buf_[i];
        }
        if (link_.list != nullptr && !link_.ready) link_.list->relink(&other.link_, &link_);
        other.link_.list = nullptr;
        other.link_.ready = false;
        other.link_.next = nullptr;
        other.slot_ = -1;
    }
 
    TryRecvFn take_fn_;
    ::rclcpp::detail::TakeSeqFn take_seq_fn_;
    ::rclcpp::detail::PumpFn pump_fn_;
    void* executor_;
    int slot_;
    ::rclcpp::detail::PendingLink link_;
    uint8_t cached_buf_[Cap];
};
 
} // namespace rclcpp
#endif // NROS_RCLCPP_FUTURE_HPP