From 6ce2706baef9bc4b5c115db8c7aa71ca10bcf064 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 28 Apr 2021 15:16:32 -0700 Subject: [PATCH 1/2] Expose subscription options Signed-off-by: Audrow Nash --- include/message_filters/subscriber.h | 67 +++++++++++++++++++++------- test/test_subscriber.cpp | 21 +++++---- 2 files changed, 64 insertions(+), 24 deletions(-) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 5adebf56..d10f8d64 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -43,6 +43,7 @@ namespace message_filters { +template > class SubscriberBase { public: @@ -56,7 +57,12 @@ class SubscriberBase * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - virtual void subscribe(rclcpp::Node::SharedPtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; + virtual void subscribe( + rclcpp::Node::SharedPtr node, + const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptionsWithAllocator options = + rclcpp::SubscriptionOptionsWithAllocator()) = 0; /** * \brief Subscribe to a topic. @@ -67,7 +73,12 @@ class SubscriberBase * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - virtual void subscribe(rclcpp::Node * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; + virtual void subscribe( + rclcpp::Node * node, + const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptionsWithAllocator options = + rclcpp::SubscriptionOptionsWithAllocator()) = 0; /** * \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. */ @@ -77,7 +88,8 @@ class SubscriberBase */ virtual void unsubscribe() = 0; }; -typedef std::shared_ptr SubscriberBasePtr; +template > +using SubscriberBasePtr = std::shared_ptr>; /** * \brief ROS subscription filter. @@ -98,8 +110,11 @@ typedef std::shared_ptr SubscriberBasePtr; void callback(const std::shared_ptr&); \endverbatim */ -template -class Subscriber : public SubscriberBase, public SimpleFilter +template< + class M, + typename AllocatorT=std::allocator +> +class Subscriber : public SubscriberBase, public SimpleFilter { public: typedef MessageEvent EventType; @@ -113,14 +128,24 @@ class Subscriber : public SubscriberBase, public SimpleFilter * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - Subscriber(rclcpp::Node::SharedPtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + Subscriber( + rclcpp::Node::SharedPtr node, + const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptionsWithAllocator options = + rclcpp::SubscriptionOptionsWithAllocator()) { - subscribe(node, topic, qos); + subscribe(node, topic, qos, options); } - Subscriber(rclcpp::Node* node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + Subscriber( + rclcpp::Node* node, + const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptionsWithAllocator options = + rclcpp::SubscriptionOptionsWithAllocator()) { - subscribe(node, topic, qos); + subscribe(node, topic, qos, options); } /** @@ -142,9 +167,14 @@ class Subscriber : public SubscriberBase, public SimpleFilter * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - void subscribe(rclcpp::Node::SharedPtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + void subscribe( + rclcpp::Node::SharedPtr node, + const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptionsWithAllocator options = + rclcpp::SubscriptionOptionsWithAllocator()) { - subscribe(node.get(), topic, qos); + subscribe(node.get(), topic, qos, options); node_raw_ = nullptr; node_shared_ = node; } @@ -159,7 +189,12 @@ class Subscriber : public SubscriberBase, public SimpleFilter * \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(rclcpp::Node * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + void subscribe( + rclcpp::Node * node, + const std::string& topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptionsWithAllocator options = + rclcpp::SubscriptionOptionsWithAllocator()) { unsubscribe(); @@ -169,10 +204,11 @@ class Subscriber : public SubscriberBase, public SimpleFilter rclcpp::QoS rclcpp_qos(rclcpp::QoSInitialization::from_rmw(qos)); rclcpp_qos.get_rmw_qos_profile() = qos; qos_ = qos; + options_ = options; sub_ = node->create_subscription(topic, rclcpp_qos, [this](std::shared_ptr msg) { this->cb(EventType(msg)); - }); + }, options); node_raw_ = node; } @@ -186,9 +222,9 @@ class Subscriber : public SubscriberBase, public SimpleFilter if (!topic_.empty()) { if (node_raw_ != nullptr) { - subscribe(node_raw_, topic_, qos_); + subscribe(node_raw_, topic_, qos_, options_); } else if (node_shared_ != nullptr) { - subscribe(node_shared_, topic_, qos_); + subscribe(node_shared_, topic_, qos_, options_); } } } @@ -242,6 +278,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter std::string topic_; rmw_qos_profile_t qos_; + rclcpp::SubscriptionOptionsWithAllocator options_; }; } // namespace message_filters diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 79e5e35a..22144013 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -44,6 +44,9 @@ typedef sensor_msgs::msg::Imu Msg; typedef std::shared_ptr MsgConstPtr; typedef std::shared_ptr MsgPtr; +auto g_qos_profile = rmw_qos_profile_default; +auto g_options = rclcpp::SubscriptionOptionsWithAllocator>(); + class Helper { public: @@ -63,7 +66,7 @@ TEST(Subscriber, simple) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic"); + Subscriber sub(node, "test_topic", g_qos_profile, g_options); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; @@ -82,7 +85,7 @@ TEST(Subscriber, simple_raw) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node.get(), "test_topic"); + Subscriber sub(node.get(), "test_topic", g_qos_profile, g_options); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; @@ -101,7 +104,7 @@ TEST(Subscriber, subUnsubSub) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic"); + Subscriber sub(node, "test_topic", g_qos_profile, g_options); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); @@ -124,7 +127,7 @@ TEST(Subscriber, subUnsubSub_raw) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node.get(), "test_topic"); + Subscriber sub(node.get(), "test_topic", g_qos_profile, g_options); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); @@ -147,7 +150,7 @@ TEST(Subscriber, switchRawAndShared) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic"); + Subscriber sub(node, "test_topic", g_qos_profile, g_options); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic2", 10); @@ -171,7 +174,7 @@ TEST(Subscriber, subInChain) auto node = std::make_shared("test_node"); Helper h; Chain c; - c.addFilter(std::make_shared >(node, "test_topic")); + c.addFilter(std::make_shared >(node, "test_topic", g_qos_profile, g_options)); c.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); @@ -211,7 +214,7 @@ TEST(Subscriber, singleNonConstCallback) { auto node = std::make_shared("test_node"); NonConstHelper h; - Subscriber sub(node, "test_topic"); + Subscriber sub(node, "test_topic", g_qos_profile, g_options); sub.registerCallback(&NonConstHelper::cb, &h); auto pub = node->create_publisher("test_topic", 10); Msg msg; @@ -228,7 +231,7 @@ TEST(Subscriber, multipleNonConstCallbacksFilterSubscriber) { auto node = std::make_shared("test_node"); NonConstHelper h, h2; - Subscriber sub(node, "test_topic"); + Subscriber sub(node, "test_topic", g_qos_profile, g_options); sub.registerCallback(&NonConstHelper::cb, &h); sub.registerCallback(&NonConstHelper::cb, &h2); auto pub = node->create_publisher("test_topic", 10); @@ -249,7 +252,7 @@ TEST(Subscriber, multipleCallbacksSomeFilterSomeDirect) { auto node = std::make_shared("test_node"); NonConstHelper h, h2; - Subscriber sub(node, "test_topic"); + Subscriber sub(node, "test_topic", g_qos_profile, g_options); sub.registerCallback(&NonConstHelper::cb, &h); auto sub2 = node->create_subscription( "test_topic", 10, std::bind(&NonConstHelper::cb, &h2, std::placeholders::_1)); From 167de4f4f6df99250eb05a4e0e0c3fcb6ed9347b Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Thu, 29 Apr 2021 13:46:21 -0700 Subject: [PATCH 2/2] Revert test_subscriber.cpp Signed-off-by: Audrow Nash --- test/test_subscriber.cpp | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 22144013..79e5e35a 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -44,9 +44,6 @@ typedef sensor_msgs::msg::Imu Msg; typedef std::shared_ptr MsgConstPtr; typedef std::shared_ptr MsgPtr; -auto g_qos_profile = rmw_qos_profile_default; -auto g_options = rclcpp::SubscriptionOptionsWithAllocator>(); - class Helper { public: @@ -66,7 +63,7 @@ TEST(Subscriber, simple) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic", g_qos_profile, g_options); + Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; @@ -85,7 +82,7 @@ TEST(Subscriber, simple_raw) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node.get(), "test_topic", g_qos_profile, g_options); + Subscriber sub(node.get(), "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; @@ -104,7 +101,7 @@ TEST(Subscriber, subUnsubSub) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic", g_qos_profile, g_options); + Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); @@ -127,7 +124,7 @@ TEST(Subscriber, subUnsubSub_raw) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node.get(), "test_topic", g_qos_profile, g_options); + Subscriber sub(node.get(), "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); @@ -150,7 +147,7 @@ TEST(Subscriber, switchRawAndShared) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic", g_qos_profile, g_options); + Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic2", 10); @@ -174,7 +171,7 @@ TEST(Subscriber, subInChain) auto node = std::make_shared("test_node"); Helper h; Chain c; - c.addFilter(std::make_shared >(node, "test_topic", g_qos_profile, g_options)); + c.addFilter(std::make_shared >(node, "test_topic")); c.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); @@ -214,7 +211,7 @@ TEST(Subscriber, singleNonConstCallback) { auto node = std::make_shared("test_node"); NonConstHelper h; - Subscriber sub(node, "test_topic", g_qos_profile, g_options); + Subscriber sub(node, "test_topic"); sub.registerCallback(&NonConstHelper::cb, &h); auto pub = node->create_publisher("test_topic", 10); Msg msg; @@ -231,7 +228,7 @@ TEST(Subscriber, multipleNonConstCallbacksFilterSubscriber) { auto node = std::make_shared("test_node"); NonConstHelper h, h2; - Subscriber sub(node, "test_topic", g_qos_profile, g_options); + Subscriber sub(node, "test_topic"); sub.registerCallback(&NonConstHelper::cb, &h); sub.registerCallback(&NonConstHelper::cb, &h2); auto pub = node->create_publisher("test_topic", 10); @@ -252,7 +249,7 @@ TEST(Subscriber, multipleCallbacksSomeFilterSomeDirect) { auto node = std::make_shared("test_node"); NonConstHelper h, h2; - Subscriber sub(node, "test_topic", g_qos_profile, g_options); + Subscriber sub(node, "test_topic"); sub.registerCallback(&NonConstHelper::cb, &h); auto sub2 = node->create_subscription( "test_topic", 10, std::bind(&NonConstHelper::cb, &h2, std::placeholders::_1));