diff --git a/src/hole_detection_alg.cpp b/src/hole_detection_alg.cpp
index bdce98c51e4745112e8354e8a0f426f3882420dc..b0e9641788f634e1465466f0de12b5e5ae0e52e6 100644
--- a/src/hole_detection_alg.cpp
+++ b/src/hole_detection_alg.cpp
@@ -15,50 +15,57 @@ 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,
+ float Yl, float Yc, float Yr, float Z,
+ pcl::PointCloud<pcl::PointXYZRGB>& cloud_in,
pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_for)
{
int l=0;
int c=0;
int r=0;
-
+//////////////////////////////////////////////////////////////////////////////
+// Hole detection box generation
+//////////////////////////////////////////////////////////////////////////////
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)))
+ if (-(box_y/2) < cloud_in.points[pointIndex].y &&
+ cloud_in.points[pointIndex].y < ((box_y/3)-(box_y/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)))
+ if (((box_y/3)-(box_y/2)) < cloud_in.points[pointIndex].y &&
+ cloud_in.points[pointIndex].y < ((box_y/2)-(box_y/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))
+ if (((box_y/2)-(box_y/3)) < cloud_in.points[pointIndex].y &&
+ cloud_in.points[pointIndex].y < (box_y/2))
{
cloud_in.points[pointIndex].r=0;
cloud_in.points[pointIndex].g=127;
@@ -70,36 +77,34 @@ for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
}
//////////////////////////////////////////////////////////////////////////////
-// 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));
+ Yl= -(box_y/2);
+ Yc= ((box_y/3)-(box_y/2));
+ Yr= ((box_y/2)-(box_y/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;
+ cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, Yl+0.05, Z));
+ Yl=Yl+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;
+ cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, Yc+0.05, Z));
+ Yc=Yc+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;
+ cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, Yr+0.05, Z));
+ Yr=Yr+0.05;
}
}
- Y=Y-0.05;
+ Z=Z+0.05;
}
-}
\ No newline at end of file
+}
diff --git a/src/hole_detection_alg_node.cpp b/src/hole_detection_alg_node.cpp
index 4c681b935c0c8551e2cceb577cff4a48ee9937af..5d84c3189b0fb1faab41260441dc12f4db7ce038 100644
--- a/src/hole_detection_alg_node.cpp
+++ b/src/hole_detection_alg_node.cpp
@@ -1,8 +1,8 @@
#include "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)
{
//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, Yl, Yc, Yr, 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,11 @@ 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;
this->alg_.unlock();
}