Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Expose Subscription Options - V2 #56

Merged
merged 3 commits into from
May 19, 2021
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
126 changes: 120 additions & 6 deletions include/message_filters/subscriber.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@
#ifndef MESSAGE_FILTERS__SUBSCRIBER_H_
#define MESSAGE_FILTERS__SUBSCRIBER_H_

#include <stdexcept>

#include <rclcpp/rclcpp.hpp>

#include "message_filters/connection.h"
Expand Down Expand Up @@ -68,6 +70,49 @@ 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;
RCLCPP_ERROR(
node->get_logger(),
"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.
*/
Expand Down Expand Up @@ -123,6 +168,34 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
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
*/
Expand All @@ -144,9 +217,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
*/
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());
}

/**
Expand All @@ -160,6 +231,47 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
*/
// 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();

Expand All @@ -169,10 +281,11 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
rclcpp::QoS rclcpp_qos(rclcpp::QoSInitialization::from_rmw(qos));
rclcpp_qos.get_rmw_qos_profile() = qos;
qos_ = qos;
options_ = options;
sub_ = node->create_subscription<M>(topic, rclcpp_qos,
[this](std::shared_ptr<M const> msg) {
this->cb(EventType(msg));
});
}, options);

node_raw_ = node;
}
Expand All @@ -186,9 +299,9 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
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_);
}
}
}
Expand Down Expand Up @@ -242,6 +355,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>

std::string topic_;
rmw_qos_profile_t qos_;
rclcpp::SubscriptionOptions options_;
};

} // namespace message_filters
Expand Down