new capability for choosing number of cells with a param

cfg/HoleDetection.cfg

@@ -29,7 +29,7 @@
 #*  POSSIBILITY OF SUCH DAMAGE.
 #***********************************************************
 
-# Author: 
+# Author:
 
 PACKAGE='iri_hole_detection'
 
@@ -40,6 +40,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(            "hole_min_p",  int_t,     0,             "No of Points to be hole (less points is a hole)",   30,       5,  200)
+gen.add(            "num_cells",  int_t,     0,             "No of cells in the detections zone",   3,       1,  10)
 gen.add(           "box_y",  double_t,  0,                              "Y distance hole detection zone",  0.6,        0,  1.5)
 gen.add(           "box_z_ini",  double_t,  0,                              "Z distance hole detection zone", 0,     -0.5,    0.5)
 gen.add(            "box_z_end",  double_t,  0,                               "Initial Z hole detection zone",    0.1,     -0.5,  0.5)

include/hole_detection_alg.h

@@ -55,7 +55,7 @@ class HoleDetectionAlgorithm
     CMutex alg_mutex_;
 
     // private attributes and methods
-    int hole_min_p;
+    int hole_min_p,num_cells;
     float box_z_end,box_x_ini,box_y,box_z_ini,box_x_end,Yl,Yc,Yr,Z;
     pcl::PointCloud<pcl::PointXYZRGB> cloud_in;
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for;
@@ -134,8 +134,8 @@ class HoleDetectionAlgorithm
     ~HoleDetectionAlgorithm(void);
 
      void cloud_all(int hole_min_p, float box_z_end, float box_x_ini,
-                    float box_y, float box_z_ini, float box_x_end,
-                    float Yl, float Yc, float Yr, float Z, 
+                    float box_y, float box_z_ini, float box_x_end, int num_cells,
+                    float Yl, float Yc, float Yr, float Z,
                     pcl::PointCloud<pcl::PointXYZRGB>& cloud_in,
                     pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out);
 

include/hole_detection_alg_node.h

@@ -55,7 +55,7 @@ class HoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<HoleDetecti
 {
   private:
 
-		int hole_min_p;
+		int hole_min_p,num_cells;
     float box_z_end,box_x_ini,box_z_ini,box_x_end,box_y,Yl,Yc,Yr,Z;
 //    bool L,C,R;
 

src/hole_detection_alg.cpp

@@ -27,7 +27,7 @@ points, there is a hole so it generates a virtual obstacle in order to notify
 the navigation stack.
 */
 void HoleDetectionAlgorithm::cloud_all(int hole_min_p, float box_z_end, float box_x_ini,
-               float box_y, float box_z_ini, float box_x_end,
+               float box_y, float box_z_ini, float box_x_end, int num_cells,
                float Yl, float Yc, float Yr, float Z,
                pcl::PointCloud<pcl::PointXYZRGB>& cloud_in,
                pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_for)
@@ -36,6 +36,8 @@ int l=0;
 int c=0;
 int r=0;
 
+int p [num_cells] = {};
+
 //////////////////////////////////////////////////////////////////////////////
 // Hole detection box generation
 //////////////////////////////////////////////////////////////////////////////
@@ -48,6 +50,21 @@ for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
       box_x_ini < cloud_in.points[pointIndex].x &&
       cloud_in.points[pointIndex].x < box_x_end)
     {
+      for (int cell=0; cell<num_cells; ++cell)
+      {
+        if ( (box_y/2)-((num_cells-cell)*(box_y/num_cells)) < cloud_in.points[pointIndex].y &&
+          cloud_in.points[pointIndex].y < (box_y/2)-((num_cells-cell-1)*(box_y/num_cells)) )
+        {
+          cloud_in.points[pointIndex].r=0;
+          cloud_in.points[pointIndex].g=int(255*(float(cell)/float(max(1,num_cells-1))));
+          cloud_in.points[pointIndex].b=255;
+          p[cell]=p[cell]+1;
+        }
+      }
+
+
+
+/*
       if (-(box_y/2) < cloud_in.points[pointIndex].y &&
         cloud_in.points[pointIndex].y < ((box_y/3)-(box_y/2)))
       {
@@ -72,6 +89,7 @@ for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
         cloud_in.points[pointIndex].b=255;
         r=r+1;
       }
+      */
     }
   }
 }
@@ -81,6 +99,7 @@ for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
 //////////////////////////////////////////////////////////////////////////////
 Z=0.1;
 
+/*
 for (size_t rowIndex=0, pointIndex=0; rowIndex<5; ++rowIndex)
 {
   Yl= -(box_y/2);
@@ -106,5 +125,22 @@ for (size_t rowIndex=0, pointIndex=0; rowIndex<5; ++rowIndex)
   }
   Z=Z+0.05;
 }
+*/
+
+
+for (size_t rowIndex=0, pointIndex=0; rowIndex<5; ++rowIndex)
+{
+  for (size_t colIndex=0; colIndex<5; ++colIndex, ++pointIndex)
+  {
+    for (int cell=0; cell<num_cells; ++cell)
+    {
+      if (p[cell] < hole_min_p)
+      {
+        cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, (box_y/2)-((num_cells-cell)*(box_y/num_cells)) + 0.05*colIndex, Z));
+      }
+    }
+  }
+  Z=Z+0.05;
+}
 
 }

src/hole_detection_alg_node.cpp

@@ -2,7 +2,7 @@
 
 HoleDetectionAlgNode::HoleDetectionAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<HoleDetectionAlgorithm>(),hole_min_p(60),box_z_end(0),box_x_ini(1),
-  box_z_ini(-0.1),box_x_end(1.8),box_y(0.9)
+  box_z_ini(-0.1),box_x_end(1.8),box_y(0.9),num_cells(3)
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
@@ -81,7 +81,7 @@ void HoleDetectionAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstP
   cloud_in_rgb.is_dense=false;
   //cloud_in_rgb.header.stamp = ros::Time::now ();
 
-  hole_detect.cloud_all(hole_min_p, box_z_end, box_x_ini, box_y, box_z_ini, box_x_end, Yl, Yc, Yr, Z, cloud_in_rgb, cloud_for);
+  hole_detect.cloud_all(hole_min_p, box_z_end, box_x_ini, box_y, box_z_ini, box_x_end, num_cells, Yl, Yc, Yr, Z, cloud_in_rgb, cloud_for);
   //unlock previously blocked shared variables
   this->alg_.unlock();
 
@@ -125,6 +125,7 @@ box_x_ini=config.box_x_ini;
 box_x_end=config.box_x_end;
 box_z_ini=config.box_z_ini;
 box_y=config.box_y;
+num_cells=config.num_cells;
   this->alg_.unlock();
 }