From d245f70f4f0859f849458886249fa25a994def23 Mon Sep 17 00:00:00 2001 From: Lucid One Date: Tue, 26 May 2015 16:01:16 -0400 Subject: [PATCH] Fixes #87 for Indigo --- pcl_ros/CMakeLists.txt | 1 + pcl_ros/cfg/RadiusOutlierRemoval.cfg | 15 ++++++++ .../pcl_ros/filters/radius_outlier_removal.h | 16 ++++++++- pcl_ros/pcl_nodelets.xml | 5 +++ .../filters/radius_outlier_removal.cpp | 34 +++++++++++++++++++ 5 files changed, 70 insertions(+), 1 deletion(-) create mode 100755 pcl_ros/cfg/RadiusOutlierRemoval.cfg diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 3192e07c..a9275ac8 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -52,6 +52,7 @@ generate_dynamic_reconfigure_options( cfg/SACSegmentationFromNormals.cfg cfg/SegmentDifferences.cfg cfg/StatisticalOutlierRemoval.cfg + cfg/RadiusOutlierRemoval.cfg cfg/VoxelGrid.cfg cfg/CropBox.cfg ) diff --git a/pcl_ros/cfg/RadiusOutlierRemoval.cfg b/pcl_ros/cfg/RadiusOutlierRemoval.cfg new file mode 100755 index 00000000..d54962e6 --- /dev/null +++ b/pcl_ros/cfg/RadiusOutlierRemoval.cfg @@ -0,0 +1,15 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'pcl_ros' + +from dynamic_reconfigure.parameter_generator_catkin import *; + +gen = ParameterGenerator () + +# enabling/disabling the unit limits +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("radius_search", double_t, 0, "Radius of the sphere that will determine which points are neighbors.", 0.1, 0.0, 10.0) +gen.add ("min_neighbors", int_t, 0, "The number of neighbors that need to be present in order to be classified as an inlier.", 5, 0, 1000) + +exit (gen.generate (PACKAGE, "pcl_ros", "RadiusOutlierRemoval")) diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h index cb75eaee..6120f9e5 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h @@ -42,6 +42,9 @@ #include #include "pcl_ros/filters/filter.h" +// Dynamic reconfigure +#include "pcl_ros/RadiusOutlierRemovalConfig.h" + namespace pcl_ros { /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain @@ -52,6 +55,9 @@ 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 @@ -72,9 +78,17 @@ namespace pcl_ros /** \brief Child initialization routine. * \param nh ROS node handle + * \param has_service set to true if the child has a Dynamic Reconfigure service */ - virtual inline bool child_init (ros::NodeHandle &nh) { return (true); } + virtual inline bool child_init (ros::NodeHandle &nh, bool &has_service); + /** \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); + + private: /** \brief The PCL filter implementation used. */ pcl::RadiusOutlierRemoval impl_; diff --git a/pcl_ros/pcl_nodelets.xml b/pcl_ros/pcl_nodelets.xml index 64fa27d8..71978735 100644 --- a/pcl_ros/pcl_nodelets.xml +++ b/pcl_ros/pcl_nodelets.xml @@ -130,6 +130,11 @@ StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. + + + RadiusOutlierRemoval uses point neighborhood statistics to filter outlier data. + + 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 da965c69..46c00a14 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -38,6 +38,40 @@ #include #include "pcl_ros/filters/radius_outlier_removal.h" +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl_ros::RadiusOutlierRemoval::child_init (ros::NodeHandle &nh, bool &has_service) +{ + // 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); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::RadiusOutlierRemoval::config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level) +{ + boost::mutex::scoped_lock 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); + } + + 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); + } + +} + + typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval,nodelet::Nodelet);