Merge from axis_Z_to_X_migration to master

CMakeLists.txt

@@ -2,7 +2,7 @@
 #                 The new CMakeLists.txt file starts here                
 # ******************************************************************** 
 cmake_minimum_required(VERSION 2.8.3)
-project(iri_hole_detection)
+project(iri_nav_hole_detection)
 
 ## Find catkin macros and libraries
 #find_package(catkin REQUIRED)
@@ -87,7 +87,7 @@ include_directories(${iriutils_INCLUDE_DIR} ./include)
 # add_library(${PROJECT_NAME} <list of source files>)
 
 ## Declare a cpp executable
-add_executable(${PROJECT_NAME} src/hole_detection_alg.cpp src/hole_detection_alg_node.cpp)
+add_executable(${PROJECT_NAME} src/iri_nav_hole_detection_alg.cpp src/iri_nav_hole_detection_alg_node.cpp)
 
 # ******************************************************************** 
 #                   Add the libraries

cfg/HoleDetection.cfg

@@ -29,9 +29,9 @@
 #*  POSSIBILITY OF SUCH DAMAGE.
 #***********************************************************
 
-# Author: 
+# Author:
 
-PACKAGE='iri_hole_detection'
+PACKAGE='iri_nav_hole_detection'
 
 from dynamic_reconfigure.parameter_generator_catkin import *
 
@@ -40,12 +40,12 @@ 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(           "box_x",  double_t,  0,                              "X distance hole detection zone",  0.6,        0,  1.5)
-gen.add(           "box_y_ini",  double_t,  0,                              "Y distance hole detection zone", 0,     -0.5,    0.5)
-gen.add(            "box_y_end",  double_t,  0,                               "Initial Y hole detection zone",    0.1,     -0.5,  0.5)
-gen.add(            "box_z_ini",  double_t,  0,                               "Initial Z hole detection zone",    0.8,      0,    2)
-gen.add(           "box_z_end",  double_t,  0,                              "Z distance hole detection zone",  0.9,        0.5,    2)
-
+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,                              "Initial Z hole detection zone", 0,     -0.5,    0.5)
+gen.add(            "box_z_end",  double_t,  0,                               "Z distance hole detection zone",    0.1,     -0.5,  0.5)
+gen.add(            "box_x_ini",  double_t,  0,                               "Initial X hole detection zone",    0.8,      0,    2)
+gen.add(           "box_x_end",  double_t,  0,                              "X distance hole detection zone",  0.9,        0.5,    2)
 
 
 exit(gen.generate(PACKAGE, "HoleDetectionAlgorithm", "HoleDetection"))

include/hole_detection_alg.h → include/iri_nav_hole_detection_alg.h

@@ -22,14 +22,14 @@
 // refer to the IRI wiki page for more information:
 // http://wikiri.upc.es/index.php/Robotics_Lab
 
-#ifndef _hole_detection_alg_h_
-#define _hole_detection_alg_h_
+#ifndef _iri_nav_hole_detection_alg_h_
+#define _iri_nav_hole_detection_alg_h_
 
-#include <iri_hole_detection/HoleDetectionConfig.h>
+#include <iri_nav_hole_detection/HoleDetectionConfig.h>
 #include "mutex.h"
 
 #include <pcl_ros/point_cloud.h>
-#include <pcl/ros/conversions.h>
+#include <pcl/conversions.h>
 #include <pcl/point_types.h>
 #include <pcl/impl/point_types.hpp>
 
@@ -55,8 +55,8 @@ class HoleDetectionAlgorithm
     CMutex alg_mutex_;
 
     // private attributes and methods
-    int hole_min_p; 
-    float box_y_end,box_z_ini,box_x,box_y_ini,box_z_end,Xl,Xc,Xr,Y;
+    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;   
   public:
@@ -66,7 +66,7 @@ class HoleDetectionAlgorithm
     * Define a Config type with the HoleDetectionConfig. All driver implementations
     * will then use the same variable type Config.
     */
-    typedef iri_hole_detection::HoleDetectionConfig Config;
+    typedef iri_nav_hole_detection::HoleDetectionConfig Config;
 
    /**
     * \brief config variable
@@ -132,6 +132,7 @@ class HoleDetectionAlgorithm
     *
     */
     ~HoleDetectionAlgorithm(void);
+<<<<<<< HEAD:include/hole_detection_alg.h
     
      void cloud_all(int hole_min_p, float box_y_end, float box_z_ini, 
                     float box_x, float box_y_ini, float box_z_end, 
@@ -139,6 +140,14 @@ class HoleDetectionAlgorithm
                     pcl::PointCloud<pcl::PointXYZRGB>& cloud_in, 
                     pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out);    
        
+=======
+
+     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, int num_cells, float Z,
+                    pcl::PointCloud<pcl::PointXYZRGB>& cloud_in,
+                    pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out);
+
+>>>>>>> axis_Z_to_axis_X_migration:include/iri_nav_hole_detection_alg.h
 };
 
 #endif

include/hole_detection_alg_node.h → include/iri_nav_hole_detection_alg_node.h

@@ -22,17 +22,17 @@
 // refer to the IRI wiki page for more information:
 // http://wikiri.upc.es/index.php/Robotics_Lab
 
-#ifndef _hole_detection_alg_node_h_
-#define _hole_detection_alg_node_h_
+#ifndef _iri_nav_hole_detection_alg_node_h_
+#define _iri_nav_hole_detection_alg_node_h_
 
 #include <iri_base_algorithm/iri_base_algorithm.h>
-#include "hole_detection_alg.h"
+#include "iri_nav_hole_detection_alg.h"
 
 #include <ros/ros.h>
 //#include <ros/publisher.h>
 
 #include <pcl_ros/point_cloud.h>
-#include <pcl/ros/conversions.h>
+#include <pcl/conversions.h>
 #include <pcl/point_types.h>
 
 
@@ -54,9 +54,9 @@ using namespace std;
 class HoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<HoleDetectionAlgorithm>
 {
   private:
-		
-		int hole_min_p;
-    float box_y_end,box_z_ini,box_y_ini,box_z_end,box_x,Xl,Xc,Xr,Y;
+
+    int hole_min_p,num_cells;
+    float box_z_end,box_x_ini,box_z_ini,box_x_end,box_y,Z;
 //    bool L,C,R;
 		
     // [publisher attributes]

package.xml

 <?xml version="1.0"?>
 <package>
-  <name>iri_hole_detection</name>
+  <name>iri_nav_hole_detection</name>
   <version>1.0.0</version>
-  <description>iri_hole_detection</description>
+  <description>iri_nav_hole_detection</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->

src/hole_detection_alg.cpp → src/iri_nav_hole_detection_alg.cpp

-#include "hole_detection_alg.h"
+#include "iri_nav_hole_detection_alg.h"
 using namespace std;
 
 HoleDetectionAlgorithm::HoleDetectionAlgorithm(void)
@@ -15,91 +15,78 @@ void HoleDetectionAlgorithm::config_update(Config& new_cfg, uint32_t level)
 
   // save the current configuration
   this->config_=new_cfg;
-  
+
   this->unlock();
 }
 
 // HoleDetectionAlgorithm Public API
-
-void HoleDetectionAlgorithm::cloud_all(int hole_min_p, float box_y_end, float box_z_ini, 
-               float box_x, float box_y_ini, float box_z_end, 
-               float Xl, float Xc, float Xr, float Y, 
-               pcl::PointCloud<pcl::PointXYZRGB>& cloud_in, 
+/*
+It selects the points inside a box generated using the input params. These points
+are separated in three regions. If any of them has less than a minimum number of
+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, int num_cells, float Z,
+               pcl::PointCloud<pcl::PointXYZRGB>& cloud_in,
                pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_for)
 {
-int l=0;
-int c=0;
-int r=0;
+
+int p [num_cells] = {};
+float threshold [num_cells+1] = {};
+
+//////////////////////////////////////////////////////////////////////////////
+// Hole detection box generation
+//////////////////////////////////////////////////////////////////////////////
+for (int i=0; i<=num_cells; ++i)
+{
+  threshold[i] = (box_y/2)-((num_cells-i)*(box_y/num_cells));
+}
 
 
 for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
 {
   for (size_t colIndex=0; colIndex<cloud_in.width; ++colIndex, ++pointIndex)
   {
-    if (box_y_ini < cloud_in.points[pointIndex].y && 
-      cloud_in.points[pointIndex].y < box_y_end && 
-      box_z_ini < cloud_in.points[pointIndex].z && 
-      cloud_in.points[pointIndex].z < box_z_end)
+    if (box_z_ini < cloud_in.points[pointIndex].z &&
+      cloud_in.points[pointIndex].z < box_z_end &&
+      box_x_ini < cloud_in.points[pointIndex].x &&
+      cloud_in.points[pointIndex].x < box_x_end)
     {
-      if (-(box_x/2) < cloud_in.points[pointIndex].x && 
-        cloud_in.points[pointIndex].x < ((box_x/3)-(box_x/2)))
-      {
-        cloud_in.points[pointIndex].r=0;
-        cloud_in.points[pointIndex].g=0;
-        cloud_in.points[pointIndex].b=255;
-        l=l+1;
-      }
-      if (((box_x/3)-(box_x/2)) < cloud_in.points[pointIndex].x && 
-        cloud_in.points[pointIndex].x < ((box_x/2)-(box_x/3)))
+      for (int cell=0; cell<num_cells; ++cell)
       {
-        cloud_in.points[pointIndex].r=0;
-        cloud_in.points[pointIndex].g=255;
-        cloud_in.points[pointIndex].b=255;
-        c=c+1;
-      }
-      if (((box_x/2)-(box_x/3)) < cloud_in.points[pointIndex].x && 
-        cloud_in.points[pointIndex].x < (box_x/2))
-      {
-        cloud_in.points[pointIndex].r=0;
-        cloud_in.points[pointIndex].g=127;
-        cloud_in.points[pointIndex].b=255;
-        r=r+1;
+        if ( threshold[cell] < cloud_in.points[pointIndex].y &&
+          cloud_in.points[pointIndex].y < threshold[cell+1] )
+        {
+          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() bracket
   }
 }
 
 //////////////////////////////////////////////////////////////////////////////
-// Hole Point Cloud
+// Virtual obstacle generation
 //////////////////////////////////////////////////////////////////////////////
-
-
-Y=-0.1;
+Z=0.1;
 
 for (size_t rowIndex=0, pointIndex=0; rowIndex<5; ++rowIndex)
 {
-  Xl= -(box_x/2);
-  Xc= ((box_x/3)-(box_x/2));
-  Xr= ((box_x/2)-(box_x/3));
   for (size_t colIndex=0; colIndex<5; ++colIndex, ++pointIndex)
-        {
-    if (l<hole_min_p)
-    {
-      cloud_for->points.push_back (pcl::PointXYZ(Xl+0.05, Y, box_z_ini));
-      Xl=Xl+0.05;
-    }
-    if (c<hole_min_p)
-    {
-      cloud_for->points.push_back (pcl::PointXYZ(Xc+0.05, Y, box_z_ini));
-      Xc=Xc+0.05;
-    }
-    if (r<hole_min_p)
+  {
+    for (int cell=0; cell<num_cells; ++cell)
     {
-      cloud_for->points.push_back (pcl::PointXYZ(Xr+0.05, Y, box_z_ini));
-      Xr=Xr+0.05;
+      if (p[cell] < hole_min_p)
+      {
+        cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, threshold[cell] +
+          (threshold[cell+1]-threshold[cell])*colIndex/4, Z));
+      }
     }
   }
-  Y=Y-0.05;
+  Z=Z+0.05;
 }
 
-}
+}
\ No newline at end of file

src/hole_detection_alg_node.cpp → src/iri_nav_hole_detection_alg_node.cpp

-#include "hole_detection_alg_node.h"
+#include "iri_nav_hole_detection_alg_node.h"
 
 HoleDetectionAlgNode::HoleDetectionAlgNode(void) :
-  algorithm_base::IriBaseAlgorithm<HoleDetectionAlgorithm>(),hole_min_p(60),box_y_end(0),box_z_ini(1),
-  box_y_ini(-0.1),box_z_end(1.8),box_x(0.9)
+  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),num_cells(3)
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
@@ -12,14 +12,14 @@ HoleDetectionAlgNode::HoleDetectionAlgNode(void) :
   this->hole_all_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("hole_zone", 10);
   // [init subscribers]
   this->input_subscriber_ = this->public_node_handle_.subscribe("input", 10, &HoleDetectionAlgNode::input_callback, this);
- 
-	
+
+
   // [init services]
-  
+
   // [init clients]
-  
+
   // [init action servers]
-  
+
   // [init action clients]
 }
 
@@ -32,18 +32,18 @@ void HoleDetectionAlgNode::mainNodeThread(void)
 {
   // [fill msg structures]
   //this->PointCloud2_msg_.data = my_var;
-  
+
   // [fill srv structure and make request to the server]
-  
+
   // [fill action structure and make request to the action server]
 
   // [publish messages]
 }
 //PointCloud2_msg_
 /*  [subscriber callbacks] */
-void HoleDetectionAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
-{ 
-  ROS_INFO("HoleDetectionAlgNode::input_callback: New Message Received"); 
+void HoleDetectionAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
+{
+  ROS_INFO("HoleDetectionAlgNode::input_callback: New Message Received");
 
 
 
@@ -55,10 +55,10 @@ void HoleDetectionAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstP
   cloud_for->is_dense=false;
   //cloud_for->header.stamp = ros::Time::now ();
 /////////////////////////////////////////////////////////////////////////////////////
-  
- //use appropiate mutex to shared variables if necessary 
- 
-  this->alg_.lock(); 
+
+ //use appropiate mutex to shared variables if necessary
+
+  this->alg_.lock();
 
   cloud_in_rgb.points.resize(cloud_in.size());
   for (size_t i = 0; i < cloud_in.points.size(); i++) {
@@ -81,33 +81,33 @@ 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_y_end, box_z_ini, box_x, box_y_ini, box_z_end, Xl, Xc, Xr, Y, cloud_in_rgb, cloud_for);
-  //unlock previously blocked shared variables 
-  this->alg_.unlock();  
-  
+  hole_detect.cloud_all(hole_min_p, box_z_end, box_x_ini, box_y, box_z_ini, box_x_end, num_cells, Z, cloud_in_rgb, cloud_for);
+  //unlock previously blocked shared variables
+  this->alg_.unlock();
+
 ///////////////////////////////////////////////////////////////////////////////////////
 
  	pcl::toROSMsg (*cloud_for, PointCloud2_msg_);
   pcl::toROSMsg (cloud_in_rgb, PointCloud2_msg_all);
-  
-  
+
+
   sensor_msgs::convertPointCloud2ToPointCloud(PointCloud2_msg_, PointCloud_msg_);
-  
+
   PointCloud_msg_.header.stamp = ros::Time::now ();
   PointCloud2_msg_.header.stamp = ros::Time::now ();
-  
+
   this->hole_obstacle_publisher_.publish(this->PointCloud_msg_);
   this->hole_all_publisher_.publish(this->PointCloud2_msg_all);
-  
-  //use appropiate mutex to shared variables if necessary 
-  //this->alg_.lock(); 
-  //this->input_mutex_.enter(); 
 
-  //std::cout << msg->data << std::endl; 
+  //use appropiate mutex to shared variables if necessary
+  //this->alg_.lock();
+  //this->input_mutex_.enter();
+
+  //std::cout << msg->data << std::endl;
 
-  //unlock previously blocked shared variables 
-  //this->alg_.unlock(); 
-  //this->input_mutex_.exit(); 
+  //unlock previously blocked shared variables
+  //this->alg_.unlock();
+  //this->input_mutex_.exit();
 }
 
 /*  [service callbacks] */
@@ -120,11 +120,12 @@ void HoleDetectionAlgNode::node_config_update(Config &config, uint32_t level)
 {
   this->alg_.lock();
 hole_min_p=config.hole_min_p;
-box_y_end=config.box_y_end;
-box_z_ini=config.box_z_ini;
 box_z_end=config.box_z_end;
-box_y_ini=config.box_y_ini;
-box_x=config.box_x;
+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();
 }
 
@@ -135,5 +136,5 @@ void HoleDetectionAlgNode::addNodeDiagnostics(void)
 /* main function */
 int main(int argc,char *argv[])
 {
-  return algorithm_base::main<HoleDetectionAlgNode>(argc, argv, "hole_detection_alg_node");
+  return algorithm_base::main<HoleDetectionAlgNode>(argc, argv, "iri_nav_hole_detection_alg_node");
 }