From a61207e294cb78111a04010ab2515c059db74db6 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Wed, 19 Jun 2024 13:44:39 +0200 Subject: [PATCH 1/2] Fixed message filter api Signed-off-by: Alejandro Hernandez Cordero --- pcl_ros/include/pcl_ros/filters/filter.hpp | 14 ++++++++++---- .../include/pcl_ros/filters/project_inliers.hpp | 17 +++++++++++++---- pcl_ros/src/pcl_ros/filters/filter.cpp | 10 +++++----- pcl_ros/src/pcl_ros/filters/project_inliers.cpp | 16 ++++++---------- 4 files changed, 34 insertions(+), 23 deletions(-) diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index c4e88d7f..9b466740 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -137,11 +137,17 @@ class Filter : public PCLNode /** \brief Pointer to parameters callback handle. */ OnSetParametersCallbackHandle::SharedPtr callback_handle_; + typedef message_filters::sync_policies::ApproximateTime< + PointCloud2, PointIndices> SyncApproxPolInputIndices; + typedef message_filters::Synchronizer SynchronizerApproxInputIndices; + + typedef message_filters::sync_policies::ExactTime< + PointCloud2, PointIndices> SyncExactPolInputIndices; + typedef message_filters::Synchronizer SynchronizerExactInputIndices; + /** \brief Synchronized input, and indices.*/ - std::shared_ptr>> sync_input_indices_e_; - std::shared_ptr>> sync_input_indices_a_; + std::shared_ptr sync_input_indices_e_; + std::shared_ptr sync_input_indices_a_; /** \brief Parameter callback * \param params parameter values to set diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp index 639289c4..2e4409b8 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -77,11 +77,20 @@ class ProjectInliers : public Filter /** \brief The message filter subscriber for model coefficients. */ message_filters::Subscriber sub_model_; + typedef message_filters::sync_policies::ApproximateTime< + PointCloud2, PointIndices, ModelCoefficients> SyncApproxPolInputIndicesModel; + typedef message_filters::Synchronizer + SynchronizerApproxInputIndicesModel; + + typedef message_filters::sync_policies::ExactTime< + PointCloud2, PointIndices, ModelCoefficients> SyncExactPolInputIndicesModel; + typedef message_filters::Synchronizer + SynchronizerExactInputIndicesModel; + /** \brief Synchronized input, indices, and model coefficients.*/ - std::shared_ptr>> sync_input_indices_model_e_; - std::shared_ptr>> sync_input_indices_model_a_; + std::shared_ptr sync_input_indices_model_e_; + std::shared_ptr sync_input_indices_model_a_; + /** \brief The PCL filter implementation used. */ pcl::ProjectInliers impl_; diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index a53b8209..0d2fd55d 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -119,14 +119,14 @@ pcl_ros::Filter::subscribe() if (use_indices_) { // Subscribe to the input using a filter auto sensor_qos_profile = - rclcpp::SensorDataQoS().keep_last(max_queue_size_).get_rmw_qos_profile(); + rclcpp::SensorDataQoS().keep_last(max_queue_size_); sub_input_filter_.subscribe(this, "input", sensor_qos_profile); sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile); if (approximate_sync_) { sync_input_indices_a_ = - std::make_shared>>(max_queue_size_); + std::make_shared(SyncApproxPolInputIndices( + max_queue_size_)); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback( std::bind( @@ -134,8 +134,8 @@ pcl_ros::Filter::subscribe() std::placeholders::_1, std::placeholders::_2)); } else { sync_input_indices_e_ = - std::make_shared>>(max_queue_size_); + std::make_shared(SyncExactPolInputIndices( + max_queue_size_)); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback( std::bind( diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index 39804ba3..25906866 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -108,29 +108,25 @@ pcl_ros::ProjectInliers::subscribe() auto qos_profile = rclcpp::QoS( rclcpp::KeepLast(max_queue_size_), - rmw_qos_profile_default).get_rmw_qos_profile(); + rmw_qos_profile_default); auto sensor_qos_profile = rclcpp::QoS( rclcpp::KeepLast(max_queue_size_), - rmw_qos_profile_sensor_data).get_rmw_qos_profile(); + rmw_qos_profile_sensor_data); sub_input_filter_.subscribe(this, "input", sensor_qos_profile); sub_indices_filter_.subscribe(this, "indices", qos_profile); sub_model_.subscribe(this, "model", qos_profile); if (approximate_sync_) { - sync_input_indices_model_a_ = std::make_shared< - message_filters::Synchronizer< - message_filters::sync_policies::ApproximateTime< - PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_); + sync_input_indices_model_a_ = std::make_shared( + SyncApproxPolInputIndicesModel(max_queue_size_)); sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_a_->registerCallback( std::bind( &ProjectInliers::input_indices_model_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else { - sync_input_indices_model_e_ = std::make_shared< - message_filters::Synchronizer< - message_filters::sync_policies::ExactTime< - PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_); + sync_input_indices_model_e_ = std::make_shared( + SyncExactPolInputIndicesModel(max_queue_size_)); sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); sync_input_indices_model_e_->registerCallback( std::bind( From 382e6a3298c32eacdc2bba799e49b79754abb3f5 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Wed, 19 Jun 2024 16:45:51 +0200 Subject: [PATCH 2/2] make linters happy Signed-off-by: Alejandro Hernandez Cordero --- pcl_ros/include/pcl_ros/filters/project_inliers.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp index 2e4409b8..b4ffd68b 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -79,12 +79,12 @@ class ProjectInliers : public Filter typedef message_filters::sync_policies::ApproximateTime< PointCloud2, PointIndices, ModelCoefficients> SyncApproxPolInputIndicesModel; - typedef message_filters::Synchronizer + typedef message_filters::Synchronizer SynchronizerApproxInputIndicesModel; typedef message_filters::sync_policies::ExactTime< PointCloud2, PointIndices, ModelCoefficients> SyncExactPolInputIndicesModel; - typedef message_filters::Synchronizer + typedef message_filters::Synchronizer SynchronizerExactInputIndicesModel; /** \brief Synchronized input, indices, and model coefficients.*/