diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 07ed359c..e9c01ad8 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -87,7 +87,7 @@ add_library(pcl_ros_filters SHARED 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/statistical_outlier_removal.cpp # src/pcl_ros/filters/voxel_grid.cpp # src/pcl_ros/filters/crop_box.cpp ) @@ -109,6 +109,10 @@ 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 +) class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp index 80f4ea64..84e5c434 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/StatisticalOutlierRemovalConfig.hpp" - namespace pcl_ros { /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more @@ -61,9 +59,6 @@ namespace pcl_ros class StatisticalOutlierRemoval : 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 @@ -71,10 +66,10 @@ protected: */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*(input), *(pcl_input)); impl_.setInputCloud(pcl_input); @@ -84,17 +79,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 */ - 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::StatisticalOutlierRemovalConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -102,6 +93,8 @@ private: public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit StatisticalOutlierRemoval(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp index e7a48a9b..a4cebaa9 100644 --- a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -35,53 +35,105 @@ * */ -#include #include "pcl_ros/filters/statistical_outlier_removal.hpp" -////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::StatisticalOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service) +pcl_ros::StatisticalOutlierRemoval::StatisticalOutlierRemoval(const rclcpp::NodeOptions & options) +: Filter("StatisticalOutlierRemovalNode", options) { - // Enable the dynamic reconfigure service - has_service = true; - srv_ = boost::make_shared>( - nh); - dynamic_reconfigure::Server::CallbackType f = - boost::bind(&StatisticalOutlierRemoval::config_callback, this, _1, _2); - srv_->setCallback(f); + rcl_interfaces::msg::ParameterDescriptor mean_k_desc; + mean_k_desc.name = "mean_k"; + mean_k_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + mean_k_desc.description = + "The number of points (k) to use for mean distance estimation."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 2; + int_range.to_value = 100; + mean_k_desc.integer_range.push_back(int_range); + } + declare_parameter(mean_k_desc.name, rclcpp::ParameterValue(2), mean_k_desc); - return true; + rcl_interfaces::msg::ParameterDescriptor stddev_desc; + stddev_desc.name = "stddev"; + stddev_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + stddev_desc.description = + "The standard deviation multiplier threshold." + "All points outside the mean +- sigma * std_mul will be considered outliers."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 5.0; + stddev_desc.floating_point_range.push_back(float_range); + } + declare_parameter(stddev_desc.name, rclcpp::ParameterValue(0.0), stddev_desc); + + rcl_interfaces::msg::ParameterDescriptor negative_desc; + negative_desc.name = "negative"; + negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + negative_desc.description = + "Set whether the inliers should be returned (true) or the outliers (false)."; + declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc); + + const std::vector param_names { + mean_k_desc.name, + stddev_desc.name, + negative_desc.name, + }; + + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &StatisticalOutlierRemoval::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); } + ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::StatisticalOutlierRemoval::config_callback( - pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level) +rcl_interfaces::msg::SetParametersResult +pcl_ros::StatisticalOutlierRemoval::config_callback(const std::vector & params) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); - if (impl_.getMeanK() != config.mean_k) { - impl_.setMeanK(config.mean_k); - NODELET_DEBUG( - "[%s::config_callback] Setting the number of points (k) to use for mean " - "distance estimation to: %d.", - getName().c_str(), config.mean_k); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "mean_k") { + if (impl_.getMeanK() != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the number of points (k) to use for mean distance estimation to: %ld.", + param.as_int()); + impl_.setMeanK(param.as_int()); + } + } + if (param.get_name() == "stddev") { + if (impl_.getStddevMulThresh() != param.as_double()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the standard deviation multiplier threshold to: %f.", + param.as_double()); + impl_.setStddevMulThresh(param.as_double()); + } + } + if (param.get_name() == "negative") { + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), + "Returning only inliers: %s.", + (param.as_bool() ? "false" : "true")); + impl_.setNegative(param.as_bool()); + } + } } - if (impl_.getStddevMulThresh() != config.stddev) { - impl_.setStddevMulThresh(config.stddev); - NODELET_DEBUG( - "[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", - getName().c_str(), config.stddev); - } - - if (impl_.getNegative() != config.negative) { - impl_.setNegative(config.negative); - NODELET_DEBUG( - "[%s::config_callback] Returning only inliers: %s.", - getName().c_str(), config.negative ? "false" : "true"); - } + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } -typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval; -PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval, nodelet::Nodelet); +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::StatisticalOutlierRemoval) diff --git a/pcl_ros/tests/filters/CMakeLists.txt b/pcl_ros/tests/filters/CMakeLists.txt index 40d3165e..14f37c6a 100644 --- a/pcl_ros/tests/filters/CMakeLists.txt +++ b/pcl_ros/tests/filters/CMakeLists.txt @@ -47,6 +47,12 @@ ament_add_pytest_test(test_pcl_ros::RadiusOutlierRemoval FILTER_PLUGIN=pcl_ros::RadiusOutlierRemoval APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index ) +ament_add_pytest_test(test_pcl_ros::StatisticalOutlierRemoval + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::StatisticalOutlierRemoval + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) # test executables ament_add_pytest_test(test_filter_extract_indices_node @@ -74,3 +80,9 @@ ament_add_pytest_test(test_filter_radius_outlier_removal_node FILTER_EXECUTABLE=filter_radius_outlier_removal_node APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index ) +ament_add_pytest_test(test_filter_statistical_outlier_removal_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_statistical_outlier_removal_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +)