diff --git a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/message_filters.hpp b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/message_filters.hpp index 28e6d4b75..ccff689be 100644 --- a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/message_filters.hpp +++ b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/message_filters.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_ros/agnocast_wrapper/autoware_agnocast_wrapper.hpp" +#include "nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp" #include "nebula_core_ros/agnocast_wrapper/node.hpp" #include diff --git a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp index 81f906c80..89e2a3d08 100644 --- a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp +++ b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp @@ -14,43 +14,109 @@ #pragma once +#include +#include + +#include +#include + +#include +#include +#include #include +#include #include +#include #include #ifdef USE_AGNOCAST_ENABLED #include -#include #include -#include #define NEBULA_MESSAGE_UNIQUE_PTR(MessageT) \ nebula::agnocast_wrapper::message_ptr +// For publisher (mutable message) #define NEBULA_MESSAGE_SHARED_PTR(MessageT) \ nebula::agnocast_wrapper::message_ptr +// For subscription (read-only message) +#define NEBULA_MESSAGE_CONST_SHARED_PTR(MessageT) \ + nebula::agnocast_wrapper::message_ptr< \ + const MessageT, nebula::agnocast_wrapper::OwnershipType::Shared> +#define NEBULA_SERVER_REQUEST_PTR(ServiceT) \ + nebula::agnocast_wrapper::message_ptr< \ + const typename ServiceT::Request, nebula::agnocast_wrapper::OwnershipType::Shared> +#define NEBULA_SERVER_RESPONSE_PTR(ServiceT) \ + nebula::agnocast_wrapper::message_ptr< \ + typename ServiceT::Response, nebula::agnocast_wrapper::OwnershipType::Shared> +#define NEBULA_CLIENT_REQUEST_PTR(ServiceT) \ + nebula::agnocast_wrapper::message_ptr< \ + typename ServiceT::Request, nebula::agnocast_wrapper::OwnershipType::Shared> +#define NEBULA_CLIENT_RESPONSE_PTR(ServiceT) \ + nebula::agnocast_wrapper::message_ptr< \ + const typename ServiceT::Response, nebula::agnocast_wrapper::OwnershipType::Shared> #define NEBULA_SUBSCRIPTION_PTR(MessageT) \ typename nebula::agnocast_wrapper::Subscription::SharedPtr #define NEBULA_PUBLISHER_PTR(MessageT) \ typename nebula::agnocast_wrapper::Publisher::SharedPtr - -#define NEBULA_CREATE_SUBSCRIPTION(message_type, node_ptr, topic, qos, callback, options) \ - nebula::agnocast_wrapper::create_subscription( \ - node_ptr, topic, qos, callback, options) -#define NEBULA_CREATE_PUBLISHER2(message_type, node_ptr, arg1, arg2) \ - nebula::agnocast_wrapper::create_publisher(node_ptr, arg1, arg2) -#define NEBULA_CREATE_PUBLISHER3(message_type, node_ptr, arg1, arg2, arg3) \ - nebula::agnocast_wrapper::create_publisher(node_ptr, arg1, arg2, arg3) +#define NEBULA_CLIENT_PTR(ServiceT) typename nebula::agnocast_wrapper::Client::SharedPtr +#define NEBULA_SERVICE_PTR(ServiceT) typename nebula::agnocast_wrapper::Service::SharedPtr +#define NEBULA_CLIENT_FUTURE(ServiceT) typename nebula::agnocast_wrapper::Client::Future +#define NEBULA_CLIENT_SHARED_FUTURE(ServiceT) \ + typename nebula::agnocast_wrapper::Client::SharedFuture +#define NEBULA_CLIENT_FUTURE_AND_REQUEST_ID(ServiceT) \ + typename nebula::agnocast_wrapper::Client::FutureAndRequestId +#define NEBULA_CLIENT_SHARED_FUTURE_AND_REQUEST_ID(ServiceT) \ + typename nebula::agnocast_wrapper::Client::SharedFutureAndRequestId +#define NEBULA_TIMER_PTR nebula::agnocast_wrapper::Timer::SharedPtr + +#define NEBULA_CREATE_SUBSCRIPTION(message_type, topic, qos, callback, options) \ + nebula::agnocast_wrapper::create_subscription(this, topic, qos, callback, options) +#define NEBULA_CREATE_SUBSCRIPTION_ON_NODE(message_type, node, topic, qos, callback, options) \ + nebula::agnocast_wrapper::create_subscription(node, topic, qos, callback, options) + +#define NEBULA_CREATE_PUBLISHER2(message_type, arg1, arg2) \ + nebula::agnocast_wrapper::create_publisher(this, arg1, arg2) +#define NEBULA_CREATE_PUBLISHER3(message_type, arg1, arg2, arg3) \ + nebula::agnocast_wrapper::create_publisher(this, arg1, arg2, arg3) +#define NEBULA_CREATE_PUBLISHER2_ON_NODE(message_type, node, arg1, arg2) \ + nebula::agnocast_wrapper::create_publisher(node, arg1, arg2) +#define NEBULA_CREATE_PUBLISHER3_ON_NODE(message_type, node, arg1, arg2, arg3) \ + nebula::agnocast_wrapper::create_publisher(node, arg1, arg2, arg3) + +#define NEBULA_CREATE_CLIENT1(service_type, service_name) \ + nebula::agnocast_wrapper::create_client(this, service_name) +#define NEBULA_CREATE_CLIENT2(service_type, service_name, qos) \ + nebula::agnocast_wrapper::create_client(this, service_name, qos) +#define NEBULA_CREATE_CLIENT3(service_type, service_name, qos, group) \ + nebula::agnocast_wrapper::create_client(this, service_name, qos, group) +#define NEBULA_CREATE_CLIENT1_ON_NODE(service_type, node, service_name) \ + nebula::agnocast_wrapper::create_client(node, service_name) +#define NEBULA_CREATE_CLIENT2_ON_NODE(service_type, node, service_name, qos) \ + nebula::agnocast_wrapper::create_client(node, service_name, qos) +#define NEBULA_CREATE_CLIENT3_ON_NODE(service_type, node, service_name, qos, group) \ + nebula::agnocast_wrapper::create_client(node, service_name, qos, group) + +#define NEBULA_CREATE_SERVICE2(service_type, service_name, callback) \ + nebula::agnocast_wrapper::create_service(this, service_name, callback) +#define NEBULA_CREATE_SERVICE3(service_type, service_name, callback, qos) \ + nebula::agnocast_wrapper::create_service(this, service_name, callback, qos) +#define NEBULA_CREATE_SERVICE4(service_type, service_name, callback, qos, group) \ + nebula::agnocast_wrapper::create_service(this, service_name, callback, qos, group) +#define NEBULA_CREATE_SERVICE2_ON_NODE(service_type, node, service_name, callback) \ + nebula::agnocast_wrapper::create_service(node, service_name, callback) +#define NEBULA_CREATE_SERVICE3_ON_NODE(service_type, node, service_name, callback, qos) \ + nebula::agnocast_wrapper::create_service(node, service_name, callback, qos) +#define NEBULA_CREATE_SERVICE4_ON_NODE(service_type, node, service_name, callback, qos, group) \ + nebula::agnocast_wrapper::create_service(node, service_name, callback, qos, group) #define NEBULA_SUBSCRIPTION_OPTIONS agnocast::SubscriptionOptions #define NEBULA_PUBLISHER_OPTIONS agnocast::PublisherOptions -#define NEBULA_HAS_ANY_SUBSCRIPTIONS(publisher) \ - (publisher->get_subscription_count() > 0 || publisher->get_intra_subscription_count() > 0) - #define ALLOCATE_OUTPUT_MESSAGE_UNIQUE(publisher) publisher->allocate_output_message_unique() #define ALLOCATE_OUTPUT_MESSAGE_SHARED(publisher) publisher->allocate_output_message_shared() +#define ALLOCATE_OUTPUT_SERVICE_REQUEST(client) client->allocate_output_service_request() namespace nebula::agnocast_wrapper { @@ -92,14 +158,42 @@ class message_interface virtual agnocast::ipc_shared_ptr move_agnocast_ptr() && noexcept = 0; virtual std::shared_ptr move_ros2_ptr() && noexcept = 0; + + virtual std::unique_ptr> clone() const = 0; }; template -class agnocast_message : public message_interface +class agnocast_message; + +template +class agnocast_message +: public message_interface { - using ros2_ptr_t = std::conditional_t< - Ownership == OwnershipType::Unique, std::unique_ptr, std::shared_ptr>; + agnocast::ipc_shared_ptr ptr_; + +public: + explicit agnocast_message(agnocast::ipc_shared_ptr && ptr) : ptr_(std::move(ptr)) {} + + MessageT & as_ref() const noexcept override { return *ptr_; } + MessageT * as_ptr() const noexcept override { return ptr_.get(); } + + agnocast::ipc_shared_ptr move_agnocast_ptr() && noexcept override + { + return std::move(ptr_); + } + // The following member function should never be called at runtime. They are implemented just for + // inheriting `message_interface`. + std::unique_ptr move_ros2_ptr() && noexcept override + { + return std::unique_ptr{}; + } +}; + +template +class agnocast_message +: public message_interface +{ agnocast::ipc_shared_ptr ptr_; public: @@ -115,24 +209,55 @@ class agnocast_message : public message_interface // The following member function should never be called at runtime. They are implemented just for // inheriting `message_interface`. - ros2_ptr_t move_ros2_ptr() && noexcept override { return ros2_ptr_t{}; } + std::shared_ptr move_ros2_ptr() && noexcept override + { + return std::shared_ptr{}; + } + + std::unique_ptr> clone() const override + { + return std::make_unique>(*this); + } }; template -class ros2_message : public message_interface +class ros2_message; + +template +class ros2_message +: public message_interface { - using ros2_ptr_t = std::conditional_t< - Ownership == OwnershipType::Unique, std::unique_ptr, std::shared_ptr>; + std::unique_ptr ptr_; - ros2_ptr_t ptr_; +public: + explicit ros2_message(std::unique_ptr && ptr) : ptr_(std::move(ptr)) {} + + MessageT & as_ref() const noexcept override { return *ptr_; } + MessageT * as_ptr() const noexcept override { return ptr_.get(); } + + std::unique_ptr move_ros2_ptr() && noexcept override { return std::move(ptr_); } + + // The following member function should never be called at runtime. They are implemented just for + // inheriting `message_interface`. + agnocast::ipc_shared_ptr move_agnocast_ptr() && noexcept override + { + return agnocast::ipc_shared_ptr{}; + } +}; + +template +class ros2_message +: public message_interface +{ + std::shared_ptr ptr_; public: - explicit ros2_message(ros2_ptr_t && ptr) : ptr_(std::move(ptr)) {} + explicit ros2_message(std::shared_ptr && ptr) : ptr_(std::move(ptr)) {} MessageT & as_ref() const noexcept override { return *ptr_; } MessageT * as_ptr() const noexcept override { return ptr_.get(); } - ros2_ptr_t move_ros2_ptr() && noexcept override { return std::move(ptr_); } + std::shared_ptr move_ros2_ptr() && noexcept override { return std::move(ptr_); } // The following member function should never be called at runtime. They are implemented just for // inheriting `message_interface`. @@ -140,15 +265,22 @@ class ros2_message : public message_interface { return agnocast::ipc_shared_ptr{}; } + + std::unique_ptr> clone() const override + { + return std::make_unique>(*this); + } }; template -class message_ptr +class message_ptr; + +template +class message_ptr { - using ros2_ptr_t = std::conditional_t< - Ownership == OwnershipType::Unique, std::unique_ptr, std::shared_ptr>; + using ros2_ptr_t = std::unique_ptr; - std::shared_ptr> ptr_; + std::unique_ptr> ptr_; template friend class AgnocastPublisher; @@ -167,22 +299,95 @@ class message_ptr message_ptr() : ptr_(nullptr) {} explicit message_ptr(agnocast::ipc_shared_ptr && ptr) - : ptr_(std::make_unique>(std::move(ptr))) + : ptr_(std::make_unique>(std::move(ptr))) { } explicit message_ptr(ros2_ptr_t && ptr) - : ptr_(std::make_unique>(std::move(ptr))) + : ptr_(std::make_unique>(std::move(ptr))) { } + message_ptr(const message_ptr & r) = delete; + message_ptr & operator=(const message_ptr & r) = delete; + + message_ptr(message_ptr && r) noexcept = default; + message_ptr & operator=(message_ptr && r) noexcept = default; + MessageT & operator*() const noexcept { return ptr_->as_ref(); } MessageT * operator->() const noexcept { return ptr_->as_ptr(); } - explicit operator bool() const noexcept { return static_cast(ptr_->as_ptr()); } + explicit operator bool() const noexcept { return ptr_ && static_cast(ptr_->as_ptr()); } - MessageT * get() const noexcept { return ptr_->as_ptr(); } + MessageT * get() const noexcept { return ptr_ ? ptr_->as_ptr() : nullptr; } +}; + +template +class message_ptr +{ + using ros2_ptr_t = std::shared_ptr; + + std::unique_ptr> ptr_; + + template + friend class AgnocastPublisher; + template + friend class ROS2Publisher; + template + friend class ROS2Client; + template + friend class AgnocastClient; + +private: + agnocast::ipc_shared_ptr move_agnocast_ptr() && noexcept + { + return std::move(*(std::move(ptr_))).move_agnocast_ptr(); + } + + auto move_ros2_ptr() && noexcept { return std::move(*(std::move(ptr_))).move_ros2_ptr(); } + +public: + message_ptr() : ptr_(nullptr) {} + + explicit message_ptr(agnocast::ipc_shared_ptr && ptr) + : ptr_(std::make_unique>(std::move(ptr))) + { + } + + explicit message_ptr(ros2_ptr_t && ptr) + : ptr_(std::make_unique>(std::move(ptr))) + { + } + + message_ptr(const message_ptr & r) + { + if (r.ptr_ != nullptr) { + ptr_ = r.ptr_->clone(); + } + } + message_ptr & operator=(const message_ptr & r) + { + if (this != &r) { + if (r.ptr_ != nullptr) { + ptr_ = r.ptr_->clone(); + } else { + ptr_ = nullptr; + } + } + return *this; + } + + message_ptr(message_ptr && r) noexcept = default; + message_ptr & operator=(message_ptr && r) noexcept = default; + + MessageT & operator*() const noexcept { return ptr_->as_ref(); } + + MessageT * operator->() const noexcept { return ptr_->as_ptr(); } + + explicit operator bool() const noexcept { return ptr_ && static_cast(ptr_->as_ptr()); } + + MessageT * get() const noexcept { return ptr_ ? ptr_->as_ptr() : nullptr; } }; // Defaults to zero if the environment variable is missing or invalid. @@ -204,45 +409,111 @@ inline bool use_agnocast() template class Subscription { - typename rclcpp::Subscription::SharedPtr ros2_sub_{nullptr}; - typename agnocast::Subscription::SharedPtr agnocast_sub_{nullptr}; - public: using SharedPtr = std::shared_ptr>; + virtual ~Subscription() = default; +}; + +template +class AgnocastSubscription : public Subscription +{ + typename agnocast::Subscription::SharedPtr subscription_; + +public: + template + explicit AgnocastSubscription( + NodeT * node, const std::string & topic_name, const rclcpp::QoS & qos, Func && callback, + const agnocast::SubscriptionOptions & options) + { + // TODO(Koichi98): NEBULA_MESSAGE_UNIQUE_PTR should be disallowed for Agnocast subscriptions. + // Agnocast uses shared memory, so mutable exclusive ownership is semantically incorrect and + // risks corrupting data read by other subscribers. Currently kept for compatibility with + // CudaPointcloudPreprocessorNode which uses UNIQUE_PTR callbacks. + static_assert( + std::is_invocable_v, NEBULA_MESSAGE_UNIQUE_PTR(MessageT) &&> || + std::is_invocable_v, NEBULA_MESSAGE_CONST_SHARED_PTR(MessageT) &&> || + std::is_invocable_v, const MessageT &>, + "callback should be invocable with an rvalue reference to either " + "NEBULA_MESSAGE_UNIQUE_PTR or NEBULA_MESSAGE_CONST_SHARED_PTR, or with a " + "const reference to the message type"); + + constexpr bool is_message_ptr_callback = + std::is_invocable_v, NEBULA_MESSAGE_UNIQUE_PTR(MessageT) &&> || + std::is_invocable_v, NEBULA_MESSAGE_CONST_SHARED_PTR(MessageT) &&>; + constexpr auto ownership = + std::is_invocable_v, NEBULA_MESSAGE_UNIQUE_PTR(MessageT) &&> + ? OwnershipType::Unique + : OwnershipType::Shared; + + subscription_ = agnocast::create_subscription( + node, topic_name, qos, + [callback = std::forward(callback)](agnocast::ipc_shared_ptr && msg) { + if constexpr (!is_message_ptr_callback) { + // msg keeps the shared-memory entry alive only while the callback runs: the + // reference is valid for the duration of the callback and no copy is made, but + // it must not be stored or used after the callback returns. Callbacks that need + // to extend the message lifetime should take NEBULA_MESSAGE_CONST_SHARED_PTR. + // as_const prevents generic callbacks from mutating the shared-memory entry, + // which other processes may be reading concurrently. + callback(std::as_const(*msg)); + } else if constexpr (ownership == OwnershipType::Unique) { + callback(message_ptr(std::move(msg))); + } else { + callback( + message_ptr( + agnocast::ipc_shared_ptr(std::move(msg)))); + } + }, + options); + } +}; + +template +class ROS2Subscription : public Subscription +{ + typename rclcpp::Subscription::SharedPtr subscription_; + +public: template - explicit Subscription( + explicit ROS2Subscription( rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos, Func && callback, const agnocast::SubscriptionOptions & options) { static_assert( std::is_invocable_v, NEBULA_MESSAGE_UNIQUE_PTR(MessageT) &&> || - std::is_invocable_v, NEBULA_MESSAGE_SHARED_PTR(MessageT) &&>, - "callback should be invocable with an rvalue reference to either NEBULA_MESSAGE_UNIQUE_PTR " - "or NEBULA_MESSAGE_SHARED_PTR"); + std::is_invocable_v, NEBULA_MESSAGE_CONST_SHARED_PTR(MessageT) &&> || + std::is_invocable_v, const MessageT &>, + "callback should be invocable with an rvalue reference to either " + "NEBULA_MESSAGE_UNIQUE_PTR or NEBULA_MESSAGE_CONST_SHARED_PTR, or with a " + "const reference to the message type"); + constexpr bool is_message_ptr_callback = + std::is_invocable_v, NEBULA_MESSAGE_UNIQUE_PTR(MessageT) &&> || + std::is_invocable_v, NEBULA_MESSAGE_CONST_SHARED_PTR(MessageT) &&>; constexpr auto ownership = std::is_invocable_v, NEBULA_MESSAGE_UNIQUE_PTR(MessageT) &&> ? OwnershipType::Unique : OwnershipType::Shared; - if (use_agnocast()) { - agnocast_sub_ = agnocast::create_subscription( - node, topic_name, qos, - [callback = std::forward(callback)](agnocast::ipc_shared_ptr && msg) { - callback(message_ptr(std::move(msg))); - }, - options); - } else { - rclcpp::SubscriptionOptions ros2_options; - ros2_options.callback_group = options.callback_group; - ros2_sub_ = node->create_subscription( - topic_name, qos, - [callback = std::forward(callback)](std::unique_ptr msg) { + rclcpp::SubscriptionOptions ros2_options; + ros2_options.callback_group = options.callback_group; + subscription_ = node->create_subscription( + topic_name, qos, + [callback = std::forward(callback)](std::unique_ptr msg) { + if constexpr (!is_message_ptr_callback) { + // as_const keeps this fallback consistent with the Agnocast path: generic + // callbacks must not observe a mutable reference on either path. + callback(std::as_const(*msg)); + } else if constexpr (ownership == OwnershipType::Unique) { callback(message_ptr(std::move(msg))); - }, - ros2_options); - } + } else { + callback( + message_ptr( + std::shared_ptr(std::move(msg)))); + } + }, + ros2_options); } }; @@ -251,8 +522,13 @@ typename Subscription::SharedPtr create_subscription( rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos, Func && callback, const agnocast::SubscriptionOptions & options) { - return std::make_shared>( - node, topic_name, qos, std::forward(callback), options); + if (use_agnocast()) { + return std::make_shared>( + node, topic_name, qos, std::forward(callback), options); + } else { + return std::make_shared>( + node, topic_name, qos, std::forward(callback), options); + } } template @@ -260,9 +536,15 @@ typename Subscription::SharedPtr create_subscription( rclcpp::Node * node, const std::string & topic_name, const size_t qos_history_depth, Func && callback, const agnocast::SubscriptionOptions & options) { - return std::make_shared>( - node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), - std::forward(callback), options); + if (use_agnocast()) { + return std::make_shared>( + node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), + std::forward(callback), options); + } else { + return std::make_shared>( + node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), + std::forward(callback), options); + } } template @@ -279,8 +561,17 @@ class Publisher virtual void publish(NEBULA_MESSAGE_UNIQUE_PTR(MessageT) && message) = 0; virtual void publish(NEBULA_MESSAGE_SHARED_PTR(MessageT) && message) = 0; + /// Publish by const reference (internally copies into allocated message). + /// This method is discouraged because it performs an implicit copy. + /// Prefer ALLOCATE_OUTPUT_MESSAGE_{UNIQUE,SHARED}(publisher) + the corresponding publish() + /// overload. May be marked [[deprecated]] in the future once autoware_cmake supports + /// suppressing deprecation warnings for test targets. + virtual void publish(const MessageT & data) = 0; + virtual uint32_t get_subscription_count() const = 0; - virtual uint32_t get_intra_subscription_count() const = 0; + virtual uint32_t get_intra_process_subscription_count() const = 0; + virtual const rmw_gid_t & get_gid() const = 0; + virtual const char * get_topic_name() const = 0; }; template @@ -289,8 +580,9 @@ class AgnocastPublisher : public Publisher typename agnocast::Publisher::SharedPtr publisher_; public: + template explicit AgnocastPublisher( - rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos, + NodeT * node, const std::string & topic_name, const rclcpp::QoS & qos, const agnocast::PublisherOptions & options) : publisher_(agnocast::create_publisher(node, topic_name, qos, options)) { @@ -306,21 +598,31 @@ class AgnocastPublisher : public Publisher return NEBULA_MESSAGE_SHARED_PTR(MessageT){publisher_->borrow_loaned_message()}; } - void publish(NEBULA_MESSAGE_UNIQUE_PTR(MessageT) && message) + void publish(NEBULA_MESSAGE_UNIQUE_PTR(MessageT) && message) override { publisher_->publish(std::move(message).move_agnocast_ptr()); } - void publish(NEBULA_MESSAGE_SHARED_PTR(MessageT) && message) + void publish(NEBULA_MESSAGE_SHARED_PTR(MessageT) && message) override { publisher_->publish(std::move(message).move_agnocast_ptr()); } + // See the comment on Publisher::publish(const MessageT &) for why this exists. + void publish(const MessageT & data) override + { + auto msg = publisher_->borrow_loaned_message(); + *msg = data; + publisher_->publish(std::move(msg)); + } + uint32_t get_subscription_count() const override { return publisher_->get_subscription_count(); } - uint32_t get_intra_subscription_count() const override + uint32_t get_intra_process_subscription_count() const override { return publisher_->get_intra_subscription_count(); } + const rmw_gid_t & get_gid() const override { return publisher_->get_gid(); } + const char * get_topic_name() const override { return publisher_->get_topic_name(); } }; template @@ -358,11 +660,16 @@ class ROS2Publisher : public Publisher publisher_->publish(*message); } + // See the comment on Publisher::publish(const MessageT &) for why this exists. + void publish(const MessageT & data) override { publisher_->publish(data); } + uint32_t get_subscription_count() const override { return publisher_->get_subscription_count(); } - uint32_t get_intra_subscription_count() const override + uint32_t get_intra_process_subscription_count() const override { return publisher_->get_intra_process_subscription_count(); } + const rmw_gid_t & get_gid() const override { return publisher_->get_gid(); } + const char * get_topic_name() const override { return publisher_->get_topic_name(); } }; template @@ -417,33 +724,779 @@ typename Publisher::SharedPtr create_publisher( } } -} // namespace nebula::agnocast_wrapper +template +class Client +{ +protected: + virtual bool wait_for_service_impl(std::chrono::nanoseconds timeout) const = 0; + +public: + using SharedPtr = std::shared_ptr>; + + using Future = std::future; + using SharedFuture = std::shared_future; + + struct FutureAndRequestId : rclcpp::detail::FutureAndRequestId + { + using rclcpp::detail::FutureAndRequestId::FutureAndRequestId; + SharedFuture share() noexcept { return this->future.share(); } + }; + struct SharedFutureAndRequestId : rclcpp::detail::FutureAndRequestId + { + using rclcpp::detail::FutureAndRequestId::FutureAndRequestId; + }; + + virtual ~Client() = default; + + virtual NEBULA_CLIENT_REQUEST_PTR(ServiceT) allocate_output_service_request() = 0; + virtual const char * get_service_name() const = 0; + + virtual bool service_is_ready() const = 0; + + template + bool wait_for_service( + std::chrono::duration timeout = std::chrono::nanoseconds(-1)) const + { + return wait_for_service_impl(std::chrono::duration_cast(timeout)); + } + + virtual FutureAndRequestId async_send_request(NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request) = 0; + virtual SharedFutureAndRequestId async_send_request( + NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request, std::function callback) = 0; +}; + +template +class AgnocastClient : public Client +{ + typename agnocast::Client::SharedPtr client_; + +protected: + bool wait_for_service_impl(std::chrono::nanoseconds timeout) const override + { + return client_->wait_for_service(timeout); + } + +public: + template + explicit AgnocastClient( + NodeT * node, const std::string & service_name, const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) + : client_(agnocast::create_client(node, service_name, qos, group)) + { + } + + NEBULA_CLIENT_REQUEST_PTR(ServiceT) allocate_output_service_request() override + { + return NEBULA_CLIENT_REQUEST_PTR(ServiceT){client_->borrow_loaned_request()}; + } + + const char * get_service_name() const override { return client_->get_service_name(); } + + bool service_is_ready() const override { return client_->service_is_ready(); } + + NEBULA_CLIENT_FUTURE_AND_REQUEST_ID(ServiceT) + async_send_request(NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request) override + { + // Wrap the promise in a shared_ptr so that the callback lambda can call set_value() + // through the pointer without needing 'mutable'. A unique_ptr wouldn't work here because + // the lambda is stored in a std::function, which requires its callable to be copyable. + auto promise_ptr = std::make_shared>(); + NEBULA_CLIENT_FUTURE(ServiceT) future = promise_ptr->get_future(); + + auto agnocast_request = std::move(request).move_agnocast_ptr(); + auto request_id = + client_ + ->async_send_request( + std::move(agnocast_request), + [promise_ptr = std::move(promise_ptr)]( + typename agnocast::Client::SharedFuture agnocast_shared_future) { + try { + typename agnocast::ipc_shared_ptr + agnocast_response = agnocast_shared_future.get(); + promise_ptr->set_value( + NEBULA_CLIENT_RESPONSE_PTR(ServiceT){std::move(agnocast_response)}); + } catch (...) { + promise_ptr->set_exception(std::current_exception()); + } + }) + .request_id; + + return NEBULA_CLIENT_FUTURE_AND_REQUEST_ID(ServiceT)(std::move(future), request_id); + } + + NEBULA_CLIENT_SHARED_FUTURE_AND_REQUEST_ID(ServiceT) + async_send_request( + NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request, + std::function callback) override + { + auto promise_ptr = std::make_shared>(); + NEBULA_CLIENT_SHARED_FUTURE(ServiceT) shared_future = promise_ptr->get_future().share(); + + auto agnocast_request = std::move(request).move_agnocast_ptr(); + auto request_id = + client_ + ->async_send_request( + std::move(agnocast_request), + [callback = std::move(callback), promise_ptr = std::move(promise_ptr), shared_future]( + typename agnocast::Client::SharedFuture agnocast_shared_future) { + // If an exception is set in the underlying future, propagate it to our promise. + try { + typename agnocast::ipc_shared_ptr + agnocast_response = agnocast_shared_future.get(); + promise_ptr->set_value( + NEBULA_CLIENT_RESPONSE_PTR(ServiceT){std::move(agnocast_response)}); + } catch (...) { + promise_ptr->set_exception(std::current_exception()); + return; + } + callback(std::move(shared_future)); + }) + .request_id; + + return NEBULA_CLIENT_SHARED_FUTURE_AND_REQUEST_ID(ServiceT)( + std::move(shared_future), request_id); + } +}; + +template +class ROS2Client : public Client +{ + typename rclcpp::Client::SharedPtr client_; + +protected: + bool wait_for_service_impl(std::chrono::nanoseconds timeout) const override + { + return client_->wait_for_service(timeout); + } + +public: + explicit ROS2Client( + rclcpp::Node * node, const std::string & service_name, const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +#if RCLCPP_VERSION_MAJOR >= 28 + : client_(node->create_client(service_name, qos, group)) #else + : client_(node->create_client(service_name, qos.get_rmw_qos_profile(), group)) +#endif + { + } -#include + NEBULA_CLIENT_REQUEST_PTR(ServiceT) allocate_output_service_request() override + { + return NEBULA_CLIENT_REQUEST_PTR(ServiceT){std::make_shared()}; + } + + const char * get_service_name() const override { return client_->get_service_name(); } + + bool service_is_ready() const override { return client_->service_is_ready(); } + + NEBULA_CLIENT_FUTURE_AND_REQUEST_ID(ServiceT) + async_send_request(NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request) override + { + auto promise_ptr = std::make_shared>(); + NEBULA_CLIENT_FUTURE(ServiceT) future = promise_ptr->get_future(); + + auto ros2_request = std::move(request).move_ros2_ptr(); + auto request_id = client_ + ->async_send_request( + ros2_request, + [promise_ptr = std::move(promise_ptr)]( + typename rclcpp::Client::SharedFuture ros2_shared_future) { + try { + std::shared_ptr ros2_response = + ros2_shared_future.get(); + promise_ptr->set_value( + NEBULA_CLIENT_RESPONSE_PTR(ServiceT){std::move(ros2_response)}); + } catch (...) { + promise_ptr->set_exception(std::current_exception()); + } + }) + .request_id; + + return NEBULA_CLIENT_FUTURE_AND_REQUEST_ID(ServiceT)(std::move(future), request_id); + } + + NEBULA_CLIENT_SHARED_FUTURE_AND_REQUEST_ID(ServiceT) + async_send_request( + NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request, + std::function callback) override + { + auto promise_ptr = std::make_shared>(); + NEBULA_CLIENT_SHARED_FUTURE(ServiceT) shared_future = promise_ptr->get_future().share(); + + auto ros2_request = std::move(request).move_ros2_ptr(); + auto request_id = + client_ + ->async_send_request( + ros2_request, + [callback = std::move(callback), promise_ptr = std::move(promise_ptr), + shared_future](typename rclcpp::Client::SharedFuture ros2_shared_future) { + // If an exception is set in the underlying future, propagate it to our promise. + try { + std::shared_ptr ros2_response = + ros2_shared_future.get(); + promise_ptr->set_value( + NEBULA_CLIENT_RESPONSE_PTR(ServiceT){std::move(ros2_response)}); + } catch (...) { + promise_ptr->set_exception(std::current_exception()); + return; + } + callback(std::move(shared_future)); + }) + .request_id; + + return NEBULA_CLIENT_SHARED_FUTURE_AND_REQUEST_ID(ServiceT)( + std::move(shared_future), request_id); + } +}; + +template +NEBULA_CLIENT_PTR(ServiceT) +create_client( + rclcpp::Node * node, const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + if (use_agnocast()) { + return std::make_shared>(node, service_name, qos, group); + } else { + return std::make_shared>(node, service_name, qos, group); + } +} + +template +class Service +{ +public: + using SharedPtr = std::shared_ptr>; + + virtual ~Service() = default; +}; + +// True when Callback takes the preferred NEBULA_SERVER_REQUEST_PTR/RESPONSE_PTR (message_ptr) +// pair, i.e. it is written against the wrapper's zero-copy service API. +template +inline constexpr bool is_message_ptr_service_callback_v = std::is_invocable_v< + std::decay_t, NEBULA_SERVER_REQUEST_PTR(ServiceT) &&, + NEBULA_SERVER_RESPONSE_PTR(ServiceT) &&>; + +// True when Callback is an rclcpp-style handler taking std::shared_ptr request/response. This lets +// utilities written for rclcpp::Node (e.g. autoware_utils_logging's LoggerLevelConfigure) be used +// unchanged on the wrapper Node, at the cost noted on the convenience paths below. +template +inline constexpr bool is_shared_ptr_service_callback_v = std::is_invocable_v< + std::decay_t, std::shared_ptr &, + std::shared_ptr &>; + +template +class AgnocastService : public Service +{ + typename agnocast::Service::SharedPtr srv_; + +public: + template + explicit AgnocastService( + NodeT * node, const std::string & service_name, Func && callback, const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) + { + static_assert( + is_message_ptr_service_callback_v, + "Callback should be invocable with NEBULA_SERVER_REQUEST_PTR and " + "NEBULA_SERVER_RESPONSE_PTR (const&, &&, or by-value)"); + + srv_ = agnocast::create_service( + node, service_name, + [callback = std::forward(callback)]( + agnocast::ipc_shared_ptr && agnocast_request, + agnocast::ipc_shared_ptr && agnocast_response) { + callback( + NEBULA_SERVER_REQUEST_PTR(ServiceT){std::move(agnocast_request)}, + NEBULA_SERVER_RESPONSE_PTR(ServiceT){std::move(agnocast_response)}); + }, + qos, group); + } +}; + +template +class ROS2Service : public Service +{ + typename rclcpp::Service::SharedPtr srv_; + +public: + template + explicit ROS2Service( + rclcpp::Node * node, const std::string & service_name, Func && callback, + const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) + { + static_assert( + is_message_ptr_service_callback_v, + "Callback should be invocable with NEBULA_SERVER_REQUEST_PTR and " + "NEBULA_SERVER_RESPONSE_PTR (const&, &&, or by-value)"); + + srv_ = node->create_service( + service_name, + [callback = std::forward(callback)]( + std::shared_ptr && ros2_request, + std::shared_ptr && ros2_response) { + callback( + NEBULA_SERVER_REQUEST_PTR(ServiceT){std::move(ros2_request)}, + NEBULA_SERVER_RESPONSE_PTR(ServiceT){std::move(ros2_response)}); + }, +#if RCLCPP_VERSION_MAJOR >= 28 + qos, group); +#else + qos.get_rmw_qos_profile(), group); +#endif + } +}; + +template +NEBULA_SERVICE_PTR(ServiceT) +create_service( + rclcpp::Node * node, const std::string & service_name, Func && callback, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + if (use_agnocast()) { + return std::make_shared>( + node, service_name, std::forward(callback), qos, group); + } else { + return std::make_shared>( + node, service_name, std::forward(callback), qos, group); + } +} + +/// @brief Type-erased timer handle for the Agnocast build. +/// +/// Backed by AgnocastTimer or ROS2Timer depending on whether Agnocast is +/// active at runtime. Must be obtained via Node::create_wall_timer() or the +/// free create_timer() — do not construct directly. set_period() is +/// intentionally private; use the free set_period(Timer::SharedPtr, ...) +/// instead, which is also available in non-Agnocast builds. +/// +/// Cross-build portability: in non-Agnocast builds NEBULA_TIMER_PTR +/// resolves to rclcpp::TimerBase::SharedPtr and exposes the full rclcpp API; +/// in Agnocast builds it resolves to this wrapper, which only exposes +/// cancel/reset/is_canceled/time_until_trigger and the free set_period(). +/// Calling any other rclcpp::TimerBase method compiles in non-Agnocast builds +/// but breaks once Agnocast is enabled. Stay within the wrapper's surface to +/// remain portable. +/// +/// Exception contract: the exception type and "throw vs no-op" behavior of +/// these methods is not normalized across backends — the rclcpp backend tends +/// to throw rclcpp::exceptions::RCLError on rcl failure, while the Agnocast +/// backend may throw std::runtime_error or silently return a sentinel. This +/// asymmetry exists across the wrapper as a whole (not Timer-specific) and is +/// expected to be normalized in a follow-up. +class Timer +{ +public: + using SharedPtr = std::shared_ptr; + virtual ~Timer() = default; + + virtual void cancel() = 0; + virtual void reset() = 0; + virtual bool is_canceled() = 0; + virtual std::chrono::nanoseconds time_until_trigger() = 0; + +private: + // Private so callers must use the free set_period() function, which also works in the + // non-agnocast build (where NEBULA_TIMER_PTR is a plain rclcpp::TimerBase, no set_period). + virtual void set_period(std::chrono::nanoseconds period) = 0; + friend void set_period(const SharedPtr & timer, std::chrono::nanoseconds period); +}; + +class AgnocastTimer : public Timer +{ + std::shared_ptr timer_; + +public: + explicit AgnocastTimer(std::shared_ptr timer) : timer_(std::move(timer)) {} + + void cancel() override { timer_->cancel(); } + void reset() override { timer_->reset(); } + bool is_canceled() override { return timer_->is_canceled(); } + std::chrono::nanoseconds time_until_trigger() override { return timer_->time_until_trigger(); } + +private: + void set_period(std::chrono::nanoseconds period) override { timer_->set_period(period); } +}; + +class ROS2Timer : public Timer +{ + rclcpp::TimerBase::SharedPtr timer_; + +public: + explicit ROS2Timer(rclcpp::TimerBase::SharedPtr timer) : timer_(std::move(timer)) {} + + void cancel() override { timer_->cancel(); } + void reset() override { timer_->reset(); } + bool is_canceled() override { return timer_->is_canceled(); } + std::chrono::nanoseconds time_until_trigger() override { return timer_->time_until_trigger(); } + +private: + // rclcpp::TimerBase does not expose a set_period API; fall back to the rcl C API and + // convert the rcl_ret_t to an rclcpp::exceptions::RCLError (matching the throw style used + // by the other rclcpp timer methods such as cancel/reset/time_until_trigger). + void set_period(std::chrono::nanoseconds period) override + { + int64_t old_period = 0; + const rcl_ret_t ret = + rcl_timer_exchange_period(timer_->get_timer_handle().get(), period.count(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set timer period"); + } + } +}; + +/// @brief Set the timer period. +/// +/// Provided as a free function so the same call site compiles in both builds: +/// in non-Agnocast builds rclcpp::TimerBase has no set_period member, so a +/// free overload is the only portable form. Timer::set_period is private to +/// prevent member-style calls that would not survive the non-Agnocast build. +/// +/// @throws std::invalid_argument if period is negative or equal to +/// std::chrono::nanoseconds::max() (mirrors rclcpp::create_wall_timer's +/// precondition 0 <= period < nanoseconds::max()). +/// @throws rclcpp::exceptions::RCLError on rcl-level failure when the ROS 2 +/// backend is active (see Timer's class-level note on exception asymmetry +/// between backends). +inline void set_period(const Timer::SharedPtr & timer, std::chrono::nanoseconds period) +{ + if (period < std::chrono::nanoseconds::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + if (period == std::chrono::nanoseconds::max()) { + throw std::invalid_argument{"timer period must be less than std::chrono::nanoseconds::max()"}; + } + timer->set_period(period); +} + +} // namespace nebula::agnocast_wrapper + +#else #define NEBULA_MESSAGE_UNIQUE_PTR(MessageT) std::unique_ptr + +// For publisher (mutable message) #define NEBULA_MESSAGE_SHARED_PTR(MessageT) std::shared_ptr +// For subscription (read-only message) +#define NEBULA_MESSAGE_CONST_SHARED_PTR(MessageT) std::shared_ptr +#define NEBULA_SERVER_REQUEST_PTR(ServiceT) std::shared_ptr +#define NEBULA_SERVER_RESPONSE_PTR(ServiceT) std::shared_ptr +#define NEBULA_CLIENT_REQUEST_PTR(ServiceT) std::shared_ptr +#define NEBULA_CLIENT_RESPONSE_PTR(ServiceT) std::shared_ptr #define NEBULA_SUBSCRIPTION_PTR(MessageT) typename rclcpp::Subscription::SharedPtr #define NEBULA_PUBLISHER_PTR(MessageT) typename rclcpp::Publisher::SharedPtr - -#define NEBULA_CREATE_SUBSCRIPTION(message_type, node_ptr, topic, qos, callback, options) \ - (node_ptr)->create_subscription(topic, qos, callback, options) -#define NEBULA_CREATE_PUBLISHER2(message_type, node_ptr, arg1, arg2) \ - (node_ptr)->create_publisher(arg1, arg2) -#define NEBULA_CREATE_PUBLISHER3(message_type, node_ptr, arg1, arg2, arg3) \ - (node_ptr)->create_publisher(arg1, arg2, arg3) +#define NEBULA_CLIENT_PTR(ServiceT) typename nebula::agnocast_wrapper::Client::SharedPtr +#define NEBULA_SERVICE_PTR(ServiceT) typename nebula::agnocast_wrapper::Service::SharedPtr +#define NEBULA_CLIENT_FUTURE(ServiceT) typename nebula::agnocast_wrapper::Client::Future +#define NEBULA_CLIENT_SHARED_FUTURE(ServiceT) \ + typename nebula::agnocast_wrapper::Client::SharedFuture +#define NEBULA_CLIENT_FUTURE_AND_REQUEST_ID(ServiceT) \ + typename nebula::agnocast_wrapper::Client::FutureAndRequestId +#define NEBULA_CLIENT_SHARED_FUTURE_AND_REQUEST_ID(ServiceT) \ + typename nebula::agnocast_wrapper::Client::SharedFutureAndRequestId +#define NEBULA_TIMER_PTR rclcpp::TimerBase::SharedPtr + +#define NEBULA_CREATE_SUBSCRIPTION(message_type, topic, qos, callback, options) \ + this->create_subscription(topic, qos, callback, options) +#define NEBULA_CREATE_SUBSCRIPTION_ON_NODE(message_type, node, topic, qos, callback, options) \ + (node)->create_subscription(topic, qos, callback, options) + +#define NEBULA_CREATE_PUBLISHER2(message_type, arg1, arg2) \ + this->create_publisher(arg1, arg2) +#define NEBULA_CREATE_PUBLISHER3(message_type, arg1, arg2, arg3) \ + this->create_publisher(arg1, arg2, arg3) +#define NEBULA_CREATE_PUBLISHER2_ON_NODE(message_type, node, arg1, arg2) \ + (node)->create_publisher(arg1, arg2) +#define NEBULA_CREATE_PUBLISHER3_ON_NODE(message_type, node, arg1, arg2, arg3) \ + (node)->create_publisher(arg1, arg2, arg3) + +#define NEBULA_CREATE_CLIENT1(service_type, service_name) \ + nebula::agnocast_wrapper::create_client(this, service_name) +#define NEBULA_CREATE_CLIENT2(service_type, service_name, qos) \ + nebula::agnocast_wrapper::create_client(this, service_name, qos) +#define NEBULA_CREATE_CLIENT3(service_type, service_name, qos, group) \ + nebula::agnocast_wrapper::create_client(this, service_name, qos, group) +#define NEBULA_CREATE_CLIENT1_ON_NODE(service_type, node, service_name) \ + nebula::agnocast_wrapper::create_client(node, service_name) +#define NEBULA_CREATE_CLIENT2_ON_NODE(service_type, node, service_name, qos) \ + nebula::agnocast_wrapper::create_client(node, service_name, qos) +#define NEBULA_CREATE_CLIENT3_ON_NODE(service_type, node, service_name, qos, group) \ + nebula::agnocast_wrapper::create_client(node, service_name, qos, group) + +#define NEBULA_CREATE_SERVICE2(service_type, service_name, callback) \ + nebula::agnocast_wrapper::create_service(this, service_name, callback) +#define NEBULA_CREATE_SERVICE3(service_type, service_name, callback, qos) \ + nebula::agnocast_wrapper::create_service(this, service_name, callback, qos) +#define NEBULA_CREATE_SERVICE4(service_type, service_name, callback, qos, group) \ + nebula::agnocast_wrapper::create_service(this, service_name, callback, qos, group) +#define NEBULA_CREATE_SERVICE2_ON_NODE(service_type, node, service_name, callback) \ + nebula::agnocast_wrapper::create_service(node, service_name, callback) +#define NEBULA_CREATE_SERVICE3_ON_NODE(service_type, node, service_name, callback, qos) \ + nebula::agnocast_wrapper::create_service(node, service_name, callback, qos) +#define NEBULA_CREATE_SERVICE4_ON_NODE(service_type, node, service_name, callback, qos, group) \ + nebula::agnocast_wrapper::create_service(node, service_name, callback, qos, group) #define NEBULA_SUBSCRIPTION_OPTIONS rclcpp::SubscriptionOptions #define NEBULA_PUBLISHER_OPTIONS rclcpp::PublisherOptions -#define NEBULA_HAS_ANY_SUBSCRIPTIONS(publisher) \ - (publisher->get_subscription_count() > 0 || publisher->get_intra_process_subscription_count() > 0) - #define ALLOCATE_OUTPUT_MESSAGE_UNIQUE(publisher) \ std::make_unique::type::ROSMessageType>() #define ALLOCATE_OUTPUT_MESSAGE_SHARED(publisher) \ std::make_shared::type::ROSMessageType>() +#define ALLOCATE_OUTPUT_SERVICE_REQUEST(client) client->allocate_output_service_request() + +namespace nebula::agnocast_wrapper +{ + +// ===== Client/Service, non-Agnocast build ===== +// +// Mirrors the Agnocast-build Client/Service abstraction so code written +// against NEBULA_CLIENT_PTR/NEBULA_SERVICE_PTR compiles unchanged in both builds. +// async_send_request() still bridges through a promise: NEBULA_CLIENT_FUTURE(ServiceT) and +// rclcpp::Client::Future are different std::future instantiations, and std::future has +// no covariant conversion between them. + +template +class Client +{ +protected: + virtual bool wait_for_service_impl(std::chrono::nanoseconds timeout) const = 0; + +public: + using SharedPtr = std::shared_ptr>; + + using Future = std::future; + using SharedFuture = std::shared_future; + + struct FutureAndRequestId : rclcpp::detail::FutureAndRequestId + { + using rclcpp::detail::FutureAndRequestId::FutureAndRequestId; + SharedFuture share() noexcept { return this->future.share(); } + }; + struct SharedFutureAndRequestId : rclcpp::detail::FutureAndRequestId + { + using rclcpp::detail::FutureAndRequestId::FutureAndRequestId; + }; + + virtual ~Client() = default; + + virtual NEBULA_CLIENT_REQUEST_PTR(ServiceT) allocate_output_service_request() = 0; + + virtual const char * get_service_name() const = 0; + + virtual bool service_is_ready() const = 0; + + template + bool wait_for_service( + std::chrono::duration timeout = std::chrono::nanoseconds(-1)) const + { + return wait_for_service_impl(std::chrono::duration_cast(timeout)); + } + + virtual FutureAndRequestId async_send_request(NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request) = 0; + virtual SharedFutureAndRequestId async_send_request( + NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request, std::function callback) = 0; +}; + +template +class ROS2Client : public Client +{ + typename rclcpp::Client::SharedPtr client_; + +protected: + bool wait_for_service_impl(std::chrono::nanoseconds timeout) const override + { + return client_->wait_for_service(timeout); + } + +public: + explicit ROS2Client( + rclcpp::Node * node, const std::string & service_name, const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +#if RCLCPP_VERSION_MAJOR >= 28 + : client_(node->create_client(service_name, qos, group)) +#else + : client_(node->create_client(service_name, qos.get_rmw_qos_profile(), group)) +#endif + { + } + + NEBULA_CLIENT_REQUEST_PTR(ServiceT) allocate_output_service_request() override + { + return std::make_shared(); + } + + const char * get_service_name() const override { return client_->get_service_name(); } + + bool service_is_ready() const override { return client_->service_is_ready(); } + + // rclcpp::Client::Future (std::future>) and + // NEBULA_CLIENT_FUTURE(ServiceT) (std::future>) are different + // std::future instantiations with no covariant conversion between them, so the result can't be + // returned directly -- bridge it through a promise instead. + NEBULA_CLIENT_FUTURE_AND_REQUEST_ID(ServiceT) + async_send_request(NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request) override + { + auto promise_ptr = std::make_shared>(); + NEBULA_CLIENT_FUTURE(ServiceT) future = promise_ptr->get_future(); + + auto request_id = client_ + ->async_send_request( + std::move(request), + [promise_ptr = std::move(promise_ptr)]( + typename rclcpp::Client::SharedFuture ros2_shared_future) { + try { + promise_ptr->set_value( + NEBULA_CLIENT_RESPONSE_PTR(ServiceT){ros2_shared_future.get()}); + } catch (...) { + promise_ptr->set_exception(std::current_exception()); + } + }) + .request_id; + + return NEBULA_CLIENT_FUTURE_AND_REQUEST_ID(ServiceT)(std::move(future), request_id); + } + + NEBULA_CLIENT_SHARED_FUTURE_AND_REQUEST_ID(ServiceT) + async_send_request( + NEBULA_CLIENT_REQUEST_PTR(ServiceT) && request, + std::function callback) override + { + auto promise_ptr = std::make_shared>(); + NEBULA_CLIENT_SHARED_FUTURE(ServiceT) shared_future = promise_ptr->get_future().share(); + + auto request_id = + client_ + ->async_send_request( + std::move(request), + [callback = std::move(callback), promise_ptr = std::move(promise_ptr), + shared_future](typename rclcpp::Client::SharedFuture ros2_shared_future) { + // If an exception is set in the underlying future, propagate it to our promise. + try { + promise_ptr->set_value( + NEBULA_CLIENT_RESPONSE_PTR(ServiceT){ros2_shared_future.get()}); + } catch (...) { + promise_ptr->set_exception(std::current_exception()); + return; + } + callback(std::move(shared_future)); + }) + .request_id; + + return NEBULA_CLIENT_SHARED_FUTURE_AND_REQUEST_ID(ServiceT)( + std::move(shared_future), request_id); + } +}; + +template +NEBULA_CLIENT_PTR(ServiceT) +create_client( + rclcpp::Node * node, const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return std::make_shared>(node, service_name, qos, group); +} + +template +class Service +{ +public: + using SharedPtr = std::shared_ptr>; + + virtual ~Service() = default; +}; + +// True when Callback takes the preferred NEBULA_SERVER_REQUEST_PTR/RESPONSE_PTR pair, i.e. it +// is written against the wrapper's service API. +template +inline constexpr bool is_message_ptr_service_callback_v = std::is_invocable_v< + std::decay_t, NEBULA_SERVER_REQUEST_PTR(ServiceT) &&, + NEBULA_SERVER_RESPONSE_PTR(ServiceT) &&>; + +// True when Callback is an rclcpp-style handler taking std::shared_ptr request/response. This lets +// utilities written for rclcpp::Node be used unchanged on the wrapper Node. +template +inline constexpr bool is_shared_ptr_service_callback_v = std::is_invocable_v< + std::decay_t, std::shared_ptr &, + std::shared_ptr &>; + +template +class ROS2Service : public Service +{ + typename rclcpp::Service::SharedPtr srv_; + +public: + template + explicit ROS2Service( + rclcpp::Node * node, const std::string & service_name, Func && callback, + const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) + { + static_assert( + is_message_ptr_service_callback_v, + "Callback should be invocable with NEBULA_SERVER_REQUEST_PTR and " + "NEBULA_SERVER_RESPONSE_PTR (const&, &&, or by-value)"); + + srv_ = node->create_service( + service_name, + [callback = std::forward(callback)]( + std::shared_ptr && ros2_request, + std::shared_ptr && ros2_response) { + callback( + NEBULA_SERVER_REQUEST_PTR(ServiceT){std::move(ros2_request)}, + NEBULA_SERVER_RESPONSE_PTR(ServiceT){std::move(ros2_response)}); + }, +#if RCLCPP_VERSION_MAJOR >= 28 + qos, group); +#else + qos.get_rmw_qos_profile(), group); +#endif + } +}; + +template +NEBULA_SERVICE_PTR(ServiceT) +create_service( + rclcpp::Node * node, const std::string & service_name, Func && callback, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return std::make_shared>( + node, service_name, std::forward(callback), qos, group); +} + +} // namespace nebula::agnocast_wrapper + +namespace nebula::agnocast_wrapper +{ + +/// @brief Set the timer period (non-Agnocast build). +/// +/// rclcpp::TimerBase has no set_period member, so we provide a free overload +/// that falls back to the rcl C API. Mirrors the Agnocast-build overload on +/// Timer::SharedPtr so the same call site works in both builds. +/// +/// @throws std::invalid_argument if period is negative or equal to +/// std::chrono::nanoseconds::max() (mirrors rclcpp::create_wall_timer's +/// precondition 0 <= period < nanoseconds::max()). +/// @throws rclcpp::exceptions::RCLError on rcl-level failure. +inline void set_period(const rclcpp::TimerBase::SharedPtr & timer, std::chrono::nanoseconds period) +{ + if (period < std::chrono::nanoseconds::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + if (period == std::chrono::nanoseconds::max()) { + throw std::invalid_argument{"timer period must be less than std::chrono::nanoseconds::max()"}; + } + int64_t old_period = 0; + const rcl_ret_t ret = + rcl_timer_exchange_period(timer->get_timer_handle().get(), period.count(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set timer period"); + } +} + +} // namespace nebula::agnocast_wrapper #endif diff --git a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/node.hpp b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/node.hpp index 30aa399c2..982aa71ae 100644 --- a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/node.hpp +++ b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/agnocast_wrapper/node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_ros/agnocast_wrapper/autoware_agnocast_wrapper.hpp" +#include "nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp" #include @@ -434,7 +434,7 @@ namespace nebula::agnocast_wrapper { // is_message_ptr_service_callback_v / is_shared_ptr_service_callback_v are defined in -// autoware_agnocast_wrapper.hpp, identically named in both builds. +// nebula_agnocast_wrapper.hpp, identically named in both builds. /// @brief Node class for the non-Agnocast build. /// diff --git a/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp index 3f1446337..2391456d8 100644 --- a/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp @@ -89,11 +89,11 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( auto pointcloud_qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - nebula_points_pub_ = NEBULA_CREATE_PUBLISHER2( + nebula_points_pub_ = NEBULA_CREATE_PUBLISHER2_ON_NODE( sensor_msgs::msg::PointCloud2, &parent_node_, "pandar_points", pointcloud_qos); - aw_points_base_pub_ = NEBULA_CREATE_PUBLISHER2( + aw_points_base_pub_ = NEBULA_CREATE_PUBLISHER2_ON_NODE( sensor_msgs::msg::PointCloud2, &parent_node_, "aw_points", pointcloud_qos); - aw_points_ex_pub_ = NEBULA_CREATE_PUBLISHER2( + aw_points_ex_pub_ = NEBULA_CREATE_PUBLISHER2_ON_NODE( sensor_msgs::msg::PointCloud2, &parent_node_, "aw_points_ex", pointcloud_qos); RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); @@ -176,13 +176,17 @@ void HesaiDecoderWrapper::on_pointcloud_decoded( rclcpp::Time cloud_stamp = rclcpp::Time(seconds_to_chrono_nano_seconds(timestamp_s).count()); - if (NEBULA_HAS_ANY_SUBSCRIPTIONS(nebula_points_pub_)) { + if ( + nebula_points_pub_->get_subscription_count() > 0 || + nebula_points_pub_->get_intra_process_subscription_count() > 0) { auto ros_pc_msg_ptr = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(nebula_points_pub_); *ros_pc_msg_ptr = nebula::ros::to_ros_msg(*pointcloud); ros_pc_msg_ptr->header.stamp = cloud_stamp; publish_cloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); } - if (NEBULA_HAS_ANY_SUBSCRIPTIONS(aw_points_base_pub_)) { + if ( + aw_points_base_pub_->get_subscription_count() > 0 || + aw_points_base_pub_->get_intra_process_subscription_count() > 0) { auto autoware_cloud_xyzi = nebula::drivers::convert_point_xyzircaedt_to_point_xyzir(*pointcloud); auto ros_pc_msg_ptr = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(aw_points_base_pub_); @@ -190,7 +194,9 @@ void HesaiDecoderWrapper::on_pointcloud_decoded( ros_pc_msg_ptr->header.stamp = cloud_stamp; publish_cloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); } - if (NEBULA_HAS_ANY_SUBSCRIPTIONS(aw_points_ex_pub_)) { + if ( + aw_points_ex_pub_->get_subscription_count() > 0 || + aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { auto autoware_ex_cloud = nebula::drivers::convert_point_xyzircaedt_to_point_xyziradt(*pointcloud, timestamp_s); auto ros_pc_msg_ptr = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(aw_points_ex_pub_); @@ -285,7 +291,7 @@ HesaiDecoderWrapper::initialize_blockage_mask_plugin() auto blockage_mask_plugin = std::make_shared( sensor_cfg_->blockage_mask_horizontal_bin_size_mdeg.value()); - auto blockage_mask_pub = NEBULA_CREATE_PUBLISHER2( + auto blockage_mask_pub = NEBULA_CREATE_PUBLISHER2_ON_NODE( sensor_msgs::msg::Image, &parent_node_, "blockage_mask", rclcpp::SensorDataQoS()); blockage_mask_plugin->set_callback(