diff --git a/cfg/ObstacleDetectionNormals.cfg b/cfg/ObstacleDetectionNormals.cfg
index 9dade53ba091c164eac7c5c9ffaa9d64dcb9512d..c9c10542f9785ef5e2b1102b72b0ba8f8cf9a0bd 100755
--- a/cfg/ObstacleDetectionNormals.cfg
+++ b/cfg/ObstacleDetectionNormals.cfg
@@ -37,11 +37,11 @@ from dynamic_reconfigure.parameter_generator_catkin import *
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( "filter_min_dist", double_t, 0, "Min distance threshold", 0.3, 0, 100)
-gen.add( "normal_KSearch", int_t, 0, "No of neighbors to estimate the surface", 100, 10, 200)
-gen.add( "normal_x", double_t, 0, "Accepted Normal X component", 1, )
-gen.add( "normal_y", double_t, 0, "Accepted Normal Y component", -0.2, )
-gen.add( "normal_z", double_t, 0, "Accepted Normal Z component", 1, )
+# 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("filter_min_dist", double_t, 0, "Min distance threshold", 0.3, 0, 100)
+gen.add("normal_KSearch", int_t, 0, "No of neighbors to estimate the surface",100,10, 200)
+gen.add("normal_x", double_t, 0, "Accepted Normal X component", 1,)
+gen.add("normal_y", double_t, 0, "Accepted Normal Y component", -0.2,)
+gen.add("normal_z", double_t, 0, "Accepted Normal Z component", 1,)
exit(gen.generate(PACKAGE, "ObstacleDetectionNormalsAlgorithm", "ObstacleDetectionNormals"))
diff --git a/config/default_config.yaml b/config/default_config.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..1c13d880ebd6a334ff48f005ecf4b7a2c101730a
--- /dev/null
+++ b/config/default_config.yaml
@@ -0,0 +1,5 @@
+filter_min_dist: 0.3
+normal_KSearchi: 30
+normal_x: 1.001
+normal_y: 1.001
+normal_z: 0.95
diff --git a/include/obstacle_detection_normals_alg.h b/include/obstacle_detection_normals_alg.h
index 7d87c54b10d9525065d3345d60faa27953ae715c..b06d03cebcd9a145d8ea0679528b5075dff790bb 100644
--- a/include/obstacle_detection_normals_alg.h
+++ b/include/obstacle_detection_normals_alg.h
@@ -28,7 +28,6 @@
#include <iri_nav_obstacle_detection_normals/ObstacleDetectionNormalsConfig.h>
#include "mutex.h"
-
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
@@ -38,10 +37,7 @@
#include <boost/make_shared.hpp>
-using namespace pcl;
-using namespace std;
-
-typedef KdTree<PointXYZ>::Ptr KdTreePtr;
+typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
//include obstacle_detection_normals_alg main library
@@ -61,17 +57,8 @@ class ObstacleDetectionNormalsAlgorithm
*/
CMutex alg_mutex_;
- // private attributes and methods
- int KSearch;
- float normal_z,normal_y,normal_x,min_dist;
-
KdTreePtr tree;
- // pcl::PointCloud<pcl::PointXYZ> cloud;
- // pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_out;
- // pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_obs;
-
-
public:
/**
* \brief define config type
@@ -151,10 +138,9 @@ class ObstacleDetectionNormalsAlgorithm
// const pcl::PointCloud<pcl::PointXYZ>& cloud,
// pcl::PointCloud<pcl::PointXYZRGBNormal>& cloud_out, pcl::PointCloud<pcl::PointXYZRGBNormal>& cloud_obs);
- void cloud_all(int KSearch, float normal_z,
- float normal_y, float normal_x, float min_dist,
- const pcl::PointCloud<pcl::PointXYZ>& cloud,
- pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs);
+ void cloud_all(const pcl::PointCloud<pcl::PointXYZ>& cloud,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs);
};
diff --git a/include/obstacle_detection_normals_alg_node.h b/include/obstacle_detection_normals_alg_node.h
index ca13439978356f3d9335044586329d6de4e7bc8b..f121236f5d5b599e5cd3d64fa5f91fe23dc5d9ae 100644
--- a/include/obstacle_detection_normals_alg_node.h
+++ b/include/obstacle_detection_normals_alg_node.h
@@ -54,10 +54,6 @@ using namespace std;
class ObstacleDetectionNormalsAlgNode : public algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>
{
private:
-
- int KSearch;
- float normal_z,normal_y,normal_x,min_dist;
-
// [publisher attributes]
ros::Publisher obstacles_publisher_;
ros::Publisher all_rg_publisher_;
@@ -65,8 +61,6 @@ class ObstacleDetectionNormalsAlgNode : public algorithm_base::IriBaseAlgorithm<
sensor_msgs::PointCloud obstaclespc_msg_;
sensor_msgs::PointCloud2 all_rg_msg_;
-
-
// [subscriber attributes]
ros::Subscriber input_subscriber_;
void input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
diff --git a/launch/node.launch b/launch/node.launch
new file mode 100644
index 0000000000000000000000000000000000000000..bc6ab27b34615632ca9e863e23c6c3da02ff9d4c
--- /dev/null
+++ b/launch/node.launch
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+ <arg name="ns" default="ana"/>
+ <arg name="node_name" default="obstacle_detection_normals"/>
+ <arg name="config_file" default="$(find iri_nav_obstacle_detection_normals)/config/default_config.yaml"/>
+ <arg name="output" default="screen"/>
+ <arg name="launch_prefix" default=""/>
+ <arg name="cloud_in" default="~pointcloud_in"/>
+
+ <group ns="$(arg ns)">
+
+ <node pkg ="iri_nav_obstacle_detection_normals"
+ type="iri_nav_obstacle_detection_normals"
+ name="$(arg node_name)"
+ output="$(arg output)"
+ launch-prefix="$(arg launch_prefix)">
+ <remap from="~input" to="$(arg cloud_in)"/>
+ <rosparam file="$(arg config_file)" command="load"/>
+ </node>
+
+ </group>
+
+</launch>
diff --git a/launch/nodelet.launch b/launch/nodelet.launch
new file mode 100644
index 0000000000000000000000000000000000000000..6ab197c82095a97b88e5d32c924ec37f16da5240
--- /dev/null
+++ b/launch/nodelet.launch
@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+ <arg name="ns" default="ana"/>
+ <arg name="node_name" default="average_point_cloud"/>
+ <arg name="camera_nodelet_manager" default="camera_nodelet_manager"/>
+ <arg name="config_file" default="$(find iri_nav_average_point_cloud)/config/default_config.yaml"/>
+ <arg name="output" default="screen"/>
+ <arg name="launch_prefix" default=""/>
+ <arg name="cloud_in" default="~pointcloud_in"/>
+
+ <group ns="$(arg ns)">
+
+ <node name="$(arg node_name)"
+ pkg="nodelet"
+ type="nodelet"
+ args="load iri_nav_average_point_cloud/AveragePointcloudAlgNodelet $(arg camera_nodelet_manager)"
+ output="$(arg output)">
+ <remap from="~input" to="$(arg cloud_in)"/>
+ <rosparam file="$(arg config_file)" command="load"/>
+ </node>
+
+ </group>
+
+</launch>
diff --git a/src/obstacle_detection_normals_alg.cpp b/src/obstacle_detection_normals_alg.cpp
index 8ac78fcfe338f0b0f35f8b59389c0587db148017..0cd5795656e2b793582e479959f8278d40c8f52d 100644
--- a/src/obstacle_detection_normals_alg.cpp
+++ b/src/obstacle_detection_normals_alg.cpp
@@ -26,85 +26,36 @@ the KSearch nearest neighbors. After that, it checks that each normal is inside
some thresholds to determine if it is a passable point or an obstacle. If some point
is nearer than a minimum distance, it is marked in a diferent color.
*/
-void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
- float normal_y, float normal_x, float min_dist,
- const pcl::PointCloud<pcl::PointXYZ>& cloud,
- pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs)
+void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::PointXYZ>& cloud,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs)
{
+ float e=0.0;
-//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr output (new pcl::PointCloud<pcl::Normal>);
-// Normal estimation using integral Images /////////////////////////////////////////////
-// NOTE: Point cloud should contain width and height structure
-
-// pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> n;
-
-// n.setNormalEstimationMethod(n.AVERAGE_3D_GRADIENT);
-// n.setMaxDepthChangeFactor(0.02f);
-// n.setNormalSmoothingSize(10.0f);
-// n.setInputCloud(cloud);
-// n.compute(*output);
-
-// ROS Fuerte version //////////////////////////////////////////////////////////////
-
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
- // [Multi-threat](NOTE: Point cloud should contain width and height structure):
- // pcl::NormaleEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
- // n.setNumberOfThreads(8);
-
- // To work with Pointers ::Ptr ///////////////////////////////////////////////////////////
- // // Set input cloud
- // n.setInputCloud (cloud);
-
- // // Create an empty kdtree representation, and pass it to the normal estimation object.
- // // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
- // pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
- // n.setSearchMethod (tree);
-
- // // Use all neighbors in a sphere of radius 3cm
- // n.setRadiusSearch (KSearch);
- // // [Multi-threat](NOTE: Point cloud should contain width and height structure) :
- // // n.setKSearch (KSearch);
-
- // double secs = ros::Time::now().toSec();
-
- // // Compute the features
- // n.compute (*output);
-
- // pcl::concatenateFields (*cloud, *output, *cloud_out);
-
- // double secs2 = ros::Time::now().toSec();
- // double diff = secs2-secs;
- // ROS_INFO("computation time: %2.2f",diff);
-
-/////////////////////////////////////////////////////////////////////////////////
std::vector<int> indices;
- indices.resize (cloud.points.size());
- for (size_t i = 0; i < indices.size (); ++i) { indices[i] = i; }
+ indices.resize(cloud.points.size());
+ for (size_t i = 0; i < indices.size (); ++i)
+ indices[i] = i;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
-
boost::shared_ptr< vector<int> > indicesptr(new vector<int> (indices));
// set normal estimation parameters
n.setIndices(indicesptr);
n.setSearchMethod(tree);
- n.setKSearch(KSearch); // Use 10 nearest neighbors to estimate the normals
+ n.setKSearch(this->config_.normal_KSearch);
- //n.setViewPoint (0.26374, 0.0, -0.0485);
//estimate
n.setInputCloud(cloud.makeShared());
n.compute(*output);
- pcl::concatenateFields (cloud, *output, *cloud_out);
-
-
-
- float e=0.0;
+ pcl::concatenateFields(cloud,*output,*cloud_out);
*cloud_obs=*cloud_out;
@@ -112,21 +63,12 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
{
for (size_t colIndex=0; colIndex<cloud_out->width; ++colIndex, ++pointIndex)
{
- //ROS_ERROR_STREAM(cloud_out->points[pointIndex].normal_z);
- if (cloud_out->points[pointIndex].x > min_dist)
+ if (cloud_out->points[pointIndex].x > this->config_.filter_min_dist)
{
- //ROS_INFO_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
-
- if (abs(cloud_out->points[pointIndex].normal_z) > normal_z &&
- cloud_out->points[pointIndex].normal_y < normal_y &&
- cloud_out->points[pointIndex].normal_x < normal_x )
+ if (abs(cloud_out->points[pointIndex].normal_z) > this->config_.normal_z &&
+ cloud_out->points[pointIndex].normal_y < this->config_.normal_y &&
+ cloud_out->points[pointIndex].normal_x < this->config_.normal_x )
{
- /*if (pointIndex % 200 == 0)
- {
- ROS_INFO_STREAM("x_normal: " << cloud_out->points[pointIndex].normal_x);
- ROS_INFO_STREAM("y_normal: " << cloud_out->points[pointIndex].normal_y);
- ROS_INFO_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
- }*/
cloud_out->points[pointIndex].r=0;
cloud_out->points[pointIndex].g=255;
cloud_out->points[pointIndex].b=0;
@@ -136,13 +78,6 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
}
else
{
- //ROS_ERROR_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
- /*if (pointIndex % 200 == 0)
- {
- ROS_ERROR_STREAM("x_normal: " << cloud_out->points[pointIndex].normal_x);
- ROS_ERROR_STREAM("y_normal: " << cloud_out->points[pointIndex].normal_y);
- ROS_ERROR_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
- }*/
cloud_out->points[pointIndex].r=255;
cloud_out->points[pointIndex].g=0;
cloud_out->points[pointIndex].b=0;
@@ -159,5 +94,4 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
}
}
e=(e/float(cloud_out->width*cloud_out->height))*100;
- //printf("error %f",e);
}
diff --git a/src/obstacle_detection_normals_alg_node.cpp b/src/obstacle_detection_normals_alg_node.cpp
index da90fc40acd6563774bf9c993bdd3c22cb02009e..dd3f7b6566bb11819df699baa60201b3a780fe78 100644
--- a/src/obstacle_detection_normals_alg_node.cpp
+++ b/src/obstacle_detection_normals_alg_node.cpp
@@ -1,8 +1,7 @@
#include "obstacle_detection_normals_alg_node.h"
ObstacleDetectionNormalsAlgNode::ObstacleDetectionNormalsAlgNode(void) :
- algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>(),KSearch(100),normal_z(1),
- normal_y(-0.2),normal_x(1),min_dist(0.3)
+ algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>()
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
@@ -49,41 +48,21 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ> cloud;
- // pcl::PointCloud<pcl::PointXYZ> cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_obs(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
pcl::fromROSMsg (*msg, cloud);
- // pcl::PointCloud<pcl::PointXYZ> cloud_in;
- // std::vector<int> indices;
- // pcl::removeNaNFromPointCloud (cloud, cloud_in, indices);
-
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- //use appropiate mutex to shared variables if necessary
this->alg_.lock();
if (!cloud.empty()){
- obstacle_detect.cloud_all(KSearch, normal_z, normal_y, normal_x, min_dist, cloud, cloud_out, cloud_obs);
+ obstacle_detect.cloud_all(cloud, cloud_out, cloud_obs);
}
//unlock previously blocked shared variables
this->alg_.unlock();
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- // all_rg_msg_.width=msg->width;
- // all_rg_msg_.height=msg->height;
- // all_rg_msg_.is_dense=msg->is_dense;
-
- // obstacles_msg_.header=msg->header;
- // obstacles_msg_.width=msg->width;
- // obstacles_msg_.height=msg->height;
- // obstacles_msg_.is_dense=msg->is_dense;
-
- pcl::toROSMsg (*cloud_out, all_rg_msg_);
- pcl::toROSMsg (*cloud_obs, obstacles_msg_);
+ pcl::toROSMsg (*cloud_out, all_rg_msg_);
+ pcl::toROSMsg (*cloud_obs, obstacles_msg_);
sensor_msgs::convertPointCloud2ToPointCloud(obstacles_msg_, obstaclespc_msg_);
@@ -91,17 +70,6 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
this->obstacles_publisher_.publish(this->obstaclespc_msg_);
this->all_rg_publisher_.publish(this->all_rg_msg_);
-
-
- //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();
}
/* [service callbacks] */
@@ -112,13 +80,6 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
void ObstacleDetectionNormalsAlgNode::node_config_update(Config &config, uint32_t level)
{
- this->alg_.lock();
-min_dist=config.filter_min_dist;
-KSearch=config.normal_KSearch;
-normal_z=config.normal_z;
-normal_y=config.normal_y;
-normal_x=config.normal_x;
- this->alg_.unlock();
}
void ObstacleDetectionNormalsAlgNode::addNodeDiagnostics(void)