diff --git a/CMakeLists.txt b/CMakeLists.txt
index c5907b874cf10f61f988e2dab392e693961e2a36..32b7762fc731e29616044f240449d1f0ac350ec7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,7 +9,7 @@ project(iri_nav_obstacle_detection_normals)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs pcl_ros)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs pcl_ros nodelet)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -64,7 +64,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
-  CATKIN_DEPENDS iri_base_algorithm sensor_msgs pcl_ros
+  CATKIN_DEPENDS iri_base_algorithm sensor_msgs pcl_ros nodelet
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -80,19 +80,22 @@ catkin_package(
 # ******************************************************************** 
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
-include_directories(${iriutils_INCLUDE_DIR} ./include)
+include_directories(${iriutils_INCLUDE_DIR})
 
 ## Declare a cpp library
 # add_library(${PROJECT_NAME} <list of source files>)
 
 ## Declare a cpp executable
 add_executable(${PROJECT_NAME} src/obstacle_detection_normals_alg.cpp src/obstacle_detection_normals_alg_node.cpp)
+add_library(${PROJECT_NAME}_nodelet src/obstacle_detection_normals_alg.cpp src/obstacle_detection_normals_alg_node.cpp)
 
 # ******************************************************************** 
 #                   Add the libraries
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_nodelet ${iriutils_LIBRARY})
 
 # ******************************************************************** 
 #               Add message headers dependencies 
@@ -102,3 +105,4 @@ target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
 #               Add dynamic reconfigure dependencies 
 # ******************************************************************** 
 add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/config/default_config.yaml b/config/default_config.yaml
index 1c13d880ebd6a334ff48f005ecf4b7a2c101730a..9fab8e59286a729193783331cf022d19fc028cb2 100755
--- a/config/default_config.yaml
+++ b/config/default_config.yaml
@@ -1,5 +1,5 @@
 filter_min_dist: 0.3
-normal_KSearchi: 30
+normal_KSearch: 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 b06d03cebcd9a145d8ea0679528b5075dff790bb..822ac1791306628fb5c0dc10e3d255dd868bcb43 100644
--- a/include/obstacle_detection_normals_alg.h
+++ b/include/obstacle_detection_normals_alg.h
@@ -25,6 +25,7 @@
 #ifndef _obstacle_detection_normals_alg_h_
 #define _obstacle_detection_normals_alg_h_
 
+#include <iri_base_algorithm/iri_base_algorithm.h>
 #include <iri_nav_obstacle_detection_normals/ObstacleDetectionNormalsConfig.h>
 #include "mutex.h"
 
@@ -46,7 +47,7 @@ typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
  *
  *
  */
-class ObstacleDetectionNormalsAlgorithm
+class ObstacleDetectionNormalsAlgorithm 
 {
   protected:
    /**
@@ -60,6 +61,7 @@ class ObstacleDetectionNormalsAlgorithm
     KdTreePtr tree;
     
   public:
+
    /**
     * \brief define config type
     *
@@ -141,7 +143,6 @@ class ObstacleDetectionNormalsAlgorithm
     void cloud_all(const pcl::PointCloud<pcl::PointXYZ>& cloud, 
                    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out, 
                    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs); 
-
 };
 
 #endif
diff --git a/include/obstacle_detection_normals_alg_node.h b/include/obstacle_detection_normals_alg_node.h
index f121236f5d5b599e5cd3d64fa5f91fe23dc5d9ae..7d49730ba3fd9da81a2d2eef0b52806d847f7fb6 100644
--- a/include/obstacle_detection_normals_alg_node.h
+++ b/include/obstacle_detection_normals_alg_node.h
@@ -44,9 +44,6 @@
 
 // [action server client headers]
 
-using namespace std;
-
-
 /**
  * \brief IRI ROS Specific Algorithm Class
  *
@@ -71,7 +68,7 @@ class ObstacleDetectionNormalsAlgNode : public algorithm_base::IriBaseAlgorithm<
     // [client attributes]
 
     // [action server attributes]
-    ObstacleDetectionNormalsAlgorithm obstacle_detect;
+    
     // [action client attributes]
 
   public:
@@ -81,7 +78,7 @@ class ObstacleDetectionNormalsAlgNode : public algorithm_base::IriBaseAlgorithm<
     * This constructor initializes specific class attributes and all ROS
     * communications variables to enable message exchange.
     */
-    ObstacleDetectionNormalsAlgNode(void);
+    ObstacleDetectionNormalsAlgNode(const ros::NodeHandle &nh=ros::NodeHandle("~"));
 
    /**
     * \brief Destructor
@@ -134,4 +131,29 @@ class ObstacleDetectionNormalsAlgNode : public algorithm_base::IriBaseAlgorithm<
     
 };
 
+#include "nodelet/nodelet.h"
+#include "threadserver.h"
+
+class ObstacleDetectionNormalsAlgNodelet : public nodelet::Nodelet
+{
+  private:
+    ObstacleDetectionNormalsAlgNode *node;
+    virtual void onInit();// initialization function
+    // thread attributes
+    CThreadServer *thread_server;
+    std::string spin_thread_id;
+  protected:
+    static void *spin_thread(void *param);
+  public:
+    ObstacleDetectionNormalsAlgNodelet();
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~ObstacleDetectionNormalsAlgNodelet();
+};
+
 #endif
diff --git a/launch/nodelet.launch b/launch/nodelet.launch
index 6ab197c82095a97b88e5d32c924ec37f16da5240..82beed37b47b1423a734f182cb8aa32060bcfcb6 100644
--- a/launch/nodelet.launch
+++ b/launch/nodelet.launch
@@ -2,9 +2,9 @@
 <launch>
 
   <arg name="ns" default="ana"/>
-  <arg name="node_name" default="average_point_cloud"/>
+  <arg name="node_name" default="obstacle_detection_normals"/>
   <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="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"/>
@@ -14,7 +14,7 @@
     <node name="$(arg node_name)"
           pkg="nodelet"
           type="nodelet"
-          args="load iri_nav_average_point_cloud/AveragePointcloudAlgNodelet $(arg camera_nodelet_manager)"
+          args="load iri_nav_obstacle_detection_normals/ObstacleDetectionNormalsAlgNodelet $(arg camera_nodelet_manager)"
           output="$(arg output)">
       <remap from="~input" to="$(arg cloud_in)"/>
       <rosparam file="$(arg config_file)" command="load"/>
diff --git a/obstacle_detection_normals_nodelet_plugin.xml b/obstacle_detection_normals_nodelet_plugin.xml
new file mode 100755
index 0000000000000000000000000000000000000000..63c9edfa1ea94de31341215d9b2dbefdc6ed09f2
--- /dev/null
+++ b/obstacle_detection_normals_nodelet_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libiri_nav_obstacle_detection_normals_nodelete">
+  <class name="iri_nav_obstacle_detection_normals/ObstacleDetectionNormalsAlgNodelet" type="ObstacleDetectionNormalsAlgNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelete for the IRI navigation Velodyne angle filter 
+    </description>
+  </class>
+</library>
diff --git a/package.xml b/package.xml
index cdfb79735af73103943ca3e111d2bae5c65d2506..912e685c5aaa1c9d7fd8a32cd0d7a74820667a5e 100644
--- a/package.xml
+++ b/package.xml
@@ -41,10 +41,12 @@
   <build_depend>iri_base_algorithm</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>pcl_ros</build_depend>
+  <build_depend>nodelet</build_depend>
 
   <run_depend>iri_base_algorithm</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>pcl_ros</run_depend>
+  <run_depend>nodelet</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/src/obstacle_detection_normals_alg.cpp b/src/obstacle_detection_normals_alg.cpp
index 0cd5795656e2b793582e479959f8278d40c8f52d..7854b2d361e6adc37cf27509cdf8adb015dfab3f 100644
--- a/src/obstacle_detection_normals_alg.cpp
+++ b/src/obstacle_detection_normals_alg.cpp
@@ -1,5 +1,4 @@
 #include "obstacle_detection_normals_alg.h"
-using namespace std;
 
 ObstacleDetectionNormalsAlgorithm::ObstacleDetectionNormalsAlgorithm(void)
 {
@@ -45,7 +44,7 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::Poi
 
   pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
 
-  boost::shared_ptr< vector<int> > indicesptr(new vector<int> (indices));
+  boost::shared_ptr< std::vector<int> > indicesptr(new std::vector<int> (indices));
   // set normal estimation parameters
   n.setIndices(indicesptr);
   n.setSearchMethod(tree);
@@ -59,12 +58,15 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::Poi
 
   *cloud_obs=*cloud_out;
 
+  std::cout << &this->config_ << std::endl;
+  std::cout << this->config_.normal_z << "," << this->config_.normal_y << "," << this->config_.normal_x << std::endl;
   for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_out->height; ++rowIndex)
   {
     for (size_t colIndex=0; colIndex<cloud_out->width; ++colIndex, ++pointIndex)
     {
       if (cloud_out->points[pointIndex].x > this->config_.filter_min_dist)
       {
+//        std::cout << "(" << rowIndex << "," << pointIndex << "): " << cloud_out->points[pointIndex].normal_z << "," << cloud_out->points[pointIndex].normal_y << "," << cloud_out->points[pointIndex].normal_x << std::endl;
         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 )
@@ -72,9 +74,9 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(const pcl::PointCloud<pcl::Poi
           cloud_out->points[pointIndex].r=0;
           cloud_out->points[pointIndex].g=255;
           cloud_out->points[pointIndex].b=0;
-          cloud_obs->points[pointIndex].x=std::numeric_limits<float>::quiet_NaN();;
-          cloud_obs->points[pointIndex].y=std::numeric_limits<float>::quiet_NaN();;
-          cloud_obs->points[pointIndex].z=std::numeric_limits<float>::quiet_NaN();;
+          cloud_obs->points[pointIndex].x=std::numeric_limits<float>::quiet_NaN();
+          cloud_obs->points[pointIndex].y=std::numeric_limits<float>::quiet_NaN();
+          cloud_obs->points[pointIndex].z=std::numeric_limits<float>::quiet_NaN();
         }
         else
         {
diff --git a/src/obstacle_detection_normals_alg_node.cpp b/src/obstacle_detection_normals_alg_node.cpp
index dd3f7b6566bb11819df699baa60201b3a780fe78..eb59a3dfa7b86a158ec746dbbe384d884dfd7367 100644
--- a/src/obstacle_detection_normals_alg_node.cpp
+++ b/src/obstacle_detection_normals_alg_node.cpp
@@ -1,7 +1,7 @@
 #include "obstacle_detection_normals_alg_node.h"
 
-ObstacleDetectionNormalsAlgNode::ObstacleDetectionNormalsAlgNode(void) :
-  algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>()
+ObstacleDetectionNormalsAlgNode::ObstacleDetectionNormalsAlgNode(const ros::NodeHandle &nh) :
+  algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>(nh)
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
@@ -56,7 +56,7 @@ void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointClo
   this->alg_.lock(); 
 
   if (!cloud.empty()){
-    obstacle_detect.cloud_all(cloud, cloud_out, cloud_obs);
+    this->alg_.cloud_all(cloud, cloud_out, cloud_obs);
   }
   //unlock previously blocked shared variables 
   this->alg_.unlock();
@@ -86,9 +86,53 @@ void ObstacleDetectionNormalsAlgNode::addNodeDiagnostics(void)
 {
 }
 
-
 /* main function */
 int main(int argc,char *argv[])
 {
   return algorithm_base::main<ObstacleDetectionNormalsAlgNode>(argc, argv, "obstacle_detection_normals_alg_node");
 }
+
+#include <pluginlib/class_list_macros.h>
+
+ObstacleDetectionNormalsAlgNodelet::ObstacleDetectionNormalsAlgNodelet()
+{
+  this->node=NULL;
+}
+
+ObstacleDetectionNormalsAlgNodelet::~ObstacleDetectionNormalsAlgNodelet(void)
+{
+  // kill the thread
+  this->thread_server->kill_thread(this->spin_thread_id);
+  this->thread_server->detach_thread(this->spin_thread_id);
+  this->thread_server->delete_thread(this->spin_thread_id);
+  this->spin_thread_id="";
+  // [free dynamic memory]
+  if(this->node!=NULL)
+    delete this->node;
+}
+
+void ObstacleDetectionNormalsAlgNodelet::onInit()
+{
+  // initialize the driver node
+  this->node=new ObstacleDetectionNormalsAlgNode(getPrivateNodeHandle());
+  // initialize the thread
+  this->thread_server=CThreadServer::instance();
+  this->spin_thread_id=getName() + "_lidar_obstacle_detector_nodelet_spin";
+  this->thread_server->create_thread(this->spin_thread_id);
+  this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
+  // start the spin thread
+  this->thread_server->start_thread(this->spin_thread_id);
+}
+
+void *ObstacleDetectionNormalsAlgNodelet::spin_thread(void *param)
+{
+  ObstacleDetectionNormalsAlgNodelet *nodelet=(ObstacleDetectionNormalsAlgNodelet *)param;
+
+  nodelet->node->spin();
+
+  pthread_exit(NULL);
+}
+
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(iri_lidar_obstacle_detector, ObstacleDetectionNormalsAlgNodelet, ObstacleDetectionNormalsAlgNodelet, nodelet::Nodelet);
+