diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/pcl_ros/include/pcl_ros/filters/crop_box.hpp index 159d6e78..36733776 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.hpp +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -63,17 +63,7 @@ protected: inline void filter( const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override - { - std::lock_guard lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + PointCloud2 & output) override; /** \brief Parameter callback * \param params parameter values to set diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp index bb752f2d..0d244551 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -61,17 +61,7 @@ protected: inline void filter( const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - std::lock_guard lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + PointCloud2 & output) override; /** \brief Parameter callback * \param params parameter values to set diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index 8ae292cb..dd18bec8 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.hpp +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -60,17 +60,7 @@ protected: inline void filter( const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override - { - std::lock_guard lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + PointCloud2 & output) override; /** \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 bee70d38..639289c4 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -68,19 +68,7 @@ protected: inline void filter( const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); - pcl_conversions::toPCL(*(model_), *(pcl_model)); - impl_.setModelCoefficients(pcl_model); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + PointCloud2 & output) override; private: /** \brief A pointer to the vector of model coefficients. */ diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp index 66e2b2e7..5f7333df 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -61,17 +61,7 @@ protected: inline void filter( const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override - { - std::lock_guard lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + PointCloud2 & output) override; /** \brief Parameter callback * \param params parameter values to set diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp index 84e5c434..f8e14a2b 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -67,17 +67,7 @@ protected: inline void filter( const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override - { - std::lock_guard lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + PointCloud2 & output) override; /** \brief Parameter callback * \param params parameter values to set diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index af206262..4b51113c 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -59,17 +59,7 @@ protected: inline void filter( const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override - { - std::lock_guard lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + PointCloud2 & output) override; /** \brief Parameter callback * \param params parameter values to set diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp index 93fcf0b6..0b9889a0 100644 --- a/pcl_ros/src/pcl_ros/filters/crop_box.cpp +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -160,6 +160,21 @@ pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options) subscribe(); } +void +pcl_ros::CropBox::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); +} + ////////////////////////////////////////////////////////////////////////////////////////////// rcl_interfaces::msg::SetParametersResult diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp index 4106c81e..d6287a1b 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -60,6 +60,21 @@ pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options) subscribe(); } +void +pcl_ros::ExtractIndices::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); +} + ////////////////////////////////////////////////////////////////////////////////////////////// rcl_interfaces::msg::SetParametersResult pcl_ros::ExtractIndices::config_callback(const std::vector & params) diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index d7a90e78..a068da77 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -68,6 +68,20 @@ pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options) subscribe(); } +void pcl_ros::PassThrough::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); +} + ////////////////////////////////////////////////////////////////////////////////////////////// rcl_interfaces::msg::SetParametersResult pcl_ros::PassThrough::config_callback(const std::vector & params) diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index 05cf7d4b..21faa96d 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -80,6 +80,23 @@ pcl_ros::ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options) subscribe(); } +void +pcl_ros::ProjectInliers::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); + pcl_conversions::toPCL(*(model_), *(pcl_model)); + impl_.setModelCoefficients(pcl_model); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); +} + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ProjectInliers::subscribe() diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp index c6adb4a2..77e22579 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -83,6 +83,21 @@ pcl_ros::RadiusOutlierRemoval::RadiusOutlierRemoval(const rclcpp::NodeOptions & subscribe(); } +void +pcl_ros::RadiusOutlierRemoval::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); +} + ////////////////////////////////////////////////////////////////////////////////////////////// rcl_interfaces::msg::SetParametersResult pcl_ros::RadiusOutlierRemoval::config_callback(const std::vector & params) diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp index a4cebaa9..0285d12a 100644 --- a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -92,6 +92,20 @@ pcl_ros::StatisticalOutlierRemoval::StatisticalOutlierRemoval(const rclcpp::Node subscribe(); } +void +pcl_ros::StatisticalOutlierRemoval::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); +} ////////////////////////////////////////////////////////////////////////////////////////////// rcl_interfaces::msg::SetParametersResult diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 18a4f0a8..d8f5844c 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -89,6 +89,20 @@ pcl_ros::VoxelGrid::VoxelGrid(const rclcpp::NodeOptions & options) subscribe(); } +void +pcl_ros::VoxelGrid::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); +} ////////////////////////////////////////////////////////////////////////////////////////////// rcl_interfaces::msg::SetParametersResult