From 9a396aea2b4002c654b4a5a2509801adc247feb3 Mon Sep 17 00:00:00 2001 From: elsayedelsheikh Date: Thu, 26 Jun 2025 18:42:39 +0000 Subject: [PATCH 01/12] initial thoughts Signed-off-by: elsayedelsheikh --- point_cloud_transport/CMakeLists.txt | 1 + .../full_subscriber_filter.hpp | 165 ++++++++++++++++++ .../src/full_subscriber_filter.cpp | 86 +++++++++ 3 files changed, 252 insertions(+) create mode 100644 point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp create mode 100644 point_cloud_transport/src/full_subscriber_filter.cpp diff --git a/point_cloud_transport/CMakeLists.txt b/point_cloud_transport/CMakeLists.txt index 56134df..eb30032 100644 --- a/point_cloud_transport/CMakeLists.txt +++ b/point_cloud_transport/CMakeLists.txt @@ -32,6 +32,7 @@ add_library(${PROJECT_NAME} src/raw_subscriber.cpp src/single_subscriber_publisher.cpp src/subscriber_filter.cpp + src/full_subscriber_filter.cpp src/subscriber.cpp ) diff --git a/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp b/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp new file mode 100644 index 0000000..2f9d8bf --- /dev/null +++ b/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp @@ -0,0 +1,165 @@ +#ifndef POINT_CLOUD_TRANSPORT__FULL_SUBSCRIBER_FILTER_HPP_ +#define POINT_CLOUD_TRANSPORT__FULL_SUBSCRIBER_FILTER_HPP_ + +#include +#include + +#include +#include +#include + +#include +#include +#include "point_cloud_transport/visibility_control.hpp" + +namespace point_cloud_transport +{ + +/// +/// PointCloud2 subscription filter. +/// +/// This class wraps Subscriber as a "filter" compatible with the message_filters +/// package. It acts as a highest-level filter, simply passing messages from a point cloud +/// transport subscription through to the filters which have connected to it. +/// +/// When this object is destroyed it will unsubscribe from the ROS subscription. +/// +/// FullSubscriberFilter has no input connection. +/// +/// The output connection for the FullSubscriberFilter object is the same signature as for rclcpp +/// subscription callbacks. +/// +class FullSubscriberFilter + : public message_filters::SubscriberBase, + public message_filters::SimpleFilter +{ +public: + POINT_CLOUD_TRANSPORT_PUBLIC + FullSubscriberFilter( + std::shared_ptr> node_interfaces, + const std::string & base_topic, + const std::string & transport, + const rclcpp::QoS & custom_qos, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + + template + POINT_CLOUD_TRANSPORT_PUBLIC + FullSubscriberFilter( + NodeT node, + const std::string & base_topic, + const std::string & transport, + const rclcpp::QoS & custom_qos, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) + { + auto node_interfaces = std::make_shared>(*node); + subscribe(node_interfaces, base_topic, transport, custom_qos, options); + } + + //! Empty constructor, use subscribe() to subscribe to a topic + POINT_CLOUD_TRANSPORT_PUBLIC + FullSubscriberFilter(); + + POINT_CLOUD_TRANSPORT_PUBLIC + ~FullSubscriberFilter(); + + /// + /// \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 base_topic The topic to subscribe to. + /// \param transport The transport hint to pass along + /// \param custom_qos Custom quality of service + /// \param options Subscriber options + /// + POINT_CLOUD_TRANSPORT_PUBLIC + void subscribe( + std::shared_ptr> node_interfaces, + const std::string & base_topic, + const std::string & transport, + const rclcpp::QoS & custom_qos, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + + template + POINT_CLOUD_TRANSPORT_PUBLIC + void subscribe( + NodeT node, + const std::string & base_topic, + const std::string & transport, + const rclcpp::QoS & custom_qos, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) + { + auto node_interfaces = std::make_shared>(*node); + subscribe(node_interfaces, base_topic, transport, custom_qos, options); + } + + void subscribe( + RequiredInterfaces /*node_interfaces*/, const std::string & /*topic*/, + const rclcpp::QoS & /*qos*/) override + {} + void subscribe( + RequiredInterfaces /*node_interfaces*/, + const std::string & /*topic*/, + const rclcpp::QoS & /*qos*/, + rclcpp::SubscriptionOptions /*options*/) override + {} + + //! Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. + POINT_CLOUD_TRANSPORT_PUBLIC + void subscribe() override; + + //! Force immediate unsubscription of this subscriber from its topic + POINT_CLOUD_TRANSPORT_PUBLIC + void unsubscribe() override; + + POINT_CLOUD_TRANSPORT_PUBLIC + std::string getTopic() const; + + //! Returns the number of publishers this subscriber is connected to. + POINT_CLOUD_TRANSPORT_PUBLIC + uint32_t getNumPublishers() const; + + //! Returns the name of the transport being used. + POINT_CLOUD_TRANSPORT_PUBLIC + std::string getTransport() const; + + //! Returns the internal point_cloud_transport::Subscriber object. + POINT_CLOUD_TRANSPORT_PUBLIC + const Subscriber & getSubscriber() const; + +private: + void cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & m) + { + this->signalMessage(m); + } + + Subscriber sub_; + std::shared_ptr> node_interfaces_; + + std::string topic_; + std::string transport_; + rclcpp::QoS qos_ = rclcpp::SystemDefaultsQoS(); + rclcpp::SubscriptionOptions options_; +}; + +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__FULL_SUBSCRIBER_FILTER_HPP_ diff --git a/point_cloud_transport/src/full_subscriber_filter.cpp b/point_cloud_transport/src/full_subscriber_filter.cpp new file mode 100644 index 0000000..a412d3d --- /dev/null +++ b/point_cloud_transport/src/full_subscriber_filter.cpp @@ -0,0 +1,86 @@ +#include "point_cloud_transport/full_subscriber_filter.hpp" + +#include +#include + +namespace point_cloud_transport +{ +FullSubscriberFilter::FullSubscriberFilter( + std::shared_ptr> node_interfaces, + const std::string & base_topic, + const std::string & transport, + const rclcpp::QoS & custom_qos, + rclcpp::SubscriptionOptions options) +{ + subscribe(node_interfaces, base_topic, transport, custom_qos, options); +} + +FullSubscriberFilter::FullSubscriberFilter() +{ +} + +FullSubscriberFilter::~FullSubscriberFilter() +{ + unsubscribe(); +} + +void FullSubscriberFilter::subscribe( + std::shared_ptr> node_interfaces, + const std::string & base_topic, + const std::string & transport, + const rclcpp::QoS & custom_qos, + rclcpp::SubscriptionOptions options) +{ + unsubscribe(); + sub_ = point_cloud_transport::create_subscription( + node_interfaces, base_topic, + std::bind(&FullSubscriberFilter::cb, this, std::placeholders::_1), + transport, custom_qos, options); + + this->node_interfaces_ = node_interfaces; + this->topic_ = base_topic; + this->transport_ = transport; + this->qos_ = custom_qos; + this->options_ = options; +} + +void FullSubscriberFilter::subscribe() +{ + if (!topic_.empty()) { + subscribe(node_interfaces_, topic_, transport_, qos_, options_); + } +} + +void FullSubscriberFilter::unsubscribe() +{ + sub_.shutdown(); +} + +std::string FullSubscriberFilter::getTopic() const +{ + return this->topic_; +} + +uint32_t FullSubscriberFilter::getNumPublishers() const +{ + return sub_.getNumPublishers(); +} + +std::string FullSubscriberFilter::getTransport() const +{ + return this->transport_; +} + +const Subscriber & FullSubscriberFilter::getSubscriber() const +{ + return sub_; +} +} // namespace point_cloud_transport From c5203249d1a026dede00292100a7f4c3418c2c88 Mon Sep 17 00:00:00 2001 From: elsayedelsheikh Date: Thu, 26 Jun 2025 19:28:12 +0000 Subject: [PATCH 02/12] Make useful for derived classes Signed-off-by: elsayedelsheikh --- .../point_cloud_transport/subscriber_filter.hpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) 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 5eaa866..990a69d 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -132,8 +132,18 @@ class SubscriberFilter : public message_filters::SimpleFilter Date: Thu, 26 Jun 2025 19:28:33 +0000 Subject: [PATCH 03/12] New subscriberfilter class Signed-off-by: elsayedelsheikh --- .../full_subscriber_filter.hpp | 73 +++++++++++-------- .../src/full_subscriber_filter.cpp | 64 ++++++++-------- 2 files changed, 74 insertions(+), 63 deletions(-) diff --git a/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp b/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp index 2f9d8bf..36b5edf 100644 --- a/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp @@ -1,3 +1,31 @@ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// + #ifndef POINT_CLOUD_TRANSPORT__FULL_SUBSCRIBER_FILTER_HPP_ #define POINT_CLOUD_TRANSPORT__FULL_SUBSCRIBER_FILTER_HPP_ @@ -9,6 +37,7 @@ #include #include +#include #include #include "point_cloud_transport/visibility_control.hpp" @@ -31,7 +60,7 @@ namespace point_cloud_transport /// class FullSubscriberFilter : public message_filters::SubscriberBase, - public message_filters::SimpleFilter + public SubscriberFilter { public: POINT_CLOUD_TRANSPORT_PUBLIC @@ -43,7 +72,7 @@ class FullSubscriberFilter rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces, const std::string & base_topic, const std::string & transport, - const rclcpp::QoS & custom_qos, + rclcpp::QoS custom_qos, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); template @@ -52,7 +81,7 @@ class FullSubscriberFilter NodeT node, const std::string & base_topic, const std::string & transport, - const rclcpp::QoS & custom_qos, + rclcpp::QoS custom_qos, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) { auto node_interfaces = std::make_shared> node_interfaces, const std::string & base_topic, const std::string & transport, - const rclcpp::QoS & custom_qos, - rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + rclcpp::QoS custom_qos, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) override; template POINT_CLOUD_TRANSPORT_PUBLIC @@ -97,7 +128,7 @@ class FullSubscriberFilter NodeT node, const std::string & base_topic, const std::string & transport, - const rclcpp::QoS & custom_qos, + rclcpp::QoS custom_qos, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) { auto node_interfaces = std::make_sharedsignalMessage(m); - } - - Subscriber sub_; std::shared_ptr @@ -13,21 +41,12 @@ FullSubscriberFilter::FullSubscriberFilter( rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces, const std::string & base_topic, const std::string & transport, - const rclcpp::QoS & custom_qos, + rclcpp::QoS custom_qos, rclcpp::SubscriptionOptions options) { subscribe(node_interfaces, base_topic, transport, custom_qos, options); } -FullSubscriberFilter::FullSubscriberFilter() -{ -} - -FullSubscriberFilter::~FullSubscriberFilter() -{ - unsubscribe(); -} - void FullSubscriberFilter::subscribe( std::shared_ptr> node_interfaces, const std::string & base_topic, const std::string & transport, - const rclcpp::QoS & custom_qos, + rclcpp::QoS custom_qos, rclcpp::SubscriptionOptions options) { unsubscribe(); @@ -44,7 +63,7 @@ void FullSubscriberFilter::subscribe( node_interfaces, base_topic, std::bind(&FullSubscriberFilter::cb, this, std::placeholders::_1), transport, custom_qos, options); - + this->node_interfaces_ = node_interfaces; this->topic_ = base_topic; this->transport_ = transport; @@ -61,26 +80,7 @@ void FullSubscriberFilter::subscribe() void FullSubscriberFilter::unsubscribe() { - sub_.shutdown(); -} - -std::string FullSubscriberFilter::getTopic() const -{ - return this->topic_; -} - -uint32_t FullSubscriberFilter::getNumPublishers() const -{ - return sub_.getNumPublishers(); + SubscriberFilter::unsubscribe(); } -std::string FullSubscriberFilter::getTransport() const -{ - return this->transport_; -} - -const Subscriber & FullSubscriberFilter::getSubscriber() const -{ - return sub_; -} } // namespace point_cloud_transport From b77e5e1038873d57a53d1501b4b26f04cc654d6e Mon Sep 17 00:00:00 2001 From: elsayedelsheikh Date: Thu, 26 Jun 2025 19:56:29 +0000 Subject: [PATCH 04/12] make copyright test happy Signed-off-by: elsayedelsheikh --- .../include/point_cloud_transport/full_subscriber_filter.hpp | 1 + point_cloud_transport/src/full_subscriber_filter.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp b/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp index 36b5edf..a53ea95 100644 --- a/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp @@ -1,3 +1,4 @@ +// Copyright (c) 2025, Nobody? // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: diff --git a/point_cloud_transport/src/full_subscriber_filter.cpp b/point_cloud_transport/src/full_subscriber_filter.cpp index 8a5ee15..a892b4d 100644 --- a/point_cloud_transport/src/full_subscriber_filter.cpp +++ b/point_cloud_transport/src/full_subscriber_filter.cpp @@ -1,3 +1,4 @@ +// Copyright (c) 2025, Nobody? // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: From da90eff8cdaf55cfca11cc63846278a11f45da88 Mon Sep 17 00:00:00 2001 From: elsayedelsheikh Date: Fri, 27 Jun 2025 01:25:53 +0300 Subject: [PATCH 05/12] update subscriber_filter Signed-off-by: elsayedelsheikh --- point_cloud_transport/CMakeLists.txt | 1 - .../full_subscriber_filter.hpp | 177 ------------------ .../subscriber_filter.hpp | 86 ++++++++- .../src/full_subscriber_filter.cpp | 87 --------- .../src/subscriber_filter.cpp | 31 ++- 5 files changed, 110 insertions(+), 272 deletions(-) delete mode 100644 point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp delete mode 100644 point_cloud_transport/src/full_subscriber_filter.cpp diff --git a/point_cloud_transport/CMakeLists.txt b/point_cloud_transport/CMakeLists.txt index eb30032..56134df 100644 --- a/point_cloud_transport/CMakeLists.txt +++ b/point_cloud_transport/CMakeLists.txt @@ -32,7 +32,6 @@ add_library(${PROJECT_NAME} src/raw_subscriber.cpp src/single_subscriber_publisher.cpp src/subscriber_filter.cpp - src/full_subscriber_filter.cpp src/subscriber.cpp ) diff --git a/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp b/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp deleted file mode 100644 index a53ea95..0000000 --- a/point_cloud_transport/include/point_cloud_transport/full_subscriber_filter.hpp +++ /dev/null @@ -1,177 +0,0 @@ -// Copyright (c) 2025, Nobody? -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// - -#ifndef POINT_CLOUD_TRANSPORT__FULL_SUBSCRIBER_FILTER_HPP_ -#define POINT_CLOUD_TRANSPORT__FULL_SUBSCRIBER_FILTER_HPP_ - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include "point_cloud_transport/visibility_control.hpp" - -namespace point_cloud_transport -{ - -/// -/// PointCloud2 subscription filter. -/// -/// This class wraps Subscriber as a "filter" compatible with the message_filters -/// package. It acts as a highest-level filter, simply passing messages from a point cloud -/// transport subscription through to the filters which have connected to it. -/// -/// When this object is destroyed it will unsubscribe from the ROS subscription. -/// -/// FullSubscriberFilter has no input connection. -/// -/// The output connection for the FullSubscriberFilter object is the same signature as for rclcpp -/// subscription callbacks. -/// -class FullSubscriberFilter - : public message_filters::SubscriberBase, - public SubscriberFilter -{ -public: - POINT_CLOUD_TRANSPORT_PUBLIC - FullSubscriberFilter( - std::shared_ptr> node_interfaces, - const std::string & base_topic, - const std::string & transport, - rclcpp::QoS custom_qos, - rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); - - template - POINT_CLOUD_TRANSPORT_PUBLIC - FullSubscriberFilter( - NodeT node, - const std::string & base_topic, - const std::string & transport, - rclcpp::QoS custom_qos, - rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) - { - auto node_interfaces = std::make_shared>(*node); - subscribe(node_interfaces, base_topic, transport, custom_qos, options); - } - - //! Empty constructor, use subscribe() to subscribe to a topic - POINT_CLOUD_TRANSPORT_PUBLIC - FullSubscriberFilter() {} - - //! Empty constructor, parent class already calls unsubscribe() - POINT_CLOUD_TRANSPORT_PUBLIC - ~FullSubscriberFilter() {} - - /// - /// \brief Subscribe to a topic. If this Subscriber is already subscribed to a topic, - /// this function will first unsubscribe. - /// \param node_interfaces the ROS node interfaces required for core node functionality, including - /// NodeBaseInterface, NodeParametersInterface, NodeTopicsInterface, and NodeLoggingInterface. - /// \param base_topic The topic to subscribe to. - /// \param transport The transport hint to pass along - /// \param custom_qos Custom quality of service - /// \param options Subscriber options - /// - POINT_CLOUD_TRANSPORT_PUBLIC - void subscribe( - std::shared_ptr> node_interfaces, - const std::string & base_topic, - const std::string & transport, - rclcpp::QoS custom_qos, - rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) override; - - template - POINT_CLOUD_TRANSPORT_PUBLIC - void subscribe( - NodeT node, - const std::string & base_topic, - const std::string & transport, - rclcpp::QoS custom_qos, - rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) - { - auto node_interfaces = std::make_shared>(*node); - subscribe(node_interfaces, base_topic, transport, custom_qos, options); - } - - void subscribe( - RequiredInterfaces /*node_interfaces*/, const std::string & /*topic*/, - const rclcpp::QoS & /*qos*/) override - {} - void subscribe( - RequiredInterfaces /*node_interfaces*/, - const std::string & /*topic*/, - const rclcpp::QoS & /*qos*/, - rclcpp::SubscriptionOptions /*options*/) override - {} - - //! Re-subscribe to a topic. - // Only works if this subscriber has previously been subscribed to a topic. - POINT_CLOUD_TRANSPORT_PUBLIC - void subscribe() override; - - //! Force immediate unsubscription of this subscriber from its topic - POINT_CLOUD_TRANSPORT_PUBLIC - void unsubscribe() override; - -private: - std::shared_ptr> node_interfaces_; - - std::string topic_; - std::string transport_; - rclcpp::QoS qos_ = rclcpp::SystemDefaultsQoS(); - rclcpp::SubscriptionOptions options_; -}; - -} // namespace point_cloud_transport -#endif // POINT_CLOUD_TRANSPORT__FULL_SUBSCRIBER_FILTER_HPP_ 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 990a69d..901b672 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -36,7 +36,7 @@ #include #include -#include // NOLINT +#include #include #include @@ -60,7 +60,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: /// @@ -95,6 +97,35 @@ class SubscriberFilter : public message_filters::SimpleFilter> node_interfaces, + const std::string & base_topic, + const std::string & transport, + rclcpp::QoS custom_qos, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + + template + POINT_CLOUD_TRANSPORT_PUBLIC + SubscriberFilter( + NodeT node, + const std::string & base_topic, + const std::string & transport, + rclcpp::QoS custom_qos, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) + { + auto node_interfaces = std::make_shared>(*node); + subscribe(node_interfaces, base_topic, transport, custom_qos, options); + } + //! Empty constructor, use subscribe() to subscribe to a topic POINT_CLOUD_TRANSPORT_PUBLIC SubscriberFilter(); @@ -143,7 +174,7 @@ class SubscriberFilter : public message_filters::SimpleFilter + POINT_CLOUD_TRANSPORT_PUBLIC + void subscribe( + NodeT node, + const std::string & base_topic, + const std::string & transport, + rclcpp::QoS custom_qos, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) + { + auto node_interfaces = std::make_shared>(*node); + subscribe(node_interfaces, base_topic, transport, custom_qos, options); + } + + //! Re-subscribe to a topic. + // Only works if this subscriber has previously been subscribed to a topic. + POINT_CLOUD_TRANSPORT_PUBLIC + void subscribe() override; + //! Force immediate unsubscription of this subscriber from its topic POINT_CLOUD_TRANSPORT_PUBLIC - void unsubscribe(); + void unsubscribe() override; POINT_CLOUD_TRANSPORT_PUBLIC std::string getTopic() const; @@ -173,13 +226,36 @@ class SubscriberFilter : public message_filters::SimpleFilter> 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/full_subscriber_filter.cpp b/point_cloud_transport/src/full_subscriber_filter.cpp deleted file mode 100644 index a892b4d..0000000 --- a/point_cloud_transport/src/full_subscriber_filter.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright (c) 2025, Nobody? -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// - -#include "point_cloud_transport/full_subscriber_filter.hpp" - -#include -#include - -namespace point_cloud_transport -{ -FullSubscriberFilter::FullSubscriberFilter( - std::shared_ptr> node_interfaces, - const std::string & base_topic, - const std::string & transport, - rclcpp::QoS custom_qos, - rclcpp::SubscriptionOptions options) -{ - subscribe(node_interfaces, base_topic, transport, custom_qos, options); -} - -void FullSubscriberFilter::subscribe( - std::shared_ptr> node_interfaces, - const std::string & base_topic, - const std::string & transport, - rclcpp::QoS custom_qos, - rclcpp::SubscriptionOptions options) -{ - unsubscribe(); - sub_ = point_cloud_transport::create_subscription( - node_interfaces, base_topic, - std::bind(&FullSubscriberFilter::cb, this, std::placeholders::_1), - transport, custom_qos, options); - - this->node_interfaces_ = node_interfaces; - this->topic_ = base_topic; - this->transport_ = transport; - this->qos_ = custom_qos; - this->options_ = options; -} - -void FullSubscriberFilter::subscribe() -{ - if (!topic_.empty()) { - subscribe(node_interfaces_, topic_, transport_, qos_, options_); - } -} - -void FullSubscriberFilter::unsubscribe() -{ - SubscriberFilter::unsubscribe(); -} - -} // namespace point_cloud_transport diff --git a/point_cloud_transport/src/subscriber_filter.cpp b/point_cloud_transport/src/subscriber_filter.cpp index 3fe9564..67c3e6b 100644 --- a/point_cloud_transport/src/subscriber_filter.cpp +++ b/point_cloud_transport/src/subscriber_filter.cpp @@ -45,6 +45,20 @@ SubscriberFilter::SubscriberFilter( subscribe(node_interfaces, base_topic, transport, rclcpp::SystemDefaultsQoS()); } +SubscriberFilter::SubscriberFilter( + std::shared_ptr> node_interfaces, + const std::string & base_topic, + const std::string & transport, + rclcpp::QoS custom_qos, + rclcpp::SubscriptionOptions options) +{ + subscribe(node_interfaces, base_topic, transport, custom_qos, options); +} + SubscriberFilter::SubscriberFilter() { } @@ -106,6 +120,19 @@ void SubscriberFilter::subscribe( node_interfaces, base_topic, std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), transport, custom_qos, options); + // Update class members + this->node_interfaces_ = node_interfaces; + this->topic_ = base_topic; + this->transport_ = transport; + this->qos_ = custom_qos; + this->options_ = options; +} + +void SubscriberFilter::subscribe() +{ + if (node_interfaces_ != nullptr) { + subscribe(node_interfaces_, topic_, transport_, qos_, options_); + } } void SubscriberFilter::unsubscribe() @@ -115,7 +142,7 @@ void SubscriberFilter::unsubscribe() std::string SubscriberFilter::getTopic() const { - return sub_.getTopic(); + return this->topic_; } uint32_t SubscriberFilter::getNumPublishers() const @@ -125,7 +152,7 @@ uint32_t SubscriberFilter::getNumPublishers() const std::string SubscriberFilter::getTransport() const { - return sub_.getTransport(); + return this->transport_; } const Subscriber & SubscriberFilter::getSubscriber() const From dc5077dced27f1f2117df530d8a8a47a86d7e2f2 Mon Sep 17 00:00:00 2001 From: elsayedelsheikh Date: Fri, 27 Jun 2025 14:39:57 +0300 Subject: [PATCH 06/12] Hide methods from users Signed-off-by: elsayedelsheikh --- .../include/point_cloud_transport/subscriber_filter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 901b672..9cbaa08 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -226,6 +226,7 @@ class SubscriberFilter POINT_CLOUD_TRANSPORT_PUBLIC const Subscriber & getSubscriber() const; + private: //! Don't use this method void subscribe( RequiredInterfaces /*node_interfaces*/, const std::string & /*topic*/, @@ -239,7 +240,6 @@ class SubscriberFilter rclcpp::SubscriptionOptions /*options*/) override {} -private: void cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & m) { signalMessage(m); From 34431b30e519372808b847f7941fda019959c4df Mon Sep 17 00:00:00 2001 From: elsayedelsheikh Date: Fri, 27 Jun 2025 14:44:24 +0300 Subject: [PATCH 07/12] Make linters happy Signed-off-by: elsayedelsheikh --- .../include/point_cloud_transport/subscriber_filter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9cbaa08..c850470 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -226,7 +226,7 @@ class SubscriberFilter POINT_CLOUD_TRANSPORT_PUBLIC const Subscriber & getSubscriber() const; - private: +private: //! Don't use this method void subscribe( RequiredInterfaces /*node_interfaces*/, const std::string & /*topic*/, From e4ab91254c0d379b0f29af71448a10551c60cb30 Mon Sep 17 00:00:00 2001 From: elsayedelsheikh Date: Sat, 28 Jun 2025 04:06:00 +0300 Subject: [PATCH 08/12] Feedback Signed-off-by: elsayedelsheikh --- .../subscriber_filter.hpp | 22 +++++-------------- .../src/subscriber_filter.cpp | 12 ---------- 2 files changed, 6 insertions(+), 28 deletions(-) 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 c850470..b4a8fb7 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -87,16 +87,6 @@ class SubscriberFilter { } - POINT_CLOUD_TRANSPORT_PUBLIC - SubscriberFilter( - std::shared_ptr> node_interfaces, - const std::string & base_topic, - const std::string & transport); - POINT_CLOUD_TRANSPORT_PUBLIC SubscriberFilter( std::shared_ptr> node_interfaces, const std::string & base_topic, const std::string & transport, - rclcpp::QoS custom_qos, + rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); - template + template POINT_CLOUD_TRANSPORT_PUBLIC SubscriberFilter( NodeT node, const std::string & base_topic, const std::string & transport, - rclcpp::QoS custom_qos, + rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) { auto node_interfaces = std::make_shared> node_interfaces, const std::string & base_topic, const std::string & transport, - rclcpp::QoS custom_qos, + rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); - template + template POINT_CLOUD_TRANSPORT_PUBLIC void subscribe( NodeT node, const std::string & base_topic, const std::string & transport, - rclcpp::QoS custom_qos, + rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) { auto node_interfaces = std::make_shared> node_interfaces, - const std::string & base_topic, - const std::string & transport) -{ - subscribe(node_interfaces, base_topic, transport, rclcpp::SystemDefaultsQoS()); -} - SubscriberFilter::SubscriberFilter( std::shared_ptr Date: Tue, 1 Jul 2025 14:56:05 +0300 Subject: [PATCH 09/12] Refactor Signed-off-by: ElSayed ElSheikh --- .../subscriber_filter.hpp | 90 +++++++------------ .../src/subscriber_filter.cpp | 27 +----- .../test/test_message_filter.cpp | 28 +++--- 3 files changed, 49 insertions(+), 96 deletions(-) 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 b4a8fb7..49f189b 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -65,28 +65,7 @@ class SubscriberFilter public message_filters::SimpleFilter { public: - /// - /// \brief Constructor - /// \param node The rclcpp node to use to subscribe. - /// \param base_topic The topic to subscribe to. - /// \param queue_size The subscription queue size - /// \param transport The transport hint to pass along - /// - [[deprecated("Use SubscriberFilter(rclcpp::node_interfaces...) instead")]] - POINT_CLOUD_TRANSPORT_PUBLIC - SubscriberFilter( - std::shared_ptr node, const std::string & base_topic, - const std::string & transport) - : SubscriberFilter( - std::make_shared>(*node), - base_topic, transport) - { - } - + [[deprecated("Use SubscriberFilter(NodeT, ...) instead")]] POINT_CLOUD_TRANSPORT_PUBLIC SubscriberFilter( std::shared_ptr> node_interfaces, const std::string & base_topic, - const std::string & transport, - rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), - rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + const std::string & transport); + /// + /// \brief Constructor + /// \param node The node used to create the subscription (either a Node or LifecycleNode). + /// \param base_topic The topic to subscribe to. + /// \param transport The transport hint to pass along + /// \param custom_qos Custom quality of service + /// \param options Subscriber options + /// template POINT_CLOUD_TRANSPORT_PUBLIC SubscriberFilter( @@ -108,12 +93,7 @@ class SubscriberFilter rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) { - auto node_interfaces = std::make_shared>(*node); - subscribe(node_interfaces, base_topic, transport, custom_qos, options); + subscribe(node, base_topic, transport, custom_qos, options); } //! Empty constructor, use subscribe() to subscribe to a topic @@ -122,23 +102,6 @@ class SubscriberFilter POINT_CLOUD_TRANSPORT_PUBLIC ~SubscriberFilter(); - /// - /// \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 base_topic The topic to subscribe to. - /// \param transport The transport hint to pass along - /// \param custom_qos Custom quality of service - /// \param options Subscriber options - /// - [[deprecated("Use subscribe(rclcpp::node_interfaces...) instead")]] - POINT_CLOUD_TRANSPORT_PUBLIC - void subscribe( - std::shared_ptr node, - const std::string & base_topic, - const std::string & transport, - rmw_qos_profile_t custom_qos = rmw_qos_profile_default, - rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); [[deprecated("Use subscribe(rclcpp::node_interfaces..., rclcpp::QoS, ...) instead")]] POINT_CLOUD_TRANSPORT_PUBLIC @@ -172,9 +135,18 @@ class SubscriberFilter rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces, const std::string & base_topic, const std::string & transport, - rclcpp::QoS custom_qos = rclcpp::SystemDefaultsQoS(), + rclcpp::QoS custom_qos, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + /// + /// \brief Subscribe to a topic. If this Subscriber is already subscribed to a topic, + /// this function will first unsubscribe. + /// \param node The Node or/ LifecycleNode to use to subscribe. + /// \param base_topic The topic to subscribe to. + /// \param transport The transport hint to pass along + /// \param custom_qos Custom quality of service + /// \param options Subscriber options + /// template POINT_CLOUD_TRANSPORT_PUBLIC void subscribe( @@ -217,17 +189,21 @@ class SubscriberFilter const Subscriber & getSubscriber() const; private: - //! Don't use this method + //! Must override parent message_filters::SubscriberBase method + // where RequiredInterfaces are just void subscribe( - RequiredInterfaces /*node_interfaces*/, const std::string & /*topic*/, - const rclcpp::QoS & /*qos*/) override + RequiredInterfaces, + const std::string &, + const rclcpp::QoS &) override {} - //! Don't use this method + + //! Must override parent message_filters::SubscriberBase method + // where RequiredInterfaces are just void subscribe( - RequiredInterfaces /*node_interfaces*/, - const std::string & /*topic*/, - const rclcpp::QoS & /*qos*/, - rclcpp::SubscriptionOptions /*options*/) override + RequiredInterfaces, + const std::string &, + const rclcpp::QoS &, + rclcpp::SubscriptionOptions) override {} void cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & m) diff --git a/point_cloud_transport/src/subscriber_filter.cpp b/point_cloud_transport/src/subscriber_filter.cpp index f0b52b6..cb3cfd4 100644 --- a/point_cloud_transport/src/subscriber_filter.cpp +++ b/point_cloud_transport/src/subscriber_filter.cpp @@ -40,11 +40,9 @@ SubscriberFilter::SubscriberFilter( rclcpp::node_interfaces::NodeTopicsInterface, rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces, const std::string & base_topic, - const std::string & transport, - rclcpp::QoS custom_qos, - rclcpp::SubscriptionOptions options) + const std::string & transport) { - subscribe(node_interfaces, base_topic, transport, custom_qos, options); + subscribe(node_interfaces, base_topic, transport, rclcpp::SystemDefaultsQoS()); } SubscriberFilter::SubscriberFilter() @@ -56,27 +54,6 @@ SubscriberFilter::~SubscriberFilter() unsubscribe(); } -void SubscriberFilter::subscribe( - std::shared_ptr node, - const std::string & base_topic, - const std::string & transport, - rmw_qos_profile_t custom_qos, - rclcpp::SubscriptionOptions options) -{ - unsubscribe(); - auto node_interfaces = std::make_shared>(*node); - sub_ = point_cloud_transport::create_subscription( - node_interfaces, base_topic, - std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), - transport, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos), - options); -} - void SubscriberFilter::subscribe( std::shared_ptr ApproximateTimePolicy; -#ifdef _MSC_VER -#pragma warning(push) -#pragma warning(disable : 4996) -#else -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - point_cloud_transport::SubscriberFilter pcl_sub1(node_, "pointcloud1", "raw"); point_cloud_transport::SubscriberFilter pcl_sub2(node_, "pointcloud2", "raw"); -#ifdef _MSC_VER -#pragma warning(pop) -#else -#pragma GCC diagnostic pop -#endif - auto sync = std::make_shared>( ApproximateTimePolicy( 10), pcl_sub1, pcl_sub2); @@ -117,9 +103,23 @@ TEST_F(TestSubscriber, create_and_release_filter_ni_api) sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproximateTimePolicy; +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + point_cloud_transport::SubscriberFilter pcl_sub1(node_interfaces_, "pointcloud1", "raw"); point_cloud_transport::SubscriberFilter pcl_sub2(node_interfaces_, "pointcloud2", "raw"); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif + auto sync = std::make_shared>( ApproximateTimePolicy( 10), pcl_sub1, pcl_sub2); From 8c4ef814ecb9f97b0ac934802264642cf740d8a2 Mon Sep 17 00:00:00 2001 From: ElSayed ElSheikh Date: Wed, 2 Jul 2025 07:51:29 +0300 Subject: [PATCH 10/12] No more node_interfaces Signed-off-by: ElSayed ElSheikh --- .../subscriber_filter.hpp | 25 +++++------ .../src/subscriber_filter.cpp | 41 +++++++++++++++---- 2 files changed, 47 insertions(+), 19 deletions(-) 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 49f189b..f8e87e8 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -116,16 +116,7 @@ class SubscriberFilter rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); - /// - /// \brief Subscribe to a topic. If this Subscriber is already subscribed to a topic, - /// this function will first unsubscribe. - /// \param node_interfaces the ROS node interfaces required for core node functionality, including - /// NodeBaseInterface, NodeParametersInterface, NodeTopicsInterface, and NodeLoggingInterface. - /// \param base_topic The topic to subscribe to. - /// \param transport The transport hint to pass along - /// \param custom_qos Custom quality of service - /// \param options Subscriber options - /// + [[deprecated("Use subscribe(NodeT, ...) instead")]] POINT_CLOUD_TRANSPORT_PUBLIC void subscribe( std::shared_ptr>(*node); - subscribe(node_interfaces, base_topic, transport, custom_qos, options); + + sub_ = point_cloud_transport::create_subscription( + ni, base_topic, + std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), + transport, custom_qos, options); + node_interfaces_ = ni; + topic_ = base_topic; + transport_ = transport; + qos_ = custom_qos; + options_ = options; } //! Re-subscribe to a topic. diff --git a/point_cloud_transport/src/subscriber_filter.cpp b/point_cloud_transport/src/subscriber_filter.cpp index cb3cfd4..9b770fe 100644 --- a/point_cloud_transport/src/subscriber_filter.cpp +++ b/point_cloud_transport/src/subscriber_filter.cpp @@ -42,7 +42,19 @@ SubscriberFilter::SubscriberFilter( const std::string & base_topic, const std::string & transport) { +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif subscribe(node_interfaces, base_topic, transport, rclcpp::SystemDefaultsQoS()); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif } SubscriberFilter::SubscriberFilter() @@ -65,8 +77,20 @@ void SubscriberFilter::subscribe( rmw_qos_profile_t custom_qos, rclcpp::SubscriptionOptions options) { +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4996) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif subscribe(node_interfaces, base_topic, transport, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos), options); +#ifdef _MSC_VER +#pragma warning(pop) +#else +#pragma GCC diagnostic pop +#endif } void SubscriberFilter::subscribe( @@ -85,18 +109,21 @@ void SubscriberFilter::subscribe( node_interfaces, base_topic, std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), transport, custom_qos, options); - // Update class members - this->node_interfaces_ = node_interfaces; - this->topic_ = base_topic; - this->transport_ = transport; - this->qos_ = custom_qos; - this->options_ = options; } void SubscriberFilter::subscribe() { + unsubscribe(); if (node_interfaces_ != nullptr) { - subscribe(node_interfaces_, topic_, transport_, qos_, options_); + 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." + ); } } From a8470961f99827cb0e46902325f396e0335931ae Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Fri, 18 Jul 2025 10:05:58 +0200 Subject: [PATCH 11/12] Added missing include Signed-off-by: Alejandro Hernandez Cordero --- point_cloud_transport/src/subscriber_filter.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/point_cloud_transport/src/subscriber_filter.cpp b/point_cloud_transport/src/subscriber_filter.cpp index 1f9ad99..e989109 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 From 02b94903f1c1781452de48b9d4cffbd781eed3bc Mon Sep 17 00:00:00 2001 From: ElSayed ElSheikh Date: Fri, 18 Jul 2025 13:12:24 +0300 Subject: [PATCH 12/12] final touch Signed-off-by: ElSayed ElSheikh --- .../point_cloud_transport/subscriber_filter.hpp | 13 ++++++++++--- point_cloud_transport/src/subscriber_filter.cpp | 15 ++++++--------- 2 files changed, 16 insertions(+), 12 deletions(-) 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 f1094c0..7b8aaee 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -42,6 +42,7 @@ #include #include #include "point_cloud_transport/visibility_control.hpp" +#include "point_cloud_transport/exception.hpp" namespace point_cloud_transport { @@ -91,7 +92,9 @@ class 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::SystemDefaultsQoS(), + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); //! Empty constructor, use subscribe() to subscribe to a topic POINT_CLOUD_TRANSPORT_PUBLIC @@ -175,7 +178,9 @@ class SubscriberFilter 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 @@ -186,7 +191,9 @@ class SubscriberFilter 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) { diff --git a/point_cloud_transport/src/subscriber_filter.cpp b/point_cloud_transport/src/subscriber_filter.cpp index e989109..f69d606 100644 --- a/point_cloud_transport/src/subscriber_filter.cpp +++ b/point_cloud_transport/src/subscriber_filter.cpp @@ -41,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() @@ -62,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(