diff --git a/include/cactus_rt/ros2/publisher.h b/include/cactus_rt/ros2/publisher.h index eac4fea..c8fd498 100644 --- a/include/cactus_rt/ros2/publisher.h +++ b/include/cactus_rt/ros2/publisher.h @@ -83,25 +83,22 @@ class Publisher : public IPublisher { const rclcpp::QoS& qos, const size_t rt_queue_size = 1000 ) { - return std::make_shared>( - logger, - node.create_publisher(topic_name, qos), - moodycamel::ReaderWriterQueue(rt_queue_size) + return std::shared_ptr>( + new Publisher( + logger, + node.create_publisher(topic_name, qos), + moodycamel::ReaderWriterQueue(rt_queue_size) + ) ); } - public: - /** - * Constructs a publisher. Shouldn't be called directly. Only public to enable make_shared. - * - * @private - */ Publisher( quill::Logger* logger, typename rclcpp::Publisher::SharedPtr publisher, moodycamel::ReaderWriterQueue&& queue ) : logger_(logger), publisher_(publisher), queue_(std::move(queue)) {} + public: template bool Publish(Args&&... args) noexcept { const bool success = queue_.try_emplace(std::forward(args)...); diff --git a/include/cactus_rt/ros2/subscription.h b/include/cactus_rt/ros2/subscription.h index 72bdd02..a501e51 100644 --- a/include/cactus_rt/ros2/subscription.h +++ b/include/cactus_rt/ros2/subscription.h @@ -62,7 +62,9 @@ class SubscriptionLatest : public ISubscription { const std::string& topic_name, const rclcpp::QoS& qos ) { - auto subscription = std::make_shared>(logger); + std::shared_ptr> subscription( + new SubscriptionLatest(logger) + ); subscription->ros_subscription_ = node.create_subscription( topic_name, @@ -76,14 +78,9 @@ class SubscriptionLatest : public ISubscription { return subscription; } - public: - /** - * @brief Do not use this method. Defined to allow make_shared to work. - * - * @private - */ explicit SubscriptionLatest(quill::Logger* logger) : logger_(logger) {} + public: StampedValue ReadLatest() noexcept { return latest_value_.Read(); }