diff --git a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp index ef6715f..7b8aaee 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -36,12 +36,13 @@ #include #include -#include // NOLINT +#include #include #include #include #include "point_cloud_transport/visibility_control.hpp" +#include "point_cloud_transport/exception.hpp" namespace point_cloud_transport { @@ -60,7 +61,9 @@ namespace point_cloud_transport /// The output connection for the SubscriberFilter object is the same signature as for rclcpp /// subscription callbacks. /// -class SubscriberFilter : public message_filters::SimpleFilter +class SubscriberFilter + : public message_filters::SubscriberBase, + public message_filters::SimpleFilter { public: /// @@ -89,7 +92,9 @@ class SubscriberFilter : public message_filters::SimpleFilter node_interfaces, const std::string & base_topic, - const std::string & transport); + const std::string & transport, + rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); //! Empty constructor, use subscribe() to subscribe to a topic POINT_CLOUD_TRANSPORT_PUBLIC @@ -140,9 +145,14 @@ class SubscriberFilter : public message_filters::SimpleFilter + void subscribe( + rclcpp::node_interfaces::NodeInterfaces< + NodeParametersInterface, + NodeTopicsInterface>, + const std::string &, + const rclcpp::QoS &) override + { + throw point_cloud_transport::Exception("Not implemented"); + } + + //! Must override parent message_filters::SubscriberBase method + // where RequiredInterfaces are just + void subscribe( + rclcpp::node_interfaces::NodeInterfaces< + NodeParametersInterface, + NodeTopicsInterface>, + const std::string &, + const rclcpp::QoS &, + rclcpp::SubscriptionOptions) override + { + throw point_cloud_transport::Exception("Not implemented"); + } + void cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & m) { signalMessage(m); } Subscriber sub_; + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface, + rclcpp::node_interfaces::NodeLoggingInterface> node_interfaces_; + + std::string topic_; + std::string transport_; + rclcpp::QoS qos_ = rclcpp::SystemDefaultsQoS(); + rclcpp::SubscriptionOptions options_; }; } // namespace point_cloud_transport diff --git a/point_cloud_transport/src/subscriber_filter.cpp b/point_cloud_transport/src/subscriber_filter.cpp index 674c866..f69d606 100644 --- a/point_cloud_transport/src/subscriber_filter.cpp +++ b/point_cloud_transport/src/subscriber_filter.cpp @@ -28,6 +28,7 @@ #include "point_cloud_transport/subscriber_filter.hpp" +#include #include #include @@ -40,9 +41,11 @@ SubscriberFilter::SubscriberFilter( rclcpp::node_interfaces::NodeTopicsInterface, rclcpp::node_interfaces::NodeLoggingInterface> node_interfaces, const std::string & base_topic, - const std::string & transport) + const std::string & transport, + rclcpp::QoS custom_qos, + rclcpp::SubscriptionOptions options) { - subscribe(node_interfaces, base_topic, transport, rclcpp::SystemDefaultsQoS()); + subscribe(node_interfaces, base_topic, transport, custom_qos, options); } SubscriberFilter::SubscriberFilter() @@ -61,13 +64,8 @@ void SubscriberFilter::subscribe( rmw_qos_profile_t custom_qos, rclcpp::SubscriptionOptions options) { - unsubscribe(); - sub_ = point_cloud_transport::create_subscription( - *node, base_topic, - std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), - transport, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos), - options); + subscribe(*node, base_topic, transport, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos), options); } void SubscriberFilter::subscribe( @@ -101,6 +99,27 @@ void SubscriberFilter::subscribe( node_interfaces, base_topic, std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), transport, custom_qos, options); + node_interfaces_ = node_interfaces; + topic_ = base_topic; + transport_ = transport; + qos_ = custom_qos; + options_ = options; +} + +void SubscriberFilter::subscribe() +{ + unsubscribe(); + if (!topic_.empty()) { + sub_ = point_cloud_transport::create_subscription( + node_interfaces_, topic_, + std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), + transport_, qos_, options_); + } else { + RCLCPP_ERROR( + node_interfaces_.get_node_logging_interface()->get_logger(), + "Cannot re-subscribe: the subscriber filter must be initialized first." + ); + } } void SubscriberFilter::unsubscribe() @@ -110,7 +129,7 @@ void SubscriberFilter::unsubscribe() std::string SubscriberFilter::getTopic() const { - return sub_.getTopic(); + return this->topic_; } uint32_t SubscriberFilter::getNumPublishers() const @@ -120,7 +139,7 @@ uint32_t SubscriberFilter::getNumPublishers() const std::string SubscriberFilter::getTransport() const { - return sub_.getTransport(); + return this->transport_; } const Subscriber & SubscriberFilter::getSubscriber() const