diff --git a/point_cloud_transport/CMakeLists.txt b/point_cloud_transport/CMakeLists.txt index c5c9fe2..323d0be 100644 --- a/point_cloud_transport/CMakeLists.txt +++ b/point_cloud_transport/CMakeLists.txt @@ -30,6 +30,7 @@ add_library(${PROJECT_NAME} src/publisher_plugin.cpp src/raw_subscriber.cpp src/single_subscriber_publisher.cpp + src/subscriber_filter.cpp src/subscriber.cpp ) 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 97417ad..cc99fd9 100644 --- a/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp +++ b/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp @@ -73,23 +73,14 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, - const std::string & transport) - { - subscribe(node, base_topic, transport); - } + const std::string & transport); //! Empty constructor, use subscribe() to subscribe to a topic POINT_CLOUD_TRANSPORT_PUBLIC - SubscriberFilter() - { - } + SubscriberFilter(); POINT_CLOUD_TRANSPORT_PUBLIC - ~SubscriberFilter() - { - unsubscribe(); - } - + ~SubscriberFilter(); /// /// \brief Subscribe to a topic. If this Subscriber is already subscribed to a topic, /// this function will first unsubscribe. @@ -105,48 +96,26 @@ class SubscriberFilter : public message_filters::SimpleFilter +#include + +namespace point_cloud_transport +{ +SubscriberFilter::SubscriberFilter( + std::shared_ptr node, const std::string & base_topic, + const std::string & transport) +{ + subscribe(node, base_topic, transport); +} + +SubscriberFilter::SubscriberFilter() +{ +} + +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(); + sub_ = point_cloud_transport::create_subscription( + node, base_topic, + std::bind(&SubscriberFilter::cb, this, std::placeholders::_1), + transport, custom_qos, options); +} + +void SubscriberFilter::unsubscribe() +{ + sub_.shutdown(); +} + +std::string SubscriberFilter::getTopic() const +{ + return sub_.getTopic(); +} + +uint32_t SubscriberFilter::getNumPublishers() const +{ + return sub_.getNumPublishers(); +} + +std::string SubscriberFilter::getTransport() const +{ + return sub_.getTransport(); +} + +const Subscriber & SubscriberFilter::getSubscriber() const +{ + return sub_; +} +} // namespace point_cloud_transport diff --git a/point_cloud_transport/test/test_message_filter.cpp b/point_cloud_transport/test/test_message_filter.cpp index db39292..6eb17d7 100644 --- a/point_cloud_transport/test/test_message_filter.cpp +++ b/point_cloud_transport/test/test_message_filter.cpp @@ -59,25 +59,24 @@ void callback( (void) msg2; } -// TEST_F(TestSubscriber, create_and_release_filter) -// { -// typedef message_filters::sync_policies::ApproximateTime< -// sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> -// ApproximateTimePolicy; +TEST_F(TestSubscriber, create_and_release_filter) +{ + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> + ApproximateTimePolicy; -// TODO(ahcorde): Review this test on Windows -// point_cloud_transport::SubscriberFilter pcl_sub1(node_, "pointcloud1", "raw"); -// point_cloud_transport::SubscriberFilter pcl_sub2(node_, "pointcloud2", "raw"); + point_cloud_transport::SubscriberFilter pcl_sub1(node_, "pointcloud1", "raw"); + point_cloud_transport::SubscriberFilter pcl_sub2(node_, "pointcloud2", "raw"); -// auto sync = std::make_shared>( -// ApproximateTimePolicy( -// 10), pcl_sub1, pcl_sub2); -// sync->registerCallback(std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + auto sync = std::make_shared>( + ApproximateTimePolicy( + 10), pcl_sub1, pcl_sub2); + sync->registerCallback(std::bind(callback, std::placeholders::_1, std::placeholders::_2)); -// pcl_sub1.unsubscribe(); -// pcl_sub2.unsubscribe(); -// sync.reset(); -// } + pcl_sub1.unsubscribe(); + pcl_sub2.unsubscribe(); + sync.reset(); +} int main(int argc, char ** argv) {