Fixes #87 for Indigo

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
 )

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"))

pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h

@@ -42,6 +42,9 @@
 #include <pcl/filters/radius_outlier_removal.h>
 #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 <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
@@ -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<pcl::PCLPointCloud2> impl_;

pcl_ros/pcl_nodelets.xml

@@ -130,6 +130,11 @@
       StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data.
     </description>
   </class>
+  <class name="pcl/RadiusOutlierRemoval" type="RadiusOutlierRemoval" base_class_type="nodelet::Nodelet">
+    <description>
+      RadiusOutlierRemoval uses point neighborhood statistics to filter outlier data.
+    </description>
+  </class>
 
   <class name="pcl/CropBox" type="CropBox" base_class_type="nodelet::Nodelet">
     <description>

pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp

@@ -38,6 +38,40 @@
 #include <pluginlib/class_list_macros.h>
 #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 <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);
+
+  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);