diff --git a/include/obstacle_detection_normals_alg_node.h b/include/obstacle_detection_normals_alg_node.h
index 8bc2c849c9ee5a44a3e5b5704d67f81bd97875da..ca13439978356f3d9335044586329d6de4e7bc8b 100644
--- a/include/obstacle_detection_normals_alg_node.h
+++ b/include/obstacle_detection_normals_alg_node.h
@@ -31,7 +31,7 @@
 #include <ros/ros.h>
 
 #include <pcl_ros/point_cloud.h>
-#include <pcl/ros/conversions.h>
+#include <pcl/conversions.h>
 #include <pcl/point_types.h>
 #include <pcl/filters/filter.h>
 
diff --git a/src/obstacle_detection_normals_alg.cpp b/src/obstacle_detection_normals_alg.cpp
index 3ce6b3f8b0b22a5d97a8e67af44e9d731d5e2488..8ac78fcfe338f0b0f35f8b59389c0587db148017 100644
--- a/src/obstacle_detection_normals_alg.cpp
+++ b/src/obstacle_detection_normals_alg.cpp
@@ -15,15 +15,20 @@ void ObstacleDetectionNormalsAlgorithm::config_update(Config& new_cfg, uint32_t
 
   // save the current configuration
   this->config_=new_cfg;
-  
+
   this->unlock();
 }
 
 // ObstacleDetectionNormalsAlgorithm Public API
-
-void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z, 
-     float normal_y, float normal_x, float min_dist, 
-     const pcl::PointCloud<pcl::PointXYZ>& cloud, 
+/*
+It calculates the normal vector to each point in the pointcloud. For that, it uses
+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)
 {
 
@@ -48,12 +53,12 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
   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); 
+  // 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> ());
@@ -90,6 +95,7 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
   n.setSearchMethod(tree);
   n.setKSearch(KSearch); // Use 10 nearest neighbors to estimate the normals
 
+  //n.setViewPoint (0.26374, 0.0, -0.0485);
   //estimate
   n.setInputCloud(cloud.makeShared());
   n.compute(*output);
@@ -100,21 +106,27 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
 
   float e=0.0;
 
-  *cloud_obs=*cloud_out; 
+  *cloud_obs=*cloud_out;
 
   for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_out->height; ++rowIndex)
   {
     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].z > min_dist)
+      if (cloud_out->points[pointIndex].x > min_dist)
       {
 				//ROS_INFO_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
-				
-        if (cloud_out->points[pointIndex].normal_z > normal_z && 
-            cloud_out->points[pointIndex].normal_y < normal_y && 
+
+        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 (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;
@@ -122,9 +134,15 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
           cloud_obs->points[pointIndex].y=std::numeric_limits<float>::quiet_NaN();;
           cloud_obs->points[pointIndex].z=std::numeric_limits<float>::quiet_NaN();;
         }
-        else 
+        else
         {
-					//ROS_ERROR_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
+          //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;
@@ -140,6 +158,6 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
       }
     }
   }
-  e=(e/float(cloud_out->width*cloud_out->height))*100; 
-  //printf("error %f",e); 
+  e=(e/float(cloud_out->width*cloud_out->height))*100;
+  //printf("error %f",e);
 }