diff --git a/config/far_config.yaml b/config/far_config.yaml
index 217b2186429cfb36e881879b387e6513e88d76b4..334e9f272c31c2c0dcd4ef45bc1ddee179dcef69 100755
--- a/config/far_config.yaml
+++ b/config/far_config.yaml
@@ -1,4 +1,4 @@
-hole_min_p: 35
+hole_min_p: 25
num_cells: 3
box_y: 0.6
box_z_ini: -0.1
diff --git a/config/near_config.yaml b/config/near_config.yaml
index 70c30b1747842ccbf7b8e62f2d0df4e9132559fa..2bfe7fe37eadbb88996110de1e1b58c8a6760032 100755
--- a/config/near_config.yaml
+++ b/config/near_config.yaml
@@ -1,5 +1,5 @@
-hole_min_p: 35
-num_cells: 3
+hole_min_p: 25
+num_cells: 2
box_y: 0.45
box_z_ini: -0.1
box_z_end: 0.1
diff --git a/include/iri_point_cloud_hole_detection_alg.h b/include/iri_point_cloud_hole_detection_alg.h
index 16835cc0cfaa9b201fcd0cb1cf6d40df1178fade..0ce064f3820c6921de81f27f7edac05d843f0b43 100644
--- a/include/iri_point_cloud_hole_detection_alg.h
+++ b/include/iri_point_cloud_hole_detection_alg.h
@@ -52,8 +52,6 @@ class PointCloudHoleDetectionAlgorithm
CMutex alg_mutex_;
// private attributes and methods
- 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:
@@ -121,7 +119,7 @@ class PointCloudHoleDetectionAlgorithm
// here define all hole_detection_alg interface methods to retrieve and set
// the driver parameters
-
+ void cloud_all(pcl::PointCloud<pcl::PointXYZRGB>& cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out);
/**
* \brief Destructor
*
@@ -129,12 +127,6 @@ class PointCloudHoleDetectionAlgorithm
*
*/
~PointCloudHoleDetectionAlgorithm(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, int num_cells, float Z,
- pcl::PointCloud<pcl::PointXYZRGB>& cloud_in,
- pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out);
-
};
#endif
diff --git a/include/iri_point_cloud_hole_detection_alg_node.h b/include/iri_point_cloud_hole_detection_alg_node.h
index c2e249b1ce7ccd574434dd2779a9df683a4d3b68..3b0ac9f8cc0973b8fc2b1d2c06290967cc601a0a 100644
--- a/include/iri_point_cloud_hole_detection_alg_node.h
+++ b/include/iri_point_cloud_hole_detection_alg_node.h
@@ -39,14 +39,11 @@
// [publisher subscriber headers]
#include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
// [service client headers]
// [action server client headers]
-using namespace std;
-
/**
* \brief IRI ROS Specific Algorithm Class
*
@@ -54,19 +51,13 @@ using namespace std;
class PointCloudHoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<PointCloudHoleDetectionAlgorithm>
{
private:
-
- 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]
ros::Publisher hole_obstacle_publisher_;
sensor_msgs::PointCloud2 PointCloud2_msg_;
- sensor_msgs::PointCloud PointCloud_msg_;
-
ros::Publisher hole_all_publisher_;
- sensor_msgs::PointCloud2 PointCloud2_msg_all;
+ sensor_msgs::PointCloud2 PointCloud2_msg_all_;
+
pcl::PointCloud<pcl::PointXYZ> cloud_in;
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_rgb;
@@ -80,7 +71,7 @@ class PointCloudHoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<P
// [client attributes]
// [action server attributes]
- PointCloudHoleDetectionAlgorithm hole_detect;
+
// [action client attributes]
public:
diff --git a/src/iri_point_cloud_hole_detection_alg.cpp b/src/iri_point_cloud_hole_detection_alg.cpp
index 8dd1bc9e73f3f66a181fffe9ba7219e1e299c14c..79c99b8ef19a563743237087b3746eaab36f6ee4 100644
--- a/src/iri_point_cloud_hole_detection_alg.cpp
+++ b/src/iri_point_cloud_hole_detection_alg.cpp
@@ -26,37 +26,34 @@ 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 PointCloudHoleDetectionAlgorithm::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)
+void PointCloudHoleDetectionAlgorithm::cloud_all(pcl::PointCloud<pcl::PointXYZRGB>& cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_for)
{
- int p [num_cells] = {};
- float threshold [num_cells+1] = {};
+ int p [this->config_.num_cells]={0};
+ float threshold[this->config_.num_cells+1]={0};
+ float Z;
//////////////////////////////////////////////////////////////////////////////
// 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(int i=0;i<=this->config_.num_cells;++i)
+ threshold[i]=(this->config_.box_y/2)-((this->config_.num_cells-i)*(this->config_.box_y/this->config_.num_cells));
-
- for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
+ for(size_t rowIndex=0,pointIndex=0;rowIndex<cloud_in.height;++rowIndex)
{
- for (size_t colIndex=0; colIndex<cloud_in.width; ++colIndex, ++pointIndex)
+ 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)
+ if(this->config_.box_z_ini<cloud_in.points[pointIndex].z &&
+ cloud_in.points[pointIndex].z<this->config_.box_z_end &&
+ this->config_.box_x_ini<cloud_in.points[pointIndex].x &&
+ cloud_in.points[pointIndex].x<this->config_.box_x_end)
{
- for (int cell=0; cell<num_cells; ++cell)
+ for(int cell=0;cell<this->config_.num_cells;++cell)
{
- if ( threshold[cell] < cloud_in.points[pointIndex].y &&
- cloud_in.points[pointIndex].y < threshold[cell+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].g=int(255*(float(cell)/float(max(1,this->config_.num_cells-1))));
cloud_in.points[pointIndex].b=255;
p[cell]=p[cell]+1;
}
@@ -70,16 +67,15 @@ void PointCloudHoleDetectionAlgorithm::cloud_all(int hole_min_p, float box_z_end
//////////////////////////////////////////////////////////////////////////////
Z=0.1;
- for (size_t rowIndex=0, pointIndex=0; rowIndex<5; ++rowIndex)
+ for(size_t rowIndex=0,pointIndex=0;rowIndex<5;++rowIndex)
{
- for (size_t colIndex=0; colIndex<5; ++colIndex, ++pointIndex)
+ for(size_t colIndex=0;colIndex<5;++colIndex,++pointIndex)
{
- for (int cell=0; cell<num_cells; ++cell)
+ for(int cell=0;cell<this->config_.num_cells; ++cell)
{
- if (p[cell] < hole_min_p)
+ if(p[cell]<this->config_.hole_min_p)
{
- cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, threshold[cell] +
- (threshold[cell+1]-threshold[cell])*colIndex/4, Z));
+ cloud_for->points.push_back(pcl::PointXYZ(this->config_.box_x_ini,threshold[cell]+(threshold[cell+1]-threshold[cell])*colIndex/4,Z));
}
}
}
diff --git a/src/iri_point_cloud_hole_detection_alg_node.cpp b/src/iri_point_cloud_hole_detection_alg_node.cpp
index 428da29783828ae722902d21954f32f601baec3d..0b5494a95e7ccb6c9cdcadfffbd4a548a59c4146 100644
--- a/src/iri_point_cloud_hole_detection_alg_node.cpp
+++ b/src/iri_point_cloud_hole_detection_alg_node.cpp
@@ -1,17 +1,16 @@
#include "iri_point_cloud_hole_detection_alg_node.h"
PointCloudHoleDetectionAlgNode::PointCloudHoleDetectionAlgNode(void) :
- algorithm_base::IriBaseAlgorithm<PointCloudHoleDetectionAlgorithm>(),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)
+ algorithm_base::IriBaseAlgorithm<PointCloudHoleDetectionAlgorithm>()
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
- this->hole_obstacle_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("hole_obstacle", 10);
- this->hole_all_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("hole_zone", 10);
+ this->hole_obstacle_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("hole_obstacle", 1);
+ this->hole_all_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("hole_zone", 1);
// [init subscribers]
- this->input_subscriber_ = this->public_node_handle_.subscribe("input", 10, &PointCloudHoleDetectionAlgNode::input_callback, this);
+ this->input_subscriber_ = this->public_node_handle_.subscribe("input", 1, &PointCloudHoleDetectionAlgNode::input_callback, this);
// [init services]
@@ -39,75 +38,44 @@ void PointCloudHoleDetectionAlgNode::mainNodeThread(void)
// [publish messages]
}
-//PointCloud2_msg_
+
/* [subscriber callbacks] */
void PointCloudHoleDetectionAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
- ROS_INFO("PointCloudHoleDetectionAlgNode::input_callback: New Message Received");
-
-
-
- pcl::fromROSMsg (*msg, cloud_in);
+ ROS_DEBUG("PointCloudHoleDetectionAlgNode::input_callback: New Message Received");
-/////////////////////////////////////////////////////////////////////////////////////
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for (new pcl::PointCloud<pcl::PointXYZ>);
- cloud_for->header.frame_id = cloud_in.header.frame_id;
- cloud_for->is_dense=false;
- //cloud_for->header.stamp = ros::Time::now ();
-/////////////////////////////////////////////////////////////////////////////////////
+ pcl::fromROSMsg(*msg,this->cloud_in);
- //use appropiate mutex to shared variables if necessary
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for(new pcl::PointCloud<pcl::PointXYZ>);
this->alg_.lock();
- cloud_in_rgb.points.resize(cloud_in.size());
- for (size_t i = 0; i < cloud_in.points.size(); i++) {
- cloud_in_rgb.points[i].x = cloud_in.points[i].x;
- cloud_in_rgb.points[i].y = cloud_in.points[i].y;
- cloud_in_rgb.points[i].z = cloud_in.points[i].z;
- cloud_in_rgb.points[i].r = 0;
- cloud_in_rgb.points[i].g = 0;
- cloud_in_rgb.points[i].b = 0;
+ this->cloud_in_rgb.points.resize(this->cloud_in.size());
+ this->cloud_in_rgb.width=this->cloud_in.width;
+ this->cloud_in_rgb.height=this->cloud_in.height;
+ for(size_t i=0;i<this->cloud_in.points.size();i++)
+ {
+ this->cloud_in_rgb.points[i].x=this->cloud_in.points[i].x;
+ this->cloud_in_rgb.points[i].y=this->cloud_in.points[i].y;
+ this->cloud_in_rgb.points[i].z=this->cloud_in.points[i].z;
+ this->cloud_in_rgb.points[i].r=0;
+ this->cloud_in_rgb.points[i].g=0;
+ this->cloud_in_rgb.points[i].b=0;
}
- cloud_in_rgb.header.frame_id = cloud_in.header.frame_id;
- cloud_in_rgb.height = cloud_in.height;
- cloud_in_rgb.width = cloud_in.width;
- // cloud_in_rgb.fields=
- // cloud_in_rgb.is_bigendian=
- // cloud_in_rgb.point_step=
- // cloud_in_rgb.row_step=
- // cloud_in_rgb.is_dense=
-
- 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, num_cells, Z, cloud_in_rgb, cloud_for);
+
+ this->alg_.cloud_all(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_);
+ pcl::toROSMsg(*cloud_for,this->PointCloud2_msg_);
+ this->PointCloud2_msg_.header=msg->header;
+ pcl::toROSMsg(this->cloud_in_rgb,this->PointCloud2_msg_all_);
+ this->PointCloud2_msg_all_.header=msg->header;
- 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;
-
- //unlock previously blocked shared variables
- //this->alg_.unlock();
- //this->input_mutex_.exit();
+ this->hole_obstacle_publisher_.publish(this->PointCloud2_msg_);
+ this->hole_all_publisher_.publish(this->PointCloud2_msg_all_);
}
/* [service callbacks] */
@@ -118,15 +86,6 @@ void PointCloudHoleDetectionAlgNode::input_callback(const sensor_msgs::PointClou
void PointCloudHoleDetectionAlgNode::node_config_update(Config &config, uint32_t level)
{
- this->alg_.lock();
-hole_min_p=config.hole_min_p;
-box_z_end=config.box_z_end;
-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();
}
void PointCloudHoleDetectionAlgNode::addNodeDiagnostics(void)