Migrate the ROS1 pcl::VoxelGrid filter to ROS2 pcl_ros::VoxelGrid (#398)

* Add the VoxelGrid filter (squash commit)

* Revert the change that brings in separate leaf sizes

pcl_ros/CMakeLists.txt

@@ -88,7 +88,7 @@ add_library(pcl_ros_filters SHARED
   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/voxel_grid.cpp
   src/pcl_ros/filters/crop_box.cpp
 )
 target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES})
@@ -117,6 +117,10 @@ 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
+)
 class_loader_hide_library_symbols(pcl_ros_filters)
 #
 ### Declare the pcl_ros_segmentation library

pcl_ros/include/pcl_ros/filters/voxel_grid.hpp

@@ -40,11 +40,9 @@
 
 // PCL includes
 #include <pcl/filters/voxel_grid.h>
+#include <vector>
 #include "pcl_ros/filters/filter.hpp"
 
-// Dynamic reconfigure
-#include "pcl_ros/VoxelGridConfig.hpp"
-
 namespace pcl_ros
 {
 /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
@@ -53,38 +51,42 @@ namespace pcl_ros
 class VoxelGrid : public Filter
 {
 protected:
-  /** \brief Pointer to a dynamic reconfigure service. */
-  boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>> srv_;
-
-  /** \brief The PCL filter implementation used. */
-  pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
-
   /** \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
     */
-  virtual void
+  inline void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
+    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 Child initialization routine.
+  /** \brief Parameter callback
-    * \param nh ROS node handle
+    * \param params parameter values to set
-    * \param has_service set to true if the child has a Dynamic Reconfigure service
     */
-  bool
+  rcl_interfaces::msg::SetParametersResult
-  child_init(ros::NodeHandle & nh, bool & has_service);
+  config_callback(const std::vector<rclcpp::Parameter> & params);
 
-  /** \brief Dynamic reconfigure callback
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
-    * \param config the config object
+
-    * \param level the dynamic reconfigure level
+private:
-    */
+  /** \brief The PCL filter implementation used. */
-  void
+  pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
-  config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level);
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit VoxelGrid(const rclcpp::NodeOptions & options);
 };
 }  // namespace pcl_ros
 

pcl_ros/src/pcl_ros/filters/filter.cpp

@@ -247,20 +247,11 @@ pcl_ros::Filter::add_common_params()
     "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max].";
   declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc);
 
-  rcl_interfaces::msg::ParameterDescriptor keep_organized_desc;
-  keep_organized_desc.name = "keep_organized";
-  keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
-  keep_organized_desc.description =
-    "Set whether the filtered points should be kept and set to NaN, "
-    "or removed from the PointCloud, thus potentially breaking its organized structure.";
-  declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc);
-
   return std::vector<std::string> {
     ffn_desc.name,
     flmin_desc.name,
     flmax_desc.name,
-    flneg_desc.name,
+    flneg_desc.name
-    keep_organized_desc.name
   };
 }
 

pcl_ros/src/pcl_ros/filters/passthrough.cpp

@@ -41,7 +41,20 @@ pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options)
 : Filter("PassThroughNode", options)
 {
   use_frame_params();
-  std::vector<std::string> param_names = add_common_params();
+  std::vector<std::string> common_param_names = add_common_params();
+
+  rcl_interfaces::msg::ParameterDescriptor keep_organized_desc;
+  keep_organized_desc.name = "keep_organized";
+  keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  keep_organized_desc.description =
+    "Set whether the filtered points should be kept and set to NaN, "
+    "or removed from the PointCloud, thus potentially breaking its organized structure.";
+  declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc);
+
+  std::vector<std::string> param_names {
+    keep_organized_desc.name,
+  };
+  param_names.insert(param_names.end(), common_param_names.begin(), common_param_names.end());
 
   callback_handle_ =
     add_on_set_parameters_callback(
@@ -50,6 +63,7 @@ pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options)
       std::placeholders::_1));
 
   config_callback(get_parameters(param_names));
+
   // TODO(daisukes): lazy subscription after rclcpp#2060
   subscribe();
 }

pcl_ros/src/pcl_ros/filters/voxel_grid.cpp

@@ -35,100 +35,144 @@
  *
  */
 
-#include <pluginlib/class_list_macros.h>
 #include "pcl_ros/filters/voxel_grid.hpp"
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-bool
+
-pcl_ros::VoxelGrid::child_init(ros::NodeHandle & nh, bool & has_service)
+pcl_ros::VoxelGrid::VoxelGrid(const rclcpp::NodeOptions & options)
+: Filter("VoxelGridNode", options)
 {
-  // Enable the dynamic reconfigure service
+  std::vector<std::string> common_param_names = add_common_params();
-  has_service = true;
-  srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>>(nh);
-  dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind(
-    &VoxelGrid::config_callback, this, _1, _2);
-  srv_->setCallback(f);
 
-  return true;
+  rcl_interfaces::msg::ParameterDescriptor leaf_size_desc;
-}
+  leaf_size_desc.name = "leaf_size";
-
+  leaf_size_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-//////////////////////////////////////////////////////////////////////////////////////////////
+  leaf_size_desc.description =
-void
+    "The size of a leaf (on x,y,z) used for downsampling";
-pcl_ros::VoxelGrid::filter(
+  {
-  const PointCloud2::ConstPtr & input,
+    rcl_interfaces::msg::FloatingPointRange float_range;
-  const IndicesPtr & indices,
+    float_range.from_value = 0.0;
-  PointCloud2 & output)
+    float_range.to_value = 1.0;
-{
+    leaf_size_desc.floating_point_range.push_back(float_range);
-  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);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level)
-{
-  boost::mutex::scoped_lock lock(mutex_);
-
-  Eigen::Vector3f leaf_size = impl_.getLeafSize();
-
-  if (leaf_size[0] != config.leaf_size) {
-    leaf_size.setConstant(config.leaf_size);
-    NODELET_DEBUG("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
-    impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]);
   }
+  declare_parameter(leaf_size_desc.name, rclcpp::ParameterValue(0.01), leaf_size_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor min_points_per_voxel_desc;
+  min_points_per_voxel_desc.name = "min_points_per_voxel";
+  min_points_per_voxel_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+  min_points_per_voxel_desc.description =
+    "The minimum number of points required for a voxel to be used.";
+  {
+    rcl_interfaces::msg::IntegerRange int_range;
+    int_range.from_value = 1;
+    int_range.to_value = 100000;
+    min_points_per_voxel_desc.integer_range.push_back(int_range);
+  }
+  declare_parameter(
+    min_points_per_voxel_desc.name, rclcpp::ParameterValue(2), min_points_per_voxel_desc);
+
+  std::vector<std::string> param_names {
+    leaf_size_desc.name,
+    min_points_per_voxel_desc.name,
+  };
+  param_names.insert(param_names.end(), common_param_names.begin(), common_param_names.end());
+
+  callback_handle_ =
+    add_on_set_parameters_callback(
+    std::bind(
+      &VoxelGrid::config_callback, this,
+      std::placeholders::_1));
+
+  config_callback(get_parameters(param_names));
+
+  // TODO(daisukes): lazy subscription after rclcpp#2060
+  subscribe();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+rcl_interfaces::msg::SetParametersResult
+pcl_ros::VoxelGrid::config_callback(const std::vector<rclcpp::Parameter> & params)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
 
   double filter_min, filter_max;
   impl_.getFilterLimits(filter_min, filter_max);
-  if (filter_min != config.filter_limit_min) {
-    filter_min = config.filter_limit_min;
-    NODELET_DEBUG(
-      "[config_callback] Setting the minimum filtering value a point will be considered "
-      "from to: %f.",
-      filter_min);
-  }
-  if (filter_max != config.filter_limit_max) {
-    filter_max = config.filter_limit_max;
-    NODELET_DEBUG(
-      "[config_callback] Setting the maximum filtering value a point will be considered "
-      "from to: %f.",
-      filter_max);
-  }
-  impl_.setFilterLimits(filter_min, filter_max);
 
-  if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
+  Eigen::Vector3f leaf_size = impl_.getLeafSize();
-    impl_.setFilterLimitsNegative(config.filter_limit_negative);
+
-    NODELET_DEBUG(
+  unsigned int minPointsPerVoxel = impl_.getMinimumPointsNumberPerVoxel();
-      "[%s::config_callback] Setting the filter negative flag to: %s.",
+
-      getName().c_str(), config.filter_limit_negative ? "true" : "false");
+  for (const rclcpp::Parameter & param : params) {
+    if (param.get_name() == "filter_field_name") {
+      // Check the current value for the filter field
+      if (impl_.getFilterFieldName() != param.as_string()) {
+        // Set the filter field if different
+        impl_.setFilterFieldName(param.as_string());
+        RCLCPP_DEBUG(
+          get_logger(), "Setting the filter field name to: %s.",
+          param.as_string().c_str());
+      }
+    }
+    if (param.get_name() == "filter_limit_min") {
+      // Check the current values for filter min-max
+      if (filter_min != param.as_double()) {
+        filter_min = param.as_double();
+        RCLCPP_DEBUG(
+          get_logger(),
+          "Setting the minimum filtering value a point will be considered from to: %f.",
+          filter_min);
+        // Set the filter min-max if different
+        impl_.setFilterLimits(filter_min, filter_max);
+      }
+    }
+    if (param.get_name() == "filter_limit_max") {
+      // Check the current values for filter min-max
+      if (filter_max != param.as_double()) {
+        filter_max = param.as_double();
+        RCLCPP_DEBUG(
+          get_logger(),
+          "Setting the maximum filtering value a point will be considered from to: %f.",
+          filter_max);
+        // Set the filter min-max if different
+        impl_.setFilterLimits(filter_min, filter_max);
+      }
+    }
+    if (param.get_name() == "filter_limit_negative") {
+      bool new_filter_limits_negative = param.as_bool();
+      if (impl_.getFilterLimitsNegative() != new_filter_limits_negative) {
+        RCLCPP_DEBUG(
+          get_logger(),
+          "Setting the filter negative flag to: %s.",
+          (new_filter_limits_negative ? "true" : "false"));
+        impl_.setFilterLimitsNegative(new_filter_limits_negative);
+      }
+    }
+    if (param.get_name() == "min_points_per_voxel") {
+      if (minPointsPerVoxel != ((unsigned int) param.as_int())) {
+        RCLCPP_DEBUG(
+          get_logger(),
+          "Setting the minimum points per voxel to: %u.",
+          minPointsPerVoxel);
+        impl_.setMinimumPointsNumberPerVoxel(param.as_int());
+      }
+    }
+    if (param.get_name() == "leaf_size") {
+      leaf_size.setConstant(param.as_double());
+      if (impl_.getLeafSize() != leaf_size) {
+        RCLCPP_DEBUG(
+          get_logger(), "Setting the downsampling leaf size to: %f %f %f.",
+          leaf_size[0], leaf_size[1], leaf_size[2]);
+        impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]);
+      }
+    }
   }
 
-  if (impl_.getFilterFieldName() != config.filter_field_name) {
+  // Range constraints are enforced by rclcpp::Parameter.
-    impl_.setFilterFieldName(config.filter_field_name);
+  rcl_interfaces::msg::SetParametersResult result;
-    NODELET_DEBUG(
+  result.successful = true;
-      "[config_callback] Setting the filter field name to: %s.",
+  return result;
-      config.filter_field_name.c_str());
-  }
-
-  // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves,
-  // we'll remove them and inherit from Filter
-  if (tf_input_frame_ != config.input_frame) {
-    tf_input_frame_ = config.input_frame;
-    NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str());
-  }
-  if (tf_output_frame_ != config.output_frame) {
-    tf_output_frame_ = config.output_frame;
-    NODELET_DEBUG(
-      "[config_callback] Setting the output TF frame to: %s.",
-      tf_output_frame_.c_str());
-  }
-  // ]---
 }
 
-typedef pcl_ros::VoxelGrid VoxelGrid;
+#include "rclcpp_components/register_node_macro.hpp"
-PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet);
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::VoxelGrid)

pcl_ros/tests/filters/CMakeLists.txt

@@ -59,6 +59,12 @@ ament_add_pytest_test(test_pcl_ros::CropBox
       FILTER_PLUGIN=pcl_ros::CropBox
   APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
 )
+ament_add_pytest_test(test_pcl_ros::VoxelGrid
+  test_filter_component.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_PLUGIN=pcl_ros::VoxelGrid
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)
 
 # test executables
 ament_add_pytest_test(test_filter_extract_indices_node
@@ -98,3 +104,9 @@ ament_add_pytest_test(test_filter_crop_box_node
       FILTER_EXECUTABLE=filter_crop_box_node
   APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
 )
+ament_add_pytest_test(test_filter_voxel_grid_node
+  test_filter_executable.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_EXECUTABLE=filter_voxel_grid_node
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)