diff --git a/cfg/HoleDetection.cfg b/cfg/HoleDetection.cfg
index 2ce0503b7b2848fbf2b32c3fb4cbe9a581aef400..b3833a6e5b6e41583466db0e10be772b6dfec4b3 100755
--- a/cfg/HoleDetection.cfg
+++ b/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)
diff --git a/include/hole_detection_alg.h b/include/hole_detection_alg.h
index fdeccc2135ed2d2568eee32e4a886b8301bf4368..3346067420075affa41069313e1632537c659af2 100644
--- a/include/hole_detection_alg.h
+++ b/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);
diff --git a/include/hole_detection_alg_node.h b/include/hole_detection_alg_node.h
index f7edab6d01048c82818ca5128380b6747283e713..c6621479078d05f5e7b9fdd3647d8c70357380f8 100644
--- a/include/hole_detection_alg_node.h
+++ b/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;
diff --git a/src/hole_detection_alg.cpp b/src/hole_detection_alg.cpp
index b0e9641788f634e1465466f0de12b5e5ae0e52e6..f151cfaf0ecf67a5e30a64199101da8f82942770 100644
--- a/src/hole_detection_alg.cpp
+++ b/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;
+}
}
diff --git a/src/hole_detection_alg_node.cpp b/src/hole_detection_alg_node.cpp
index 5d84c3189b0fb1faab41260441dc12f4db7ce038..53a5224415442515d506d82dbc71bf6be1cf649c 100644
--- a/src/hole_detection_alg_node.cpp
+++ b/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();
}