diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd27ab0ab859cc702cf2d68a36e69db886771197..461cd0f0ca5aad6eb072847790cf61ed0981f3e1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ project(iri_point_cloud_hole_detection)
 #                 Add catkin additional components here
 # ******************************************************************** 
 # find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs pcl_ros roscpp iri_utils Eigen)
-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)
@@ -65,7 +65,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
 # ******************************************************************** 
@@ -88,12 +88,15 @@ include_directories(${iriutils_INCLUDE_DIR} ./include)
 
 ## Declare a cpp executable
 add_executable(${PROJECT_NAME} src/iri_point_cloud_hole_detection_alg.cpp src/iri_point_cloud_hole_detection_alg_node.cpp)
+add_library(${PROJECT_NAME}_nodelet src/iri_point_cloud_hole_detection_alg.cpp src/iri_point_cloud_hole_detection_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,4 +105,5 @@ target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
 # ******************************************************************** 
- add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/include/iri_point_cloud_hole_detection_alg_node.h b/include/iri_point_cloud_hole_detection_alg_node.h
index 3b0ac9f8cc0973b8fc2b1d2c06290967cc601a0a..8a6d57ffc8d7f7394fdf49ca360675543c4d59af 100644
--- a/include/iri_point_cloud_hole_detection_alg_node.h
+++ b/include/iri_point_cloud_hole_detection_alg_node.h
@@ -81,7 +81,7 @@ class PointCloudHoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<P
     * This constructor initializes specific class attributes and all ROS
     * communications variables to enable message exchange.
     */
-    PointCloudHoleDetectionAlgNode(void);
+    PointCloudHoleDetectionAlgNode(const ros::NodeHandle &nh=ros::NodeHandle("~"));
 
    /**
     * \brief Destructor
@@ -133,4 +133,30 @@ class PointCloudHoleDetectionAlgNode : public algorithm_base::IriBaseAlgorithm<P
     // [test functions]
 };
 
+#include "nodelet/nodelet.h"
+#include "threadserver.h"
+
+class PointCloudHoleDetectionAlgNodelet : public nodelet::Nodelet
+{
+  private:
+    PointCloudHoleDetectionAlgNode *node;
+    virtual void onInit();// initialization function
+    // thread attributes
+    CThreadServer *thread_server;
+    std::string spin_thread_id;
+  protected:
+    static void *spin_thread(void *param);
+  public:
+    PointCloudHoleDetectionAlgNodelet();
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~PointCloudHoleDetectionAlgNodelet();
+};
+
+
 #endif
diff --git a/launch/nodelet.launch b/launch/nodelet.launch
index 8bc3fd6b2fc60d0bb6def78f87a44483cffff4e8..5636ced5bb631be7986348a02663bd7752e8db52 100644
--- a/launch/nodelet.launch
+++ b/launch/nodelet.launch
@@ -14,7 +14,7 @@
     <node name="$(arg node_name)"
           pkg="nodelet"
           type="nodelet"
-          args="load iri_point_cloud_hole_detection/AveragePointCloudAlgNodelet $(arg camera_nodelet_manager)"
+          args="load iri_point_cloud_hole_detection/PointCloudHoleDetectionAlgNodelet $(arg camera_nodelet_manager)"
           output="$(arg output)">
       <remap from="~input" to="$(arg cloud_in)"/>
       <rosparam file="$(arg config_file)" command="load"/>
diff --git a/package.xml b/package.xml
index 7d1923bdb6a1a692f86d0953a6bcdee39887fd36..b7bcb626fe787f710c4083ed068c0ebcd33ea46e 100644
--- a/package.xml
+++ b/package.xml
@@ -41,14 +41,16 @@
   <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>
     <!-- Other tools can request additional information be placed here -->
+    <nodelet plugin="${prefix}/point_cloud_hole_detector_nodelet_plugin.xml" />
   </export>
 </package>
diff --git a/point_cloud_hole_detector_nodelet_plugin.xml b/point_cloud_hole_detector_nodelet_plugin.xml
new file mode 100755
index 0000000000000000000000000000000000000000..f71920c692a152c203f3939c625ba9de540bcde0
--- /dev/null
+++ b/point_cloud_hole_detector_nodelet_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libiri_point_cloud_hole_detection_nodelet">
+  <class name="iri_point_cloud_hole_detection/PointCloudHoleDetectionAlgNodelet" type="PointCloudHoleDetectionAlgNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelete for the IRI navigation Velodyne angle filter 
+    </description>
+  </class>
+</library>
diff --git a/src/iri_point_cloud_hole_detection_alg_node.cpp b/src/iri_point_cloud_hole_detection_alg_node.cpp
index 0b5494a95e7ccb6c9cdcadfffbd4a548a59c4146..9e069067c10b0ac47adbbf08c64650be58a47d20 100644
--- a/src/iri_point_cloud_hole_detection_alg_node.cpp
+++ b/src/iri_point_cloud_hole_detection_alg_node.cpp
@@ -1,7 +1,7 @@
 #include "iri_point_cloud_hole_detection_alg_node.h"
 
-PointCloudHoleDetectionAlgNode::PointCloudHoleDetectionAlgNode(void) :
-  algorithm_base::IriBaseAlgorithm<PointCloudHoleDetectionAlgorithm>()
+PointCloudHoleDetectionAlgNode::PointCloudHoleDetectionAlgNode(const ros::NodeHandle &nh) :
+  algorithm_base::IriBaseAlgorithm<PointCloudHoleDetectionAlgorithm>(nh)
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
@@ -97,3 +97,48 @@ int main(int argc,char *argv[])
 {
   return algorithm_base::main<PointCloudHoleDetectionAlgNode>(argc, argv, "iri_point_cloud_hole_detection_alg_node");
 }
+
+#include <pluginlib/class_list_macros.h>
+
+PointCloudHoleDetectionAlgNodelet::PointCloudHoleDetectionAlgNodelet()
+{
+  this->node=NULL;
+}
+
+PointCloudHoleDetectionAlgNodelet::~PointCloudHoleDetectionAlgNodelet(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 PointCloudHoleDetectionAlgNodelet::onInit()
+{
+  // initialize the driver node
+  this->node=new PointCloudHoleDetectionAlgNode(getPrivateNodeHandle());
+  // initialize the thread
+  this->thread_server=CThreadServer::instance();
+  this->spin_thread_id=getName() + "_point_cloud_hole_detector_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 *PointCloudHoleDetectionAlgNodelet::spin_thread(void *param)
+{
+  PointCloudHoleDetectionAlgNodelet *nodelet=(PointCloudHoleDetectionAlgNodelet *)param;
+
+  nodelet->node->spin();
+
+  pthread_exit(NULL);
+}
+
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(iri_point_cloud_hole_detection, PointCloudHoleDetectionAlgNodelet, PointCloudHoleDetectionAlgNodelet, nodelet::Nodelet);
+