From 0725edeb6c53b7b28b5707dce130419bdeba8033 Mon Sep 17 00:00:00 2001 From: Dominik Authaler Date: Mon, 12 Feb 2024 10:34:48 +0000 Subject: [PATCH 1/5] more generic implementation using NodeInterfaces from rclcpp Signed-off-by: Dominik Authaler --- include/message_filters/subscriber.h | 206 ++++++++++++++------------- test/test_subscriber.cpp | 2 +- 2 files changed, 107 insertions(+), 101 deletions(-) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 387758a..98375d0 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -39,6 +39,8 @@ #include #include +#include +#include #include "message_filters/connection.h" #include "message_filters/simple_filter.h" @@ -46,67 +48,39 @@ namespace message_filters { -template class SubscriberBase { public: - typedef std::shared_ptr NodePtr; + using NodeParametersInterface = rclcpp::node_interfaces::NodeParametersInterface; + using NodeTopicsInterface = rclcpp::node_interfaces::NodeTopicsInterface; - virtual ~SubscriberBase() = default; - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe - */ - virtual void subscribe(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + virtual ~SubscriberBase() = default; /** * \brief Subscribe to a topic. * * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. * - * \param node The rclcpp::Node to use to subscribe. + * \param node The NodeInterfaces to use to subscribe. * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - virtual void subscribe(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; + virtual void subscribe(RequiredInterfaces node_interfaces, const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; /** * \brief Subscribe to a topic. * * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * This override allows SubscriptionOptions to be passed into the class without changing API. * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - virtual void subscribe( - NodePtr node, - const std::string& topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) - { - this->subscribe(node.get(), topic, qos, options); - }; - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. + * \param node The NodeInterfaces to use to subscribe. * \param topic The topic to subscribe to. * \param qos The rmw qos profile to use to subscribe. * \param options The subscription options to use to subscribe. */ virtual void subscribe( - NodeType * node, + RequiredInterfaces node_interfaces, const std::string& topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) = 0; @@ -120,8 +94,8 @@ class SubscriberBase */ virtual void unsubscribe() = 0; }; -template -using SubscriberBasePtr = std::shared_ptr>; + +using SubscriberBasePtr = std::shared_ptr; /** * \brief ROS subscription filter. @@ -161,62 +135,93 @@ struct message_type template using message_type_t = typename message_type::type; -template +template class Subscriber -: public SubscriberBase +: public SubscriberBase , public SimpleFilter> { public: - typedef std::shared_ptr NodePtr; typedef message_type_t MessageType; typedef MessageEvent EventType; /** - * \brief Constructor - * - * See the rclcpp::Node::subscribe() variants for more information on the parameters + * \brief Constructor for rclcpp::Node / rclcpp_lifecycle::LifecycleNode. * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param node The NodeInterfaces to use to subscribe. * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - Subscriber(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + Subscriber(RequiredInterfaces node_interfaces, const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) { - subscribe(node, topic, qos); + subscribe(node_interfaces, topic, qos); } - Subscriber(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) - { - subscribe(node, topic, qos); - } + /** + * \brief Constructor for rclcpp::Node::SharedPtr / rclcpp_lifecycle::LifecycleNode::SharedPtr. + * + * \param node The NodeT::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe + */ + template + Subscriber(std::shared_ptr node, const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) : Subscriber(node.get(), topic, qos) {} /** - * \brief Constructor + * \brief Constructor for raw pointer to rclcpp::Node / rclcpp_lifecycle::LifecycleNode. * - * See the rclcpp::Node::subscribe() variants for more information on the parameters + * \param node The NodeT raw pointer to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe + */ + template + Subscriber(NodeT * node, const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) : Subscriber(*node, topic, qos) {} + + /** + * \brief Constructor for rclcpp::Node / rclcpp_lifecycle::LifecycleNode. * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param node_interfaces The NodeInterfaces to use to subscribe. * \param topic The topic to subscribe to. * \param qos The rmw qos profile to use to subscribe. * \param options The subscription options to use to subscribe. */ - Subscriber( - NodePtr node, - const std::string& topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) + Subscriber(RequiredInterfaces node_interfaces, + const std::string& topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) { - subscribe(node.get(), topic, qos, options); + subscribe(node_interfaces, topic, qos, options); } - Subscriber( - NodeType * node, - const std::string& topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) - { - subscribe(node, topic, qos, options); - } + /** + * \brief Constructor for rclcpp::Node::SharedPtr / rclcpp_lifecycle::LifecycleNode::SharedPtr. + * + * \param node The NodeT::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + template + Subscriber(std::shared_ptr node, + const std::string& topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) : Subscriber(node.get(), topic, qos, options) {} + + /** + * \brief Constructor for raw pointer to rclcpp::Node / rclcpp_lifecycle::LifecycleNode. + * + * \param node The NodeT raw pointer to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + template + Subscriber(NodeT * node, + const std::string& topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) : Subscriber(*node, topic, qos, options) {} /** * \brief Empty constructor, use subscribe() to subscribe to a topic @@ -233,13 +238,14 @@ class Subscriber * * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param node_interfaces The NodeInterfaces to use to subscribe. * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - void subscribe(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) override + // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead. + void subscribe(RequiredInterfaces node_interfaces, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) override { - subscribe(node.get(), topic, qos, rclcpp::SubscriptionOptions()); + subscribe(node_interfaces, topic, qos, rclcpp::SubscriptionOptions()); } /** @@ -247,14 +253,15 @@ class Subscriber * * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. * - * \param node The rclcpp::Node to use to subscribe. + * \param node The NodeT::SharedPtr to use to subscribe. * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead. - void subscribe(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) override + template + void subscribe(std::shared_ptr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) { - subscribe(node, topic, qos, rclcpp::SubscriptionOptions()); + subscribe(node.get(), topic, qos, rclcpp::SubscriptionOptions()); } /** @@ -262,20 +269,15 @@ class Subscriber * * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param node The NodeT raw pointer to use to subscribe. * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. + * \param qos (optional) The rmw qos profile to use to subscribe */ - void subscribe( - NodePtr node, - const std::string& topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) override + // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead. + template + void subscribe(NodeT * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) { - subscribe(node.get(), topic, qos, options); - node_raw_ = nullptr; - node_shared_ = node; + subscribe(*node, topic, qos, rclcpp::SubscriptionOptions()); } /** @@ -283,14 +285,14 @@ class Subscriber * * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. * - * \param node The rclcpp::Node to use to subscribe. + * \param node The NodeInterfaces to use to subscribe. * \param topic The topic to subscribe to. * \param qos The rmw qos profile to use to subscribe * \param options The subscription options to use to subscribe. */ // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead. void subscribe( - NodeType * node, + RequiredInterfaces node_interfaces, const std::string& topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) override @@ -304,12 +306,21 @@ class Subscriber rclcpp_qos.get_rmw_qos_profile() = qos; qos_ = qos; options_ = options; - sub_ = node->template create_subscription(topic, rclcpp_qos, - [this](const std::shared_ptr msg) { - this->cb(EventType(msg)); - }, options); - node_raw_ = node; + // Note: temporary variables are solely needed because the involved submodules are inconsistent about the passing + // of shared_ptr. While the node_interfaces return by value, the create_subscription function expects the + // shared_ptr to be passed via reference... + auto parameters_interface = node_interfaces.get(); + auto topics_interface = node_interfaces.get(); + + sub_ = rclcpp::create_subscription(parameters_interface, + topics_interface, + topic, rclcpp_qos, + [this](const std::shared_ptr msg) { + this->cb(EventType(msg)); + }, options); + + node_interfaces_ = node_interfaces; } } @@ -320,11 +331,7 @@ class Subscriber { if (!topic_.empty()) { - if (node_raw_ != nullptr) { - subscribe(node_raw_, topic_, qos_, options_); - } else if (node_shared_ != nullptr) { - subscribe(node_shared_, topic_, qos_, options_); - } + subscribe(node_interfaces_, topic_, qos_, options_); } } @@ -372,8 +379,7 @@ class Subscriber typename rclcpp::Subscription::SharedPtr sub_; - NodePtr node_shared_; - NodeType * node_raw_ {nullptr}; + RequiredInterfaces node_interfaces_; std::string topic_; rmw_qos_profile_t qos_; diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 88d5ab2..cd5c1a1 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -272,7 +272,7 @@ TEST(Subscriber, lifecycle) { auto node = std::make_shared("test_node"); Helper h; - message_filters::Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); pub->on_activate(); From fdcc60158a5466f170fb4fb2eade40d1e936304d Mon Sep 17 00:00:00 2001 From: Dominik Authaler Date: Mon, 12 Feb 2024 18:54:50 +0000 Subject: [PATCH 2/5] refactoring to keep old API Signed-off-by: Dominik Authaler --- include/message_filters/subscriber.h | 39 +++++++++++++++++++++++----- test/test_subscriber.cpp | 2 +- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 98375d0..a05d6c0 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -48,15 +48,28 @@ namespace message_filters { +/// Utility struct for highlighting deprecated template parameters +struct DeprecatedTemplateParameter {}; + +template class SubscriberBase { public: + [[deprecated]] typedef std::shared_ptr NodePtr; + using NodeParametersInterface = rclcpp::node_interfaces::NodeParametersInterface; using NodeTopicsInterface = rclcpp::node_interfaces::NodeTopicsInterface; using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + SubscriberBase() { + if constexpr (not std::is_same_v) { + // TODO: deprecation warning, similar to static_assert but only throwing a warning + } + } + virtual ~SubscriberBase() = default; + /** * \brief Subscribe to a topic. * @@ -95,7 +108,8 @@ class SubscriberBase virtual void unsubscribe() = 0; }; -using SubscriberBasePtr = std::shared_ptr; +template +using SubscriberBasePtr = std::shared_ptr>; /** * \brief ROS subscription filter. @@ -135,15 +149,20 @@ struct message_type template using message_type_t = typename message_type::type; -template -class Subscriber -: public SubscriberBase +template +class Subscriber +: public SubscriberBase , public SimpleFilter> { public: typedef message_type_t MessageType; typedef MessageEvent EventType; + // Note: can be removed once the deprecated template parameter NodeType is removed + using RequiredInterfaces = typename SubscriberBase::RequiredInterfaces; + using NodeParametersInterface = typename SubscriberBase::NodeParametersInterface; + using NodeTopicsInterface = typename SubscriberBase::NodeTopicsInterface; + /** * \brief Constructor for rclcpp::Node / rclcpp_lifecycle::LifecycleNode. * @@ -154,6 +173,10 @@ class Subscriber Subscriber(RequiredInterfaces node_interfaces, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) { + if constexpr (not std::is_same_v) { + // TODO: deprecation warning, similar to static_assert but only throwing a warning + } + subscribe(node_interfaces, topic, qos); } @@ -192,6 +215,10 @@ class Subscriber const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) { + if constexpr (not std::is_same_v) { + // TODO: deprecation warning, similar to static_assert but only throwing a warning + } + subscribe(node_interfaces, topic, qos, options); } @@ -310,8 +337,8 @@ class Subscriber // Note: temporary variables are solely needed because the involved submodules are inconsistent about the passing // of shared_ptr. While the node_interfaces return by value, the create_subscription function expects the // shared_ptr to be passed via reference... - auto parameters_interface = node_interfaces.get(); - auto topics_interface = node_interfaces.get(); + auto parameters_interface = node_interfaces.template get(); + auto topics_interface = node_interfaces.template get(); sub_ = rclcpp::create_subscription(parameters_interface, topics_interface, diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index cd5c1a1..88d5ab2 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -272,7 +272,7 @@ TEST(Subscriber, lifecycle) { auto node = std::make_shared("test_node"); Helper h; - message_filters::Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); pub->on_activate(); From 0a98ee94d5b969389bfaf0a14fd9a356a579fd92 Mon Sep 17 00:00:00 2001 From: Dominik Authaler Date: Tue, 13 Feb 2024 06:29:14 +0000 Subject: [PATCH 3/5] readded previously missing virtual functions of SubscriberBase for API compatibility Signed-off-by: Dominik Authaler --- include/message_filters/subscriber.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index a05d6c0..6cda5d0 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -70,6 +70,18 @@ class SubscriberBase virtual ~SubscriberBase() = default; + /** + * \brief Subscribe to a topic (deprecated in favor of NodeInterfaces interface). + */ + [[deprecated]] virtual void subscribe(NodePtr /*node*/, const std::string& /*topic*/, + const rmw_qos_profile_t /*qos*/ = rmw_qos_profile_default) {}; + + /** + * \brief Subscribe to a topic (deprecated in favor of NodeInterfaces interface). + */ + [[deprecated]] virtual void subscribe(NodeType * /*node*/, const std::string& /*topic*/, + const rmw_qos_profile_t /*qos*/ = rmw_qos_profile_default) {}; + /** * \brief Subscribe to a topic. * From d7cff5b067761cb2d050dfb7a3b0ea45e5952f99 Mon Sep 17 00:00:00 2001 From: Dominik Authaler Date: Tue, 13 Feb 2024 12:36:30 +0000 Subject: [PATCH 4/5] deprecation warnings for second template parameter Co-authored-by: Jonas Otto Signed-off-by: Dominik Authaler --- include/message_filters/subscriber.h | 45 ++++++++++++++++++++-------- 1 file changed, 32 insertions(+), 13 deletions(-) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 6cda5d0..ac4e633 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -62,11 +62,12 @@ class SubscriberBase using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; - SubscriberBase() { - if constexpr (not std::is_same_v) { - // TODO: deprecation warning, similar to static_assert but only throwing a warning - } - } + template{}, int> = 0> + SubscriberBase() {} + + template{}, int> = 0> + [[deprecated("Template parameter of SubscriberBase class has been deprecated and will be removed in future releases")]] + SubscriberBase() {} virtual ~SubscriberBase() = default; @@ -175,6 +176,30 @@ class Subscriber using NodeParametersInterface = typename SubscriberBase::NodeParametersInterface; using NodeTopicsInterface = typename SubscriberBase::NodeTopicsInterface; + /** + * \brief Solely for highlighting deprecated template parameters with a compiler warning + */ + template{}, int> = 0> + [[deprecated("Second template parameter of Subscriber class is deprecated and will be removed in a future release")]] + Subscriber(RequiredInterfaces node_interfaces, + const std::string& topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) + { + subscribe(node_interfaces, topic, qos, options); + } + + /** + * \brief Solely for highlighting deprecated template parameters with a compiler warning + */ + template{}, int> = 0> + [[deprecated("Second template parameter of Subscriber class is deprecated and will be removed in a future release")]] + Subscriber(RequiredInterfaces node_interfaces, const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) + { + subscribe(node_interfaces, topic, qos); + } + /** * \brief Constructor for rclcpp::Node / rclcpp_lifecycle::LifecycleNode. * @@ -182,13 +207,10 @@ class Subscriber * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ + template{}, int> = 0> Subscriber(RequiredInterfaces node_interfaces, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) { - if constexpr (not std::is_same_v) { - // TODO: deprecation warning, similar to static_assert but only throwing a warning - } - subscribe(node_interfaces, topic, qos); } @@ -222,15 +244,12 @@ class Subscriber * \param qos The rmw qos profile to use to subscribe. * \param options The subscription options to use to subscribe. */ + template{}, int> = 0> Subscriber(RequiredInterfaces node_interfaces, const std::string& topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) { - if constexpr (not std::is_same_v) { - // TODO: deprecation warning, similar to static_assert but only throwing a warning - } - subscribe(node_interfaces, topic, qos, options); } From 43e56d7f2ec9ec1d0273812cbf7c5a751a68d4fc Mon Sep 17 00:00:00 2001 From: Dominik Authaler Date: Sun, 18 Feb 2024 13:43:30 +0000 Subject: [PATCH 5/5] added test showcasing subscriber usage with node interfaces Signed-off-by: Dominik Authaler --- test/test_subscriber.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 88d5ab2..10339a2 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -287,6 +287,31 @@ TEST(Subscriber, lifecycle) ASSERT_GT(h.count_, 0); } +TEST(Subscriber, node_interfaces) +{ + auto node = std::make_shared("test_node"); + Helper h; + + // disassemble node into relevant interfaces + using NodeParametersInterface = rclcpp::node_interfaces::NodeParametersInterface; + using NodeTopicsInterface = rclcpp::node_interfaces::NodeTopicsInterface; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + + message_filters::Subscriber sub(RequiredInterfaces(*node), "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("test_topic", 10); + pub->on_activate(); + rclcpp::Clock ros_clock; + auto start = ros_clock.now(); + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { + pub->publish(Msg()); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + } + + ASSERT_GT(h.count_, 0); +} + int main(int argc, char **argv) {