Used the maximum allowed inclination as a parameter.

Solved a bug with the use of abs() instead of fabs().

cfg/ObstacleDetectionNormals.cfg

@@ -39,9 +39,7 @@ gen = ParameterGenerator()
 
 #       Name                       Type       Reconfiguration level  Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,  0,                     "Maximum velocity scale factor",  0.5,      0.0,  1.0)
-gen.add("filter_min_dist",         double_t,  0,                     "Min distance threshold",         0.3,        0,  100)
+gen.add("filter_min_dist",         double_t,  0,                     "Min distance threshold",         0.3,      0,  100)
 gen.add("normal_KSearch",          int_t,     0,                     "No of neighbors to estimate the surface",100,10, 200)
-gen.add("normal_x",                double_t,  0,                     "Accepted Normal X component",    1,)
-gen.add("normal_y",                double_t,  0,                     "Accepted Normal Y component",    -0.2,)
-gen.add("normal_z",                double_t,  0,                     "Accepted Normal Z component",    1,)
+gen.add("max_inc",                 double_t,  0,                     "maximum inclination in rad",     0.25,     0.0,  1.0)
 exit(gen.generate(PACKAGE, "ObstacleDetectionNormalsAlgorithm", "ObstacleDetectionNormals"))

config/default_config.yaml

 filter_min_dist: 0.3
 normal_KSearch: 30
-normal_x: 1.001
-normal_y: 1.001
-normal_z: 0.95
+max_inc: 0.25

src/obstacle_detection_normals_alg.cpp

@@ -58,18 +58,14 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::Poi
 
   *cloud_obs=*cloud_out;
 
-  std::cout << &this->config_ << std::endl;
-  std::cout << this->config_.normal_z << "," << this->config_.normal_y << "," << this->config_.normal_x << std::endl;
   for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_out->height; ++rowIndex)
   {
     for (size_t colIndex=0; colIndex<cloud_out->width; ++colIndex, ++pointIndex)
     {
       if (cloud_out->points[pointIndex].x > this->config_.filter_min_dist)
       {
-//        std::cout << "(" << rowIndex << "," << pointIndex << "): " << cloud_out->points[pointIndex].normal_z << "," << cloud_out->points[pointIndex].normal_y << "," << cloud_out->points[pointIndex].normal_x << std::endl;
-        if (abs(cloud_out->points[pointIndex].normal_z) > this->config_.normal_z &&
-            cloud_out->points[pointIndex].normal_y < this->config_.normal_y &&
-            cloud_out->points[pointIndex].normal_x < this->config_.normal_x )
+        std::cout << "(" << rowIndex << "," << pointIndex << "): " << cloud_out->points[pointIndex].normal_z << "," << this->config_.max_inc << std::endl;
+        if (fabs(cloud_out->points[pointIndex].normal_z) > cos(this->config_.max_inc))
         {
           cloud_out->points[pointIndex].r=0;
           cloud_out->points[pointIndex].g=255;