node.hpp

Go to the documentation of this file.

// nros-cpp: Node class
// Freestanding C++ — no exceptions, no STL required
 
#ifndef NROS_CPP_NODE_HPP
#define NROS_CPP_NODE_HPP
 
#include <cstdint>
#include <cstddef>
#include <type_traits> // Phase 189.M3.3.e — SFINAE on the callback-style create_service
#if defined(NROS_CPP_STD) || (__STDC_HOSTED__ + 0)
#include <cstdlib> // getenv — Phase 123.B.3 env-aware init
#if defined(NROS_CPP_STD) || (__STDC_HOSTED__ + 0)
#include <cstdio> // fopen — Phase 212.L.5 init_with_launch path-exists check
#endif
#endif
 
// Phase 118.D: ffi.h MUST come before qos.hpp so qos.hpp's
// `#ifndef NROS_CPP_FFI_H` guard sees the canonical types and skips
// its local redefinitions.
#include "nros_cpp_ffi.h"
 
#include "nros/result.hpp"
#include "nros/nros_cpp_config_generated.h"
#include "nros/qos.hpp"
// Phase 189.M3.1 — rclcpp-style named-options structs
// (`SubscriptionOptions` / `PublisherOptions`) used by the 4-arg
// `create_subscription` / `create_publisher` overloads below.
#include "nros/options.hpp"
// Phase 84.G8: heavy entity headers (publisher / subscription / service /
// client / action_server / action_client) are no longer pulled in here.
// Each entity header provides the out-of-line definition of its
// corresponding `Node::create_X<T>()` template and includes `node.hpp`
// itself. Consumers that #include `nros/nros.hpp` (the umbrella) still
// get every entity + every create method via that path; consumers that
// only want lightweight Node access can include this header directly
// and pay for only the light entities (timer, guard_condition,
// executor) below.
#include "nros/timer.hpp"
#include "nros/guard_condition.hpp"
#include "nros/executor.hpp"
 
#ifdef NROS_RMW_CYCLONEDDS
extern "C" int32_t nros_rmw_cyclonedds_register(void);
#endif
#if defined(NROS_RMW_XRCE) || defined(NROS_RMW_XRCE_CFFI)
extern "C" int32_t nros_rmw_xrce_register(void);
#endif
#ifdef NROS_RMW_ZENOH_CFFI
extern "C" int32_t nros_rmw_zenoh_register(void);
#endif
#ifdef NROS_RMW_UORB
extern "C" int32_t nros_rmw_uorb_register(void);
#endif
 
namespace nros {
 
// Phase 84.G8: forward declarations of the heavy entity class
// templates. Full definitions live in the corresponding `*.hpp`,
// which also provide the out-of-line `Node::create_X<>` template
// bodies — consumers only pay for the entities they #include.
template <typename M> class Publisher;
template <typename M> class Subscription;
template <typename S> class Service;
template <typename S> class Client;
template <typename A> class ActionServer;
template <typename A> class ActionClient;
// Phase 122.3.d.b — L1 polling-mode action wrappers.
template <typename A> class PollingActionServer;
template <typename A> class PollingActionClient;
// Phase 242.1 (RFC-0044) — rclcpp-faithful IS-A-node base. It wraps an owned
// `Node` and creates that node against an executor-bound handle in its ctor, so
// it needs friend access to set `executor_handle_` + call `Node::create`
// (the same private-create pattern `Executor` / `NodeBuilder` already use).
class ComponentNode;
 
inline Result init(const char* locator = nullptr, uint8_t domain_id = 0);
 
inline Result init(const char* locator, uint8_t domain_id, const char* session_name);
 
inline Result init_with_launch_auto(int argc = 0, char** argv = nullptr,
                                    const char* session_name = nullptr);
 
inline Result init_with_launch(const char* path, int argc = 0, char** argv = nullptr,
                               const char* session_name = nullptr);
 
inline Result shutdown();
 
class Node {
  public:
    Node() : handle_(), initialized_(false), executor_handle_(nullptr) {}
 
    static Result create(Node& out, const char* name, const char* ns = nullptr) {
        if (!out.executor_handle_) {
            return Result(ErrorCode::NotInitialized);
        }
 
        nros_cpp_ret_t ret = nros_cpp_node_create(out.executor_handle_, name, ns, &out.handle_);
 
        if (ret == 0) {
            out.initialized_ = true;
        }
        return Result(ret);
    }
 
    const char* get_name() const {
        if (!initialized_) return "";
        return nros_cpp_node_get_name(&handle_);
    }
 
    const char* get_namespace() const {
        if (!initialized_) return "";
        return nros_cpp_node_get_namespace(&handle_);
    }
 
    const void* get_logger() const {
        if (!initialized_) return nullptr;
        return nros_cpp_node_get_logger(&handle_);
    }
 
    bool is_valid() const { return initialized_; }
 
    const nros_cpp_node_t* ffi_handle() const { return initialized_ ? &handle_ : nullptr; }
 
    void set_qos_overrides(const nros_cpp_qos_override_t* overrides, size_t len) {
        if (initialized_) {
            ::nros_cpp_node_set_qos_overrides(&handle_, overrides, len);
        }
    }
 
    void* executor_handle() const { return initialized_ ? executor_handle_ : nullptr; }
 
    template <typename M>
    Result create_publisher(Publisher<M>& out, const char* topic,
                            const QoS& qos = QoS::default_profile());
 
    template <typename M>
    Result create_publisher(Publisher<M>& out, const char* topic, const QoS& qos,
                            const PublisherOptions& options);
 
    template <typename M>
    Result create_subscription(Subscription<M>& out, const char* topic,
                               const QoS& qos = QoS::default_profile());
 
    template <typename M>
    Result create_subscription(Subscription<M>& out, const char* topic, const QoS& qos,
                               const SubscriptionOptions& options);
 
    template <
        typename M, typename F,
        typename = typename std::enable_if<std::is_convertible<F, void (*)(const M&)>::value>::type>
    Result create_subscription(Subscription<M>& out, const char* topic, F callback,
                               const QoS& qos = QoS::default_profile(),
                               const SubscriptionOptions& options = {});
 
    template <typename M, typename F,
              typename = typename std::enable_if<
                  std::is_convertible<F, void (*)(const M&, const uint8_t*, size_t)>::value>::type>
    Result create_subscription_with_info(Subscription<M>& out, const char* topic, F callback,
                                         const QoS& qos = QoS::default_profile(),
                                         const SubscriptionOptions& options = {});
 
    template <typename S>
    Result create_service(Service<S>& out, const char* service_name,
                          const QoS& qos = QoS::services());
 
    template <typename S, typename F,
              typename = typename std::enable_if<std::is_convertible<
                  F, void (*)(const typename S::Request&, typename S::Response&)>::value>::type>
    Result create_service(Service<S>& out, const char* service_name, F callback,
                          const QoS& qos = QoS::services(), const ServiceOptions& options = {});
 
    template <typename S>
    Result create_client(Client<S>& out, const char* service_name,
                         const QoS& qos = QoS::services());
 
    template <typename S, typename F,
              typename = typename std::enable_if<
                  std::is_convertible<F, void (*)(const typename S::Response&)>::value>::type>
    Result create_client(Client<S>& out, const char* service_name, F callback,
                         const QoS& qos = QoS::services(), const ClientOptions& options = {});
 
    template <typename A>
    Result create_action_server(ActionServer<A>& out, const char* action_name,
                                const QoS& qos = QoS::services(),
                                const ActionServerOptions& options = {});
 
    template <typename A>
    Result create_action_client(ActionClient<A>& out, const char* action_name,
                                const QoS& qos = QoS::services());
 
    template <typename A>
    Result create_polling_action_server(PollingActionServer<A>& out, const char* action_name);
 
    template <typename A>
    Result create_polling_action_client(PollingActionClient<A>& out, const char* action_name);
 
    Result create_timer(Timer& out, uint64_t period_ms, nros_cpp_timer_callback_t callback,
                        void* context = nullptr) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        size_t handle_id = 0;
        nros_cpp_ret_t ret =
            nros_cpp_timer_create(executor_handle_, period_ms, callback, context, &handle_id);
        if (ret == 0) {
            out.executor_ = executor_handle_;
            out.handle_id_ = handle_id;
            out.initialized_ = true;
        }
        return Result(ret);
    }
 
    Result create_timer_oneshot(Timer& out, uint64_t delay_ms, nros_cpp_timer_callback_t callback,
                                void* context = nullptr) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        size_t handle_id = 0;
        nros_cpp_ret_t ret = nros_cpp_timer_create_oneshot(executor_handle_, delay_ms, callback,
                                                           context, &handle_id);
        if (ret == 0) {
            out.executor_ = executor_handle_;
            out.handle_id_ = handle_id;
            out.initialized_ = true;
        }
        return Result(ret);
    }
 
    Result create_guard_condition(GuardCondition& out, nros_cpp_guard_callback_t callback,
                                  void* context = nullptr) {
        if (!initialized_) return Result(ErrorCode::NotInitialized);
        nros_cpp_ret_t ret =
            nros_cpp_guard_condition_create(executor_handle_, callback, context, out.storage_);
        if (ret == 0) {
            out.initialized_ = true;
        }
        return Result(ret);
    }
 
    ~Node() {
        if (initialized_) {
            nros_cpp_node_destroy(&handle_);
            initialized_ = false;
        }
    }
 
    // Move semantics (non-copyable)
    Node(Node&& other)
        : handle_(other.handle_), initialized_(other.initialized_),
          executor_handle_(other.executor_handle_) {
        other.initialized_ = false;
        other.executor_handle_ = nullptr;
    }
 
    Node& operator=(Node&& other) {
        if (this != &other) {
            if (initialized_) {
                nros_cpp_node_destroy(&handle_);
            }
            handle_ = other.handle_;
            initialized_ = other.initialized_;
            executor_handle_ = other.executor_handle_;
            other.initialized_ = false;
            other.executor_handle_ = nullptr;
        }
        return *this;
    }
 
  private:
    Node(const Node&) = delete;
    Node& operator=(const Node&) = delete;
 
    nros_cpp_node_t handle_;
    bool initialized_;
    void* executor_handle_; // Set by nros::init() via friendship
 
    friend class Executor;
    friend class NodeBuilder;
    friend class ComponentNode; // Phase 242.1 — ctor-creates the owned node
    friend Result init(const char* locator, uint8_t domain_id);
    friend Result init(const char* locator, uint8_t domain_id, const char* session_name);
    friend Result shutdown();
    friend bool ok();
    friend Result create_node(Node& out, const char* name, const char* ns);
    friend Result spin_once(int32_t timeout_ms);
    friend Result spin();
    friend Result spin(uint32_t duration_ms, int32_t poll_ms);
    friend void* global_handle();
 
    // Global executor inline storage for init/shutdown free functions.
    //
    // Use a template-static-member trick instead of a function-local static.
    // Function-local statics need __cxa_guard_acquire/release on first-call
    // initialisation; on NuttX the resulting guard logic returns NULL for
    // the storage pointer (observed empirically with LTO on armv7a-nuttx-eabihf,
    // even with constant-initialisation `= {}`). A template static member is
    // emitted into .bss like a file-scope variable and gets COMDAT-folded by
    // the linker, sidestepping the guarded-init path entirely.
    template <int = 0> struct GlobalStorageHolder {
        alignas(8) static uint8_t storage[NROS_CPP_EXECUTOR_STORAGE_SIZE];
        static bool initialized;
    };
    static uint8_t* global_storage() { return GlobalStorageHolder<>::storage; }
    static bool& global_initialized() { return GlobalStorageHolder<>::initialized; }
};
 
// Out-of-class definitions for Node::GlobalStorageHolder<> — the template
// machinery means these get emitted as COMDAT symbols, so multiple TUs
// including this header all collapse to a single .bss allocation.
template <int N>
alignas(8) uint8_t Node::GlobalStorageHolder<N>::storage[NROS_CPP_EXECUTOR_STORAGE_SIZE] = {};
template <int N> bool Node::GlobalStorageHolder<N>::initialized = false;
 
// -- Free function implementations --
 
inline Result init(const char* locator, uint8_t domain_id) {
#if defined(NROS_CPP_STD) || (__STDC_HOSTED__ + 0)
    // Phase 123.B.3 — on hosted builds, fall through to env vars
    // ($NROS_LOCATOR / $ROS_DOMAIN_ID) so the no-arg `nros::init()`
    // call works without `getenv()` boilerplate in user code.
    // Explicit non-null `locator` / non-zero `domain_id` still win.
    if (locator == nullptr) {
        const char* env_loc = ::std::getenv("NROS_LOCATOR");
        if (env_loc != nullptr && env_loc[0] != '\0') {
            locator = env_loc;
        } else {
            locator = "tcp/127.0.0.1:7447";
        }
    }
    if (domain_id == 0) {
        const char* env_dom = ::std::getenv("ROS_DOMAIN_ID");
        if (env_dom != nullptr && env_dom[0] != '\0') {
            // Parse decimal digits inline — no <cstdlib> dep.
            unsigned acc = 0;
            for (const char* p = env_dom; *p; ++p) {
                if (*p < '0' || *p > '9') {
                    acc = 0;
                    break;
                }
                acc = acc * 10 + static_cast<unsigned>(*p - '0');
                if (acc > 232) {
                    acc = 0;
                    break;
                }
            }
            domain_id = static_cast<uint8_t>(acc);
        }
    }
#endif
    return init(locator, domain_id, "nros_cpp");
}
 
inline Result init(const char* locator, uint8_t domain_id, const char* session_name) {
    // NROS_CPP_RET_INVALID_ARGUMENT = -3 (defined in nros_cpp_ffi.h
    // which isn't included from this header — duplicate the value
    // inline; generated header is the source of truth).
    if (session_name == nullptr) {
        return Result(-3);
    }
#if defined(NROS_CPP_STD) || (__STDC_HOSTED__ + 0)
    // Issue #39 — apply the same `$NROS_LOCATOR` / `$ROS_DOMAIN_ID` env
    // fallback as the 2-arg `init()` when `locator` is null / `domain_id` is
    // 0. This makes `init_with_launch_auto()` (which delegates here with a
    // null locator) honor the env overlay instead of passing a null locator
    // to the backend → TransportError / degraded session.
    if (locator == nullptr) {
        const char* env_loc = ::std::getenv("NROS_LOCATOR");
        if (env_loc != nullptr && env_loc[0] != '\0') {
            locator = env_loc;
        } else {
            locator = "tcp/127.0.0.1:7447";
        }
    }
    if (domain_id == 0) {
        const char* env_dom = ::std::getenv("ROS_DOMAIN_ID");
        if (env_dom != nullptr && env_dom[0] != '\0') {
            unsigned acc = 0;
            for (const char* p = env_dom; *p; ++p) {
                if (*p < '0' || *p > '9') {
                    acc = 0;
                    break;
                }
                acc = acc * 10 + static_cast<unsigned>(*p - '0');
                if (acc > 232) {
                    acc = 0;
                    break;
                }
            }
            domain_id = static_cast<uint8_t>(acc);
        }
    }
#endif
    // Phase 128.C.1 — RMW-blind init. Every backend (cyclonedds,
    // xrce, dds, zenoh, uorb, …) contributes its registration entry
    // to the `RMW_INIT_ENTRIES` linker section via the
    // `NROS_RMW_REGISTER_BACKEND` macro (C/C++) or
    // `#[linkme::distributed_slice]` (Rust). The cffi runtime's
    // section walker fires inside `nros_cpp_init` and dispatches to
    // whichever backends were linked into this binary. No
    // `#ifdef NROS_RMW_*` chain here, no CMake-driven fan-out — the
    // user's `target_link_libraries(... NanoRos::Rmw::<name>)` is
    // the only selector.
    nros_cpp_ret_t ret =
        nros_cpp_init(locator, domain_id, session_name, nullptr, Node::global_storage());
    if (ret == 0) {
        Node::global_initialized() = true;
    }
    return Result(ret);
}
 
inline Result shutdown() {
    if (!Node::global_initialized()) {
        return Result::success();
    }
    nros_cpp_ret_t ret = nros_cpp_fini(Node::global_storage());
    Node::global_initialized() = false;
    return Result(ret);
}
 
// -- Phase 212.L.5 launch-aware init --
//
// Both `init_with_launch_auto` and `init_with_launch(path)` delegate to
// the existing 3-arg `init` after resolving the launch overlay (today:
// env vars only — see header docs for the follow-up plan). The session
// name falls back to `"nros_cpp"` so existing callsites keep working.
 
inline Result init_with_launch_auto(int argc, char** argv, const char* session_name) {
    (void)argc;
    (void)argv;
    // TODO (Phase 212.L.5 follow-up):
    //   1. If $NROS_RUNTIME_OVERLAY is set, read the JSON sidecar and
    //      fold its params/remaps/env into the init call.
    //   2. Else walk <CARGO_MANIFEST_DIR>/launch/* and parse the XML
    //      in-process.
    // For now the env overlay (NROS_LOCATOR / ROS_DOMAIN_ID consumed by
    // the 2-arg `init`) is the only channel.
    const char* name = (session_name != nullptr) ? session_name : "nros_cpp";
    return init(nullptr, 0, name);
}
 
inline Result init_with_launch(const char* path, int argc, char** argv, const char* session_name) {
    (void)argc;
    (void)argv;
    // NROS_CPP_RET_INVALID_ARGUMENT = -3 (mirrors the 3-arg init guard).
    if (path == nullptr) {
        return Result(-3);
    }
#if defined(NROS_CPP_STD) || (__STDC_HOSTED__ + 0)
    // Verify the file exists so misspelled paths fail fast at init time
    // instead of surfacing as a silently-empty overlay later.
    if (FILE* f = std::fopen(path, "rb")) {
        std::fclose(f);
    } else {
        return Result(ErrorCode::NotInitialized);
    }
#endif
    // TODO (Phase 212.L.5 follow-up): parse `path` as launch XML and
    // fold params/remaps/env into the init call. Today the env overlay
    // is the only channel.
    const char* name = (session_name != nullptr) ? session_name : "nros_cpp";
    return init(nullptr, 0, name);
}
 
inline bool ok() {
    return Node::global_initialized();
}
 
inline Result create_node(Node& out, const char* name, const char* ns = nullptr) {
    if (!Node::global_initialized()) {
        return Result(ErrorCode::NotInitialized);
    }
    out.executor_handle_ = Node::global_storage();
    return Node::create(out, name, ns);
}
 
inline Expected<Node> make_node(const char* name, const char* ns = nullptr) {
    Node n;
    Result r = create_node(n, name, ns);
    if (!r.ok()) return Expected<Node>::error(r);
    return Expected<Node>::ok(::std::move(n));
}
 
// -- Executor::create_node implementation (requires full Node definition) --
 
inline Result Executor::create_node(Node& out, const char* name, const char* ns) {
    if (!initialized_) return Result(ErrorCode::NotInitialized);
    out.executor_handle_ = storage_;
    return Node::create(out, name, ns);
}
 
// -- Phase 104.C.9 — NodeBuilder ----------------------------------------
//
// Mirrors Rust's `Executor::node_builder(name).rmw(...).locator(...).
// domain_id(...).namespace(...).sched(...).build()` chain. The C++
// wrapper is value-typed and stack-allocated; it accumulates options
// into an inline `nros_cpp_node_options_t` and ships it to
// `nros_cpp_node_create_ex` on `.build()`.
//
// Usage:
// ```cpp
// nros::Node node;
// NROS_TRY(executor.node_builder("egress")
//              .rmw("cyclonedds")
//              .domain_id(0)
//              .build(node));
// ```
 
class NodeBuilder {
  public:
    NodeBuilder(void* executor_handle, const char* name)
        : executor_handle_(executor_handle), name_(name),
          options_(nros_cpp_node_get_default_options()) {}
 
    NodeBuilder& rmw(const char* name) {
        copy_bounded(name, options_.rmw_name, &options_.rmw_name_len, NROS_CPP_RMW_NAME_LEN);
        return *this;
    }
 
    NodeBuilder& locator(const char* loc) {
        copy_bounded(loc, options_.locator, &options_.locator_len, NROS_CPP_LOCATOR_LEN);
        return *this;
    }
 
    NodeBuilder& domain_id(uint32_t id) {
        options_.domain_id_override = id;
        return *this;
    }
 
    NodeBuilder& namespace_(const char* ns) {
        copy_bounded(ns, options_.namespace_, &options_.namespace_len, NROS_CPP_NAMESPACE_LEN);
        return *this;
    }
 
    NodeBuilder& sched(uint8_t sc_id) {
        options_.sched_context_id = sc_id;
        return *this;
    }
 
    Result build(Node& out) const {
        if (!executor_handle_) return Result(ErrorCode::NotInitialized);
        out.executor_handle_ = executor_handle_;
        nros_cpp_ret_t ret =
            nros_cpp_node_create_ex(executor_handle_, name_, &options_, &out.handle_);
        if (ret == 0) {
            out.initialized_ = true;
        }
        return Result(ret);
    }
 
  private:
    static void copy_bounded(const char* src, uint8_t* dst, size_t* dst_len, size_t cap) {
        size_t n = 0;
        if (src != nullptr) {
            while (src[n] != '\0' && n < cap) {
                dst[n] = static_cast<uint8_t>(src[n]);
                ++n;
            }
        }
        // Zero out the tail so stale bytes don't leak across reuses.
        for (size_t i = n; i < cap; ++i) {
            dst[i] = 0;
        }
        *dst_len = n;
    }
 
    void* executor_handle_;
    const char* name_;
    nros_cpp_node_options_t options_;
};
 
inline NodeBuilder Executor::node_builder(const char* name) {
    return NodeBuilder(initialized_ ? handle() : nullptr, name);
}
 
} // namespace nros
 
#endif // NROS_CPP_NODE_HPP