diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index c51d3b1f..07ed359c 100644 --- a/pcl_ros/CMakeLists.txt +++ b/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 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 420f4df5..66e2b2e7 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #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> 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 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 & 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 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 0a01b0b1..c6adb4a2 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -35,46 +35,84 @@ * */ -#include #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>(nh); - dynamic_reconfigure::Server::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 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 & params) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard 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) diff --git a/pcl_ros/tests/filters/CMakeLists.txt b/pcl_ros/tests/filters/CMakeLists.txt index edbf1dae..40d3165e 100644 --- a/pcl_ros/tests/filters/CMakeLists.txt +++ b/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 +)