Migrate the ROS1 pcl_ros::RadiusOutlierRemoval filter to ROS2 (#399)

* Add the RadiusOutlierRemoval filter.

* Lint

* PLay iit again, Linty Sam.

* Header order

* This is now just getting embarrassing

* Move parameter to class constructor.

pcl_ros/CMakeLists.txt

@@ -86,7 +86,7 @@ add_library(pcl_ros_filters SHARED
   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/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
@@ -105,6 +105,10 @@ 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
+)
 class_loader_hide_library_symbols(pcl_ros_filters)
 #
 ### Declare the pcl_ros_segmentation library

pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp

@@ -40,11 +40,9 @@
 
 // PCL includes
 #include <pcl/filters/radius_outlier_removal.h>
+#include <vector>
 #include "pcl_ros/filters/filter.hpp"
 
-// Dynamic reconfigure
-#include "pcl_ros/RadiusOutlierRemovalConfig.hpp"
-
 namespace pcl_ros
 {
 /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
@@ -55,9 +53,6 @@ namespace pcl_ros
 class RadiusOutlierRemoval : public Filter
 {
 protected:
-  /** \brief Pointer to a dynamic reconfigure service. */
-  boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>> 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
@@ -65,9 +60,10 @@ protected:
     */
   inline void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
-    PointCloud2 & output)
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    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);
@@ -77,17 +73,13 @@ protected:
     pcl_conversions::moveFromPCL(pcl_output, output);
   }
 
-  /** \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 inline bool child_init(ros::NodeHandle & nh, bool & has_service);
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
 
-  /** \brief Dynamic reconfigure callback
-    * \param config the config object
-    * \param level the dynamic reconfigure level
-    */
-  void config_callback(pcl_ros::RadiusOutlierRemovalConfig & config, uint32_t level);
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 
 private:
   /** \brief The PCL filter implementation used. */
@@ -95,6 +87,8 @@ private:
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit RadiusOutlierRemoval(const rclcpp::NodeOptions & options);
 };
 }  // namespace pcl_ros
 

pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp

@@ -35,46 +35,84 @@
  *
  */
 
-#include <pluginlib/class_list_macros.h>
 #include "pcl_ros/filters/radius_outlier_removal.hpp"
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-bool
-pcl_ros::RadiusOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service)
+pcl_ros::RadiusOutlierRemoval::RadiusOutlierRemoval(const rclcpp::NodeOptions & options)
+: Filter("RadiusOutlierRemovalNode", options)
 {
-  // Enable the dynamic reconfigure service
-  has_service = true;
-  srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>>(nh);
-  dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>::CallbackType f = boost::bind(
-    &RadiusOutlierRemoval::config_callback, this, _1, _2);
-  srv_->setCallback(f);
+  rcl_interfaces::msg::ParameterDescriptor min_neighbors_desc;
+  min_neighbors_desc.name = "min_neighbors";
+  min_neighbors_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+  min_neighbors_desc.description =
+    "The number of neighbors that need to be present in order to be classified as an inlier.";
+  {
+    rcl_interfaces::msg::IntegerRange int_range;
+    int_range.from_value = 0;
+    int_range.to_value = 1000;
+    min_neighbors_desc.integer_range.push_back(int_range);
+  }
+  declare_parameter(min_neighbors_desc.name, rclcpp::ParameterValue(5), min_neighbors_desc);
 
-  return true;
+  rcl_interfaces::msg::ParameterDescriptor radius_search_desc;
+  radius_search_desc.name = "radius_search";
+  radius_search_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  radius_search_desc.description =
+    "Radius of the sphere that will determine which points are neighbors.";
+  {
+    rcl_interfaces::msg::FloatingPointRange float_range;
+    float_range.from_value = 0.0;
+    float_range.to_value = 10.0;
+    radius_search_desc.floating_point_range.push_back(float_range);
+  }
+  declare_parameter(radius_search_desc.name, rclcpp::ParameterValue(0.1), radius_search_desc);
+
+  const std::vector<std::string> param_names {
+    min_neighbors_desc.name,
+    radius_search_desc.name,
+  };
+
+  callback_handle_ =
+    add_on_set_parameters_callback(
+    std::bind(
+      &RadiusOutlierRemoval::config_callback, this,
+      std::placeholders::_1));
+
+  config_callback(get_parameters(param_names));
+
+  // TODO(daisukes): lazy subscription after rclcpp#2060
+  subscribe();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl_ros::RadiusOutlierRemoval::config_callback(
-  pcl_ros::RadiusOutlierRemovalConfig & config,
-  uint32_t level)
+rcl_interfaces::msg::SetParametersResult
+pcl_ros::RadiusOutlierRemoval::config_callback(const std::vector<rclcpp::Parameter> & params)
 {
-  boost::mutex::scoped_lock lock(mutex_);
+  std::lock_guard<std::mutex> lock(mutex_);
 
-  if (impl_.getMinNeighborsInRadius() != config.min_neighbors) {
-    impl_.setMinNeighborsInRadius(config.min_neighbors);
-    NODELET_DEBUG(
-      "[%s::config_callback] Setting the number of neighbors in radius: %d.",
-      getName().c_str(), config.min_neighbors);
+  for (const rclcpp::Parameter & param : params) {
+    if (param.get_name() == "min_neighbors") {
+      if (impl_.getMinNeighborsInRadius() != param.as_int()) {
+        RCLCPP_DEBUG(
+          get_logger(), "Setting the number of neighbors in radius: %ld.",
+          param.as_int());
+        impl_.setMinNeighborsInRadius(param.as_int());
+      }
+    }
+    if (param.get_name() == "radius_search") {
+      if (impl_.getRadiusSearch() != param.as_double()) {
+        RCLCPP_DEBUG(
+          get_logger(), "Setting the radius to search neighbors: %f.",
+          param.as_double());
+        impl_.setRadiusSearch(param.as_double());
       }
-
-  if (impl_.getRadiusSearch() != config.radius_search) {
-    impl_.setRadiusSearch(config.radius_search);
-    NODELET_DEBUG(
-      "[%s::config_callback] Setting the radius to search neighbors: %f.",
-      getName().c_str(), config.radius_search);
     }
   }
 
+  // Range constraints are enforced by rclcpp::Parameter.
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  return result;
+}
 
-typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval;
-PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval, nodelet::Nodelet);
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::RadiusOutlierRemoval)

pcl_ros/tests/filters/CMakeLists.txt

@@ -41,6 +41,12 @@ ament_add_pytest_test(test_pcl_ros::ProjectInliers
       PARAMETERS={'model_type':0}
   APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
 )
+ament_add_pytest_test(test_pcl_ros::RadiusOutlierRemoval
+  test_filter_component.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_PLUGIN=pcl_ros::RadiusOutlierRemoval
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)
 
 # test executables
 ament_add_pytest_test(test_filter_extract_indices_node
@@ -62,3 +68,9 @@ ament_add_pytest_test(test_filter_project_inliers_node
       PARAMETERS={'model_type':0}
   APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
 )
+ament_add_pytest_test(test_filter_radius_outlier_removal_node
+  test_filter_executable.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_EXECUTABLE=filter_radius_outlier_removal_node
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)