client.hpp

Go to the documentation of this file.

// nros-cpp: Action client class
// Freestanding C++ — no exceptions, no STL required
 
#ifndef NROS_RCLCPP_ACTION_CLIENT_HPP
#define NROS_RCLCPP_ACTION_CLIENT_HPP
 
#include <cstdint>
#include <cstddef>
#include <string.h>
 
#include "rclcpp/detail/config.hpp"
#include "rclcpp/detail/entity_name.hpp" // phase-444 — the one entity-name copy
#include "rclcpp/logging.hpp" // phase-417 stage 3 — NROS_RCLCPP_REFUSE_* + rclcpp::detail::refuse
#include "rclcpp/result.hpp"
#include "rclcpp/size_bound.hpp" // rclcpp::rx_buffer_capacity<M> — the receive-buffer size
#include "rclcpp/future.hpp"
#include "rclcpp/stream.hpp"
// Issue 0796 — `CancelReturnCode` (the `action_msgs/srv/CancelGoal` RPC status
// `cancel_goal` resolves to) is declared beside its per-goal sibling
// `CancelResponse` in action_server.hpp, which is also where
// polling_action_server.hpp reaches for them.
#include "rclcpp_action/server.hpp"
 
// Phase 118.D: FFI declarations sourced from cbindgen-generated
// `nros_cpp_ffi.h`. cbindgen renders Rust `*const [u8; 16]` /
// `*mut [u8; 16]` parameters as C++ pointer-to-array
// (`const uint8_t (*goal_id)[16]`); member-function bodies below
// take `goal_id` as a 16-byte array (decays to `uint8_t*`) and
// `reinterpret_cast` at the call site to bridge the two ABI
// equivalent shapes.
#include "rclcpp/detail/nros_cpp_ffi.h"
 
extern "C" {
typedef void (*nros_cpp_action_client_goal_response_callback_t)(bool accepted,
                                                                const uint8_t goal_id[16],
                                                                void* ctx);
typedef void (*nros_cpp_action_client_feedback_callback_t)(const uint8_t goal_id[16],
                                                           const uint8_t* data, size_t len,
                                                           void* ctx);
typedef void (*nros_cpp_action_client_result_callback_t)(const uint8_t goal_id[16], int32_t status,
                                                         const uint8_t* data, size_t len,
                                                         void* ctx);
 
nros_cpp_ret_t nros_cpp_action_client_set_callbacks(
    void* handle, nros_cpp_action_client_goal_response_callback_t goal_response,
    nros_cpp_action_client_feedback_callback_t feedback,
    nros_cpp_action_client_result_callback_t result, void* context);
} // extern "C"
 
namespace rclcpp {
class Node;
}
 
// ============================================================================
// `rclcpp_action::Client<A>` -- DEFINED here (RFC-0089: rclcpp_action:: is the home)
// ============================================================================
//
// phase-428: the definition moved from `nros::ActionClient<A>` to
// `rclcpp_action::Client<A>` (the `nros::` alias went in phase-483 W1). A RENAME
// as well as a move -- upstream's namespace already says "action", so the type
// is `Client`, not `ActionClient`. Note the two `Client`s do not collide:
// `rclcpp::Client<S>` is the SERVICE client, in a different namespace.
namespace rclcpp_action {
 
template <typename A> class Client {
  public:
    using GoalType = typename A::Goal;
    using ResultType = typename A::Result;
    using FeedbackType = typename A::Feedback;
 
    struct GoalAccept {
        uint8_t goal_id[16];
        bool accepted;
 
        // Receive-buffer size for `Future<GoalAccept>`; the payload is 17 bytes
        // (16-byte goal_id + 1 accepted byte), rounded up.
        static const size_t SERIALIZED_SIZE_MAX = 32;
 
        // Issue 0329 — the 17-byte wire layout is owned Rust-side
        // (`nros_cpp_action_goal_accept_decode`, beside its producer); the header
        // no longer hand-decodes it, so the two cannot drift.
        static int ffi_deserialize(const uint8_t* data, size_t len, GoalAccept* out) {
            if (!out) return -1;
            return nros_cpp_action_goal_accept_decode(data, len, &out->goal_id, &out->accepted) ==
                           NROS_CPP_RET_OK
                       ? 0
                       : -1;
        }
    };
 
    ::rclcpp::Result wait_for_action_server(uint32_t timeout_ms) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        return ::rclcpp::Result(
            nros_cpp_action_client_wait_for_action_server(storage_, timeout_ms));
    }
 
    template <typename T = void>::rclcpp::Result wait_for_action_server() {
        static_assert(::rclcpp::detail::refuse<T>::value, NROS_RCLCPP_REFUSE_UNBOUNDED_WAIT);
        return ::rclcpp::Result(::rclcpp::ErrorCode::Unsupported);
    }
 
    bool action_server_is_ready() const {
        if (!initialized_) return false;
        return nros_cpp_action_client_action_server_is_ready(
            const_cast<void*>(static_cast<const void*>(storage_)));
    }
 
    const char* get_action_name() const { return initialized_ ? action_name_ : ""; }
 
    ::rclcpp::Result send_goal(const GoalType& goal, uint8_t goal_id[16]) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
 
        uint8_t buf[::rclcpp::detail::buffer_bounds<GoalType>::tx];
        size_t len = 0;
        if (GoalType::ffi_serialize(&goal, buf, sizeof(buf), &len) != 0) {
            return ::rclcpp::Result(::rclcpp::ErrorCode::Error);
        }
        return ::rclcpp::Result(nros_cpp_action_client_send_goal(
            storage_, buf, len, reinterpret_cast<uint8_t(*)[16]>(goal_id)));
    }
 
    ::rclcpp::Result send_goal(const GoalType& goal, ::rclcpp_action::GoalUUID& goal_id) {
        return send_goal(goal, goal_id.data());
    }
 
    ::rclcpp::Result get_result(const uint8_t goal_id[16], ResultType& result) {
        return get_result_sized<::rclcpp::rx_buffer_capacity<ResultType>::value>(goal_id, result);
    }
 
    ::rclcpp::Result get_result(const ::rclcpp_action::GoalUUID& goal_id, ResultType& result) {
        return get_result_sized<::rclcpp::rx_buffer_capacity<ResultType>::value>(goal_id.data(),
                                                                                 result);
    }
 
    template <size_t Cap>
    ::rclcpp::Result get_result_sized(const uint8_t goal_id[16], ResultType& result) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
 
        uint8_t buf[Cap];
        size_t len = 0;
        nros_cpp_ret_t ret = nros_cpp_action_client_get_result(
            storage_, executor_, reinterpret_cast<const uint8_t(*)[16]>(goal_id), buf, sizeof(buf),
            &len);
        if (ret != 0) return ::rclcpp::Result(ret);
 
        if (ResultType::ffi_deserialize(buf, len, &result) != 0) {
            return ::rclcpp::Result(::rclcpp::ErrorCode::Error);
        }
        return ::rclcpp::Result::success();
    }
 
    // =================================================================
    // Future-based API — non-blocking, polled via Future<T>
    // =================================================================
 
    ::rclcpp::Future<GoalAccept> send_goal_future(const GoalType& goal) {
        if (!initialized_) return ::rclcpp::Future<GoalAccept>();
 
        uint8_t buf[::rclcpp::detail::buffer_bounds<GoalType>::tx];
        size_t len = 0;
        if (GoalType::ffi_serialize(&goal, buf, sizeof(buf), &len) != 0) {
            return ::rclcpp::Future<GoalAccept>();
        }
 
        uint8_t goal_id[16];
        nros_cpp_ret_t ret = nros_cpp_action_client_send_goal_async(
            storage_, buf, len, reinterpret_cast<uint8_t(*)[16]>(goal_id));
        if (ret != 0) return ::rclcpp::Future<GoalAccept>();
 
        return ::rclcpp::Future<GoalAccept>(storage_,
                                            &nros_cpp_action_client_try_recv_goal_response,
                                            0 // slot 0 (single outstanding goal request)
        );
    }
 
    ::rclcpp::Future<ResultType> get_result_future(const uint8_t goal_id[16]) {
        return get_result_future_sized<::rclcpp::rx_buffer_capacity<ResultType>::value>(goal_id);
    }
 
    ::rclcpp::Future<ResultType> get_result_future(const ::rclcpp_action::GoalUUID& goal_id) {
        return get_result_future_sized<::rclcpp::rx_buffer_capacity<ResultType>::value>(
            goal_id.data());
    }
 
    template <size_t Cap>
    ::rclcpp::Future<ResultType, Cap> get_result_future_sized(const uint8_t goal_id[16]) {
        using Fut = ::rclcpp::Future<ResultType, Cap>;
        if (!initialized_) return Fut();
 
        nros_cpp_ret_t ret = nros_cpp_action_client_get_result_async(
            storage_, reinterpret_cast<const uint8_t(*)[16]>(goal_id));
        if (ret != 0) return Fut();
 
        return Fut(storage_, &nros_cpp_action_client_try_recv_result,
                   0 // slot 0 (single outstanding result request)
        );
    }
 
    ::rclcpp::Result try_recv_feedback(FeedbackType& feedback) {
        return try_recv_feedback_sized<::rclcpp::rx_buffer_capacity<FeedbackType>::value>(feedback);
    }
 
    template <size_t Cap>::rclcpp::Result try_recv_feedback_sized(FeedbackType& feedback) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
 
        uint8_t buf[Cap];
        size_t len = 0;
        nros_cpp_ret_t ret =
            nros_cpp_action_client_try_recv_feedback(storage_, buf, sizeof(buf), &len);
        if (ret != 0) return ::rclcpp::Result(ret);
        if (len == 0) return ::rclcpp::Result(::rclcpp::ErrorCode::TryAgain);
        if (FeedbackType::ffi_deserialize(buf, len, &feedback) != 0) {
            return ::rclcpp::Result(::rclcpp::ErrorCode::Error);
        }
        return ::rclcpp::Result::success();
    }
 
    ::rclcpp::Stream<FeedbackType>& feedback_stream() {
        if (initialized_ && !feedback_stream_.is_valid()) {
            feedback_stream_.bind(storage_, &nros_cpp_action_client_try_recv_feedback);
        }
        return feedback_stream_;
    }
 
    const ::rclcpp::Stream<FeedbackType>& feedback_stream() const { return feedback_stream_; }
 
    // =================================================================
    // Async (non-blocking) API — callbacks invoked during spin_once()
    // =================================================================
 
    struct SendGoalOptions {
        void (*goal_response)(bool accepted, const uint8_t goal_id[16], void* ctx);
        void (*feedback)(const uint8_t goal_id[16], const uint8_t* data, size_t len, void* ctx);
        void (*result)(const uint8_t goal_id[16], int32_t status, const uint8_t* data, size_t len,
                       void* ctx);
        void* context;
 
        SendGoalOptions() : goal_response(0), feedback(0), result(0), context(0) {}
    };
 
    ::rclcpp::Result send_goal_async(const GoalType& goal, uint8_t goal_id[16]) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
 
        uint8_t buf[::rclcpp::detail::buffer_bounds<GoalType>::tx];
        size_t len = 0;
        if (GoalType::ffi_serialize(&goal, buf, sizeof(buf), &len) != 0) {
            return ::rclcpp::Result(::rclcpp::ErrorCode::Error);
        }
        return ::rclcpp::Result(nros_cpp_action_client_send_goal_async(
            storage_, buf, len, reinterpret_cast<uint8_t(*)[16]>(goal_id)));
    }
 
    ::rclcpp::Result send_goal_async(const GoalType& goal, ::rclcpp_action::GoalUUID& goal_id) {
        return send_goal_async(goal, goal_id.data());
    }
 
    ::rclcpp::Result cancel_goal(const uint8_t goal_id[16]) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        return ::rclcpp::Result(nros_cpp_action_client_cancel_goal(
            storage_, reinterpret_cast<const uint8_t(*)[16]>(goal_id)));
    }
 
    ::rclcpp::Result cancel_goal(const ::rclcpp_action::GoalUUID& goal_id) {
        return cancel_goal(goal_id.data());
    }
 
    ::rclcpp::Result try_recv_cancel_response(::rclcpp_action::CancelReturnCode& out) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        int8_t code = 0;
        nros_cpp_ret_t ret = nros_cpp_action_client_try_recv_cancel_response(storage_, &code);
        if (ret != 0) return ::rclcpp::Result(ret);
        out = static_cast<::rclcpp_action::CancelReturnCode>(code);
        return ::rclcpp::Result::success();
    }
 
    ::rclcpp::Result get_result_async(const uint8_t goal_id[16]) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        return ::rclcpp::Result(nros_cpp_action_client_get_result_async(
            storage_, reinterpret_cast<const uint8_t(*)[16]>(goal_id)));
    }
 
    ::rclcpp::Result get_result_async(const ::rclcpp_action::GoalUUID& goal_id) {
        return get_result_async(goal_id.data());
    }
 
    ::rclcpp::Result set_callbacks(const SendGoalOptions& options) {
        if (!initialized_) return ::rclcpp::Result(::rclcpp::ErrorCode::NotInitialized);
        return ::rclcpp::Result(nros_cpp_action_client_set_callbacks(
            storage_, options.goal_response, options.feedback, options.result, options.context));
    }
 
    void poll() {
        if (!initialized_) return;
        nros_cpp_action_client_poll(storage_);
    }
 
    bool is_valid() const { return initialized_; }
 
    ~Client() {
        if (initialized_) {
            nros_cpp_action_client_destroy(storage_);
            initialized_ = false;
        }
        feedback_stream_ = ::rclcpp::Stream<FeedbackType>();
    }
 
    // Move semantics (non-copyable). Relocation goes through the
    // `nros_cpp_action_client_relocate` runtime call (Phase 84.C1), which also
    // RE-POINTS the arena entry's callback `context` at the new `storage_`
    // (issue 1496 — before that, a moved-from temporary left the entry naming
    // dead bytes). The feedback stream is rebound to the new storage afterwards.
    Client(Client&& other)
        : executor_(other.executor_), initialized_(other.initialized_), action_name_{} {
        ::memcpy(action_name_, other.action_name_, sizeof(action_name_));
        if (other.initialized_) {
            nros_cpp_action_client_relocate(other.storage_, storage_);
            other.initialized_ = false;
        }
        other.feedback_stream_ = ::rclcpp::Stream<FeedbackType>();
    }
 
    Client& operator=(Client&& other) {
        if (this != &other) {
            if (initialized_) {
                nros_cpp_action_client_destroy(storage_);
                feedback_stream_ = ::rclcpp::Stream<FeedbackType>();
            }
            executor_ = other.executor_;
            initialized_ = other.initialized_;
            ::memcpy(action_name_, other.action_name_, sizeof(action_name_));
            if (other.initialized_) {
                nros_cpp_action_client_relocate(other.storage_, storage_);
                other.initialized_ = false;
            }
            other.feedback_stream_ = ::rclcpp::Stream<FeedbackType>();
        }
        return *this;
    }
 
    Client() : executor_(nullptr), initialized_(false), action_name_{} {}
 
  private:
    Client(const Client&) = delete;
    Client& operator=(const Client&) = delete;
 
    friend class ::rclcpp::Node;
 
    alignas(8) uint8_t storage_[NROS_CPP_ACTION_CLIENT_STORAGE_SIZE];
    void* executor_; // Stashed executor handle (Phase 82) for blocking helpers
    bool initialized_;
    ::rclcpp::Stream<FeedbackType> feedback_stream_;
    // Phase 87.6 put a `char action_name_[256]` here for an accessor that was
    // never written, and an earlier phase-417 W4.b wave deleted it as dead
    // state. W4.b's second half writes `get_action_name()`, so it is back and
    // it is read — see the note on `Server<A>::get_action_name`.
    char action_name_[::rclcpp_action::ACTION_NAME_MAX];
};
 
} // namespace rclcpp_action
 
// Phase 84.G8: out-of-line definition of Node::create_action_client<A>().
#include "rclcpp/node.hpp"
 
namespace rclcpp {} // namespace rclcpp
 
namespace rclcpp {
template <typename A>
Result Node::create_action_client(::rclcpp_action::Client<A>& out, const char* action_name,
                                  const ::rclcpp::QoS& qos) {
    if (!initialized_) return Result(::rclcpp::ErrorCode::NotInitialized);
    nros_cpp_qos_t ffi_qos = ::rclcpp::detail::qos_to_ffi(qos);
    const nros_cpp_action_hashes_t hashes = ::rclcpp::detail::action_endpoint_hashes<A>();
    nros_cpp_ret_t ret = nros_cpp_action_client_create(
        &handle_, action_name, A::TYPE_NAME, A::Goal::TYPE_HASH, &hashes, ffi_qos, out.storage_);
    if (ret == 0) {
        out.executor_ = executor_handle_;
        // phase-417 W4.b — remember the name for `get_action_name()`, truncating
        // at `rclcpp_action::ACTION_NAME_MAX` exactly as the server and the polling tiers do.
        ::rclcpp::detail::assign_entity_name(out.action_name_, action_name);
        out.initialized_ = true;
    }
    return Result(ret);
}
} // namespace rclcpp
 
namespace rclcpp {} // namespace rclcpp
 
#endif // NROS_RCLCPP_ACTION_CLIENT_HPP