Enable filters in humble

pcl_ros/CMakeLists.txt

@@ -26,6 +26,7 @@ find_package(tf2_ros REQUIRED)
 set(dependencies
   pcl_conversions
   rclcpp
+  rclcpp_components
   sensor_msgs
   geometry_msgs
   tf2
@@ -80,19 +81,48 @@ ament_target_dependencies(pcl_ros_tf
 #
 #
 ### Declare the pcl_ros_filters library
-#add_library(pcl_ros_filters
-#  src/pcl_ros/filters/extract_indices.cpp
-#  src/pcl_ros/filters/filter.cpp
-#  src/pcl_ros/filters/passthrough.cpp
-#  src/pcl_ros/filters/project_inliers.cpp
-#  src/pcl_ros/filters/radius_outlier_removal.cpp
-#  src/pcl_ros/filters/statistical_outlier_removal.cpp
-#  src/pcl_ros/filters/voxel_grid.cpp
-#  src/pcl_ros/filters/crop_box.cpp
-#)
-#target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
-#add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg)
-#class_loader_hide_library_symbols(pcl_ros_filters)
+add_library(pcl_ros_filters SHARED
+  src/pcl_ros/filters/extract_indices.cpp
+  src/pcl_ros/filters/filter.cpp
+  src/pcl_ros/filters/passthrough.cpp
+  src/pcl_ros/filters/project_inliers.cpp
+  src/pcl_ros/filters/radius_outlier_removal.cpp
+  src/pcl_ros/filters/statistical_outlier_removal.cpp
+  src/pcl_ros/filters/voxel_grid.cpp
+  src/pcl_ros/filters/crop_box.cpp
+)
+target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES})
+ament_target_dependencies(pcl_ros_filters ${dependencies})
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::ExtractIndices"
+  EXECUTABLE filter_extract_indices_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::PassThrough"
+  EXECUTABLE filter_passthrough_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::ProjectInliers"
+  EXECUTABLE filter_project_inliers_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::RadiusOutlierRemoval"
+  EXECUTABLE filter_radius_outlier_removal_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::StatisticalOutlierRemoval"
+  EXECUTABLE filter_statistical_outlier_removal_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::CropBox"
+  EXECUTABLE filter_crop_box_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::VoxelGrid"
+  EXECUTABLE filter_voxel_grid_node
+)
+# add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg)
+class_loader_hide_library_symbols(pcl_ros_filters)
 #
 ### Declare the pcl_ros_segmentation library
 #add_library (pcl_ros_segmentation
@@ -163,6 +193,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(tests/filters)
   #add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp)
   #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
   #add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false)
@@ -184,7 +215,7 @@ install(
     pcd_to_pointcloud_lib
 #    pcl_ros_io
 #    pcl_ros_features
-#    pcl_ros_filters
+    pcl_ros_filters
 #    pcl_ros_surface
 #    pcl_ros_segmentation
 #    pointcloud_to_pcd

pcl_ros/include/pcl_ros/filters/crop_box.hpp

@@ -41,11 +41,9 @@
 
 // PCL includes
 #include <pcl/filters/crop_box.h>
+#include <vector>
 #include "pcl_ros/filters/filter.hpp"
 
-// Dynamic reconfigure
-#include "pcl_ros/CropBoxConfig.hpp"
-
 namespace pcl_ros
 {
 /** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box.
@@ -57,49 +55,32 @@ namespace pcl_ros
 class CropBox : public Filter
 {
 protected:
-  /** \brief Pointer to a dynamic reconfigure service. */
-  boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>> srv_;  // TODO(xxx)
-
   /** \brief Call the actual filter.
     * \param input the input point cloud dataset
     * \param indices the input set of indices to use from \a input
     * \param output the resultant filtered dataset
     */
   inline void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
-    PointCloud2 & output)
-  {
-    boost::mutex::scoped_lock 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);
-  }
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
 
-  /** \brief Child initialization routine.
-    * \param nh ROS node handle
-    * \param has_service set to true if the child has a Dynamic Reconfigure service
+  /** \brief Parameter callback
+    * \param params parameter values to set
     */
-  bool
-  child_init(ros::NodeHandle & nh, bool & has_service);
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
 
-  /** \brief Dynamic reconfigure service callback.
-    * \param config the dynamic reconfigure CropBoxConfig object
-    * \param level the dynamic reconfigure level
-    */
-  void
-  config_callback(pcl_ros::CropBoxConfig & config, uint32_t level);
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 
 private:
   /** \brief The PCL filter implementation used. */
   pcl::CropBox<pcl::PCLPointCloud2> impl_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit CropBox(const rclcpp::NodeOptions & options);
 };
 }  // namespace pcl_ros
 

pcl_ros/include/pcl_ros/filters/extract_indices.hpp

@@ -40,9 +40,8 @@
 
 // PCL includes
 #include <pcl/filters/extract_indices.h>
-
+#include <vector>
 #include "pcl_ros/filters/filter.hpp"
-#include "pcl_ros/ExtractIndicesConfig.hpp"
 
 namespace pcl_ros
 {
@@ -53,46 +52,32 @@ namespace pcl_ros
 class ExtractIndices : public Filter
 {
 protected:
-  /** \brief Pointer to a dynamic reconfigure service. */
-  boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>> srv_;
-
   /** \brief Call the actual filter.
     * \param input the input point cloud dataset
     * \param indices the input set of indices to use from \a input
     * \param output the resultant filtered dataset
     */
   inline void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
-    PointCloud2 & output)
-  {
-    boost::mutex::scoped_lock 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);
-  }
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
 
-  /** \brief Child initialization routine.
-    * \param nh ROS node handle
-    * \param has_service set to true if the child has a Dynamic Reconfigure service
+  /** \brief Parameter callback
+    * \param params parameter values to set
     */
-  virtual bool
-  child_init(ros::NodeHandle & nh, bool & has_service);
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
 
-  /** \brief Dynamic reconfigure service callback. */
-  void
-  config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level);
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 
 private:
   /** \brief The PCL filter implementation used. */
   pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit ExtractIndices(const rclcpp::NodeOptions & options);
 };
 }  // namespace pcl_ros
 

pcl_ros/include/pcl_ros/filters/filter.hpp

@@ -38,11 +38,10 @@
 #ifndef PCL_ROS__FILTERS__FILTER_HPP_
 #define PCL_ROS__FILTERS__FILTER_HPP_
 
-#include <pcl/filters/filter.h>
-#include <dynamic_reconfigure/server.h>
+#include <memory>
 #include <string>
-#include "pcl_ros/pcl_nodelet.hpp"
-#include "pcl_ros/FilterConfig.hpp"
+#include <vector>
+#include "pcl_ros/pcl_node.hpp"
 
 namespace pcl_ros
 {
@@ -52,19 +51,30 @@ namespace sync_policies = message_filters::sync_policies;
   * applicable to all filters are defined here as static methods.
   * \author Radu Bogdan Rusu
   */
-class Filter : public PCLNodelet
+class Filter : public PCLNode<sensor_msgs::msg::PointCloud2>
 {
 public:
-  typedef sensor_msgs::PointCloud2 PointCloud2;
+  typedef sensor_msgs::msg::PointCloud2 PointCloud2;
 
   typedef pcl::IndicesPtr IndicesPtr;
   typedef pcl::IndicesConstPtr IndicesConstPtr;
 
-  Filter() {}
+  /** \brief Filter constructor
+    * \param node_name node name
+    * \param options node options
+    */
+  Filter(std::string node_name, const rclcpp::NodeOptions & options);
 
 protected:
+  /** \brief declare and subscribe to param callback for input_frame and output_frame params */
+  void
+  use_frame_params();
+
+  /** \brief Add common parameters */
+  std::vector<std::string> add_common_params();
+
   /** \brief The input PointCloud subscriber. */
-  ros::Subscriber sub_input_;
+  rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;
 
   message_filters::Subscriber<PointCloud2> sub_input_filter_;
 
@@ -96,18 +106,7 @@ class Filter : public PCLNodelet
   std::string tf_output_frame_;
 
   /** \brief Internal mutex. */
-  boost::mutex mutex_;
-
-  /** \brief Child initialization routine.
-    * \param nh ROS node handle
-    * \param has_service set to true if the child has a Dynamic Reconfigure service
-    */
-  virtual bool
-  child_init(ros::NodeHandle & nh, bool & has_service)
-  {
-    has_service = false;
-    return true;
-  }
+  std::mutex mutex_;
 
   /** \brief Virtual abstract filter method. To be implemented by every child.
     * \param input the input point cloud dataset.
@@ -116,7 +115,7 @@ class Filter : public PCLNodelet
     */
   virtual void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
     PointCloud2 & output) = 0;
 
   /** \brief Lazy transport subscribe routine. */
@@ -127,36 +126,38 @@ class Filter : public PCLNodelet
   virtual void
   unsubscribe();
 
-  /** \brief Nodelet initialization routine. */
+  /** \brief Create publishers for output PointCloud2 data. */
   virtual void
-  onInit();
+  createPublishers();
 
   /** \brief Call the child filter () method, optionally transform the result, and publish it.
     * \param input the input point cloud dataset.
     * \param indices a pointer to the vector of point indices to use.
     */
   void
-  computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices);
+  computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices);
 
 private:
-  /** \brief Pointer to a dynamic reconfigure service. */
-  boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
+  /** \brief Pointer to parameters callback handle. */
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 
   /** \brief Synchronized input, and indices.*/
-  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
+  std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
     PointIndices>>> sync_input_indices_e_;
-  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
+  std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
     PointIndices>>> sync_input_indices_a_;
 
-  /** \brief Dynamic reconfigure service callback. */
-  virtual void
-  config_callback(pcl_ros::FilterConfig & config, uint32_t level);
+  /** \brief Parameter callback
+    * \param params parameter values to set
+    */
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
 
   /** \brief PointCloud2 + Indices data callback. */
   void
   input_indices_callback(
-    const PointCloud2::ConstPtr & cloud,
-    const PointIndicesConstPtr & indices);
+    const PointCloud2::ConstSharedPtr & cloud,
+    const PointIndices::ConstSharedPtr & indices);
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW