diff --git a/include/obstacle_detection_normals_alg_node.h b/include/obstacle_detection_normals_alg_node.h
index 7d49730ba3fd9da81a2d2eef0b52806d847f7fb6..8df5330a484219f65a68e31a3d2a6ca571912005 100644
--- a/include/obstacle_detection_normals_alg_node.h
+++ b/include/obstacle_detection_normals_alg_node.h
@@ -53,10 +53,9 @@ class ObstacleDetectionNormalsAlgNode : public algorithm_base::IriBaseAlgorithm<
   private:
     // [publisher attributes]
     ros::Publisher obstacles_publisher_;
-    ros::Publisher all_rg_publisher_;
+    ros::Publisher free_space_publisher_;
     sensor_msgs::PointCloud2 obstacles_msg_;
-    sensor_msgs::PointCloud obstaclespc_msg_;
-    sensor_msgs::PointCloud2 all_rg_msg_;
+    sensor_msgs::PointCloud2 free_space_msg_;
 
     // [subscriber attributes]
     ros::Subscriber input_subscriber_;
diff --git a/src/obstacle_detection_normals_alg.cpp b/src/obstacle_detection_normals_alg.cpp
index ce9b8e0382bf4901a441e9fee73686edcab5a1c3..1f54855dd9259ebcfa1e2ccca5fb6ba521f07f2c 100644
--- a/src/obstacle_detection_normals_alg.cpp
+++ b/src/obstacle_detection_normals_alg.cpp
@@ -78,6 +78,9 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::Poi
           cloud_out->points[pointIndex].r=255;
           cloud_out->points[pointIndex].g=0;
           cloud_out->points[pointIndex].b=0;
+          cloud_out->points[pointIndex].x=std::numeric_limits<float>::quiet_NaN();
+          cloud_out->points[pointIndex].y=std::numeric_limits<float>::quiet_NaN();
+          cloud_out->points[pointIndex].z=std::numeric_limits<float>::quiet_NaN();
           //error counter looking a plane without obstacles
           e=e+1.0;
         }
diff --git a/src/obstacle_detection_normals_alg_node.cpp b/src/obstacle_detection_normals_alg_node.cpp
index 2ac97ee5fc5a17082ea39a01a75d20466d08e8fb..5a7a209a8640a5d0397aa4a802c22dcc1d5c51c4 100644
--- a/src/obstacle_detection_normals_alg_node.cpp
+++ b/src/obstacle_detection_normals_alg_node.cpp
@@ -7,8 +7,8 @@ ObstacleDetectionNormalsAlgNode::ObstacleDetectionNormalsAlgNode(const ros::Node
   //this->loop_rate_ = 2;//in [Hz]
 
   // [init publishers]
-  this->obstacles_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("obstacles", 1);
-  this->all_rg_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("all_rg", 1);
+  this->obstacles_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
+  this->free_space_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("free_space", 1);
   
   // [init subscribers]
   this->input_subscriber_ = this->public_node_handle_.subscribe("input", 1, &ObstacleDetectionNormalsAlgNode::input_callback, this);
@@ -47,6 +47,7 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
 
   // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
   pcl::PointCloud<pcl::PointXYZ> cloud;
+  std::vector<int> indices;
 
   pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
   pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_obs(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
@@ -61,15 +62,13 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
   //unlock previously blocked shared variables 
   this->alg_.unlock();
 
-  pcl::toROSMsg (*cloud_out, all_rg_msg_);
+  pcl::toROSMsg (*cloud_out, free_space_msg_);
   pcl::toROSMsg (*cloud_obs, obstacles_msg_);
 
-  sensor_msgs::convertPointCloud2ToPointCloud(obstacles_msg_, obstaclespc_msg_);
-  
-  all_rg_msg_.header=msg->header;
+  free_space_msg_.header=msg->header;
 
-  this->obstacles_publisher_.publish(this->obstaclespc_msg_);
-  this->all_rg_publisher_.publish(this->all_rg_msg_);
+  this->obstacles_publisher_.publish(this->obstacles_msg_);
+  this->free_space_publisher_.publish(this->free_space_msg_);
 }
 
 /*  [service callbacks] */