Move filter() function implementation for all filters (#405)

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
+    PointCloud2 & output) override;
-  {
-    std::lock_guard<std::mutex> 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);
-  }
 
   /** \brief Parameter callback
     * \param params parameter values to set

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)
+    PointCloud2 & output) override;
-  {
-    std::lock_guard<std::mutex> 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);
-  }
 
   /** \brief Parameter callback
     * \param params parameter values to set

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
+    PointCloud2 & output) override;
-  {
-    std::lock_guard<std::mutex> 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);
-  }
 
   /** \brief Parameter callback
     * \param params parameter values to set

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)
+    PointCloud2 & output) override;
-  {
-    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);
-  }
 
 private:
   /** \brief A pointer to the vector of model coefficients. */

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
+    PointCloud2 & output) override;
-  {
-    std::lock_guard<std::mutex> 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);
-  }
 
   /** \brief Parameter callback
     * \param params parameter values to set

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
+    PointCloud2 & output) override;
-  {
-    std::lock_guard<std::mutex> 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);
-  }
 
   /** \brief Parameter callback
     * \param params parameter values to set

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
+    PointCloud2 & output) override;
-  {
-    std::lock_guard<std::mutex> 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);
-  }
 
   /** \brief Parameter callback
     * \param params parameter values to set

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<std::mutex> 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/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<std::mutex> 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<rclcpp::Parameter> & params)

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<std::mutex> 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<rclcpp::Parameter> & params)

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()

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<std::mutex> 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<rclcpp::Parameter> & params)

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<std::mutex> 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/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<std::mutex> 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