diff --git a/CMakeLists.txt b/CMakeLists.txt
index c250d3c654a732cd0dc24683573653f6ef8f0ebe..0ee7f72f0c187d7b1b7929b5ea7e3c68f584d168 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/cfg/HoleDetection.cfg b/cfg/HoleDetection.cfg
index a906897897bb04cdff6c696e3a9516026c86f3e6..d8c1a88d6ab27efa8db25da633d77a14987a6849 100755
--- a/cfg/HoleDetection.cfg
+++ b/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"))
diff --git a/include/hole_detection_alg.h b/include/iri_nav_hole_detection_alg.h
similarity index 84%
rename from include/hole_detection_alg.h
rename to include/iri_nav_hole_detection_alg.h
index a9d24a01741a4faf3e32e53faad371581d48078f..4649017b639d4166cc08f80b8ad9a874d490d8c1 100644
--- a/include/hole_detection_alg.h
+++ b/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
diff --git a/include/hole_detection_alg_node.h b/include/iri_nav_hole_detection_alg_node.h
similarity index 94%
rename from include/hole_detection_alg_node.h
rename to include/iri_nav_hole_detection_alg_node.h
index bdc9912b08a04b46505b040ad0dd53c2d74fad9d..28221d8e8dbf5522036f4d724c3aec4fcc283a3a 100644
--- a/include/hole_detection_alg_node.h
+++ b/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]
diff --git a/package.xml b/package.xml
index 373dba09a09213c67a389fc69b601454a18b246e..7e51220cff94a0e86badeefd7f2089587076821e 100644
--- a/package.xml
+++ b/package.xml
@@ -1,8 +1,8 @@
<?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: -->
diff --git a/src/hole_detection_alg.cpp b/src/hole_detection_alg.cpp
deleted file mode 100644
index bdce98c51e4745112e8354e8a0f426f3882420dc..0000000000000000000000000000000000000000
--- a/src/hole_detection_alg.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-#include "hole_detection_alg.h"
-using namespace std;
-
-HoleDetectionAlgorithm::HoleDetectionAlgorithm(void)
-{
-}
-
-HoleDetectionAlgorithm::~HoleDetectionAlgorithm(void)
-{
-}
-
-void HoleDetectionAlgorithm::config_update(Config& new_cfg, uint32_t level)
-{
- this->lock();
-
- // 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,
- pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_for)
-{
-int l=0;
-int c=0;
-int r=0;
-
-
-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_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)))
- {
- 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;
- }
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Hole Point Cloud
-//////////////////////////////////////////////////////////////////////////////
-
-
-Y=-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)
- {
- cloud_for->points.push_back (pcl::PointXYZ(Xr+0.05, Y, box_z_ini));
- Xr=Xr+0.05;
- }
- }
- Y=Y-0.05;
-}
-
-}
\ No newline at end of file
diff --git a/src/iri_nav_hole_detection_alg.cpp b/src/iri_nav_hole_detection_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f72fbbbff43404275ea5b8788265804260d5a8e9
--- /dev/null
+++ b/src/iri_nav_hole_detection_alg.cpp
@@ -0,0 +1,92 @@
+#include "iri_nav_hole_detection_alg.h"
+using namespace std;
+
+HoleDetectionAlgorithm::HoleDetectionAlgorithm(void)
+{
+}
+
+HoleDetectionAlgorithm::~HoleDetectionAlgorithm(void)
+{
+}
+
+void HoleDetectionAlgorithm::config_update(Config& new_cfg, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=new_cfg;
+
+ this->unlock();
+}
+
+// HoleDetectionAlgorithm Public API
+/*
+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 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_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)
+ {
+ for (int cell=0; cell<num_cells; ++cell)
+ {
+ 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
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Virtual obstacle generation
+//////////////////////////////////////////////////////////////////////////////
+Z=0.1;
+
+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, threshold[cell] +
+ (threshold[cell+1]-threshold[cell])*colIndex/4, Z));
+ }
+ }
+ }
+ Z=Z+0.05;
+}
+
+}
diff --git a/src/hole_detection_alg_node.cpp b/src/iri_nav_hole_detection_alg_node.cpp
similarity index 79%
rename from src/hole_detection_alg_node.cpp
rename to src/iri_nav_hole_detection_alg_node.cpp
index 4c681b935c0c8551e2cceb577cff4a48ee9937af..5159593f36cb2bb23300e595529d90b009e824ce 100644
--- a/src/hole_detection_alg_node.cpp
+++ b/src/iri_nav_hole_detection_alg_node.cpp
@@ -1,8 +1,8 @@
-#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");
}