From 617c2ce1ea9e0ce8d5e5ea570f8049df725eedbc Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Mon, 17 May 2021 15:08:01 -0700 Subject: [PATCH 1/3] Allow SubscriptionOptions in subscriber Signed-off-by: Audrow Nash --- include/message_filters/subscriber.h | 124 +++++++++++++++++++++++++-- 1 file changed, 118 insertions(+), 6 deletions(-) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 5adebf5..a21c06d 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -35,6 +35,8 @@ #ifndef MESSAGE_FILTERS__SUBSCRIBER_H_ #define MESSAGE_FILTERS__SUBSCRIBER_H_ +#include + #include #include "message_filters/connection.h" @@ -68,6 +70,47 @@ class SubscriberBase * \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; + + /** + * \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( + rclcpp::Node::SharedPtr 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 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, + rclcpp::SubscriptionOptions options) + { + (void) options; + throw std::runtime_error("SubscriberBase::subscribe with four arguments has not been overridden"); + this->subscribe(node, topic, qos); + } + /** * \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. */ @@ -123,6 +166,34 @@ class Subscriber : public SubscriberBase, public SimpleFilter subscribe(node, topic, qos); } + /** + * \brief Constructor + * + * See the rclcpp::Node::subscribe() variants for more information on the parameters + * + * \param node The rclcpp::Node::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. + */ + Subscriber( + rclcpp::Node::SharedPtr node, + const std::string& topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) + { + subscribe(node, topic, qos, options); + } + + Subscriber( + rclcpp::Node* node, + const std::string& topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) + { + subscribe(node, topic, qos, options); + } + /** * \brief Empty constructor, use subscribe() to subscribe to a topic */ @@ -144,9 +215,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter */ void subscribe(rclcpp::Node::SharedPtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) { - subscribe(node.get(), topic, qos); - node_raw_ = nullptr; - node_shared_ = node; + subscribe(node, topic, qos, rclcpp::SubscriptionOptions()); } /** @@ -160,6 +229,47 @@ class Subscriber : public SubscriberBase, public SimpleFilter */ // 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) + { + subscribe(node, topic, qos, rclcpp::SubscriptionOptions()); + } + + /** + * \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 The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + void subscribe( + rclcpp::Node::SharedPtr node, + const std::string& topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) + { + subscribe(node.get(), topic, qos, options); + node_raw_ = nullptr; + node_shared_ = node; + } + + /** + * \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 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( + rclcpp::Node * node, + const std::string& topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) { unsubscribe(); @@ -169,10 +279,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 +297,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 +353,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter std::string topic_; rmw_qos_profile_t qos_; + rclcpp::SubscriptionOptions options_; }; } // namespace message_filters From 7481d277938f6032fb4e64ff7d14465677bb9292 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 18 May 2021 08:39:12 -0700 Subject: [PATCH 2/3] Log warning instead of throwing error Signed-off-by: Audrow Nash --- include/message_filters/subscriber.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index a21c06d..05eef82 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -107,7 +107,7 @@ class SubscriberBase rclcpp::SubscriptionOptions options) { (void) options; - throw std::runtime_error("SubscriberBase::subscribe with four arguments has not been overridden"); + RCLCPP_WARN(node->get_logger(), "SubscriberBase::subscribe with four arguments has not been overridden"); this->subscribe(node, topic, qos); } From 667f1d83cc0faaf95d674db0b56c9187b4ccf0e8 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 18 May 2021 12:55:12 -0700 Subject: [PATCH 3/3] Log error instead of warning Signed-off-by: Audrow Nash --- include/message_filters/subscriber.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 05eef82..3764ebd 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -107,7 +107,9 @@ class SubscriberBase rclcpp::SubscriptionOptions options) { (void) options; - RCLCPP_WARN(node->get_logger(), "SubscriberBase::subscribe with four arguments has not been overridden"); + RCLCPP_ERROR( + node->get_logger(), + "SubscriberBase::subscribe with four arguments has not been overridden"); this->subscribe(node, topic, qos); }