diff --git a/pcl_ros/include/pcl_ros/io/concatenate_data.h b/pcl_ros/include/pcl_ros/io/concatenate_data.h
index 7f00c6d5..89603d2a 100644
--- a/pcl_ros/include/pcl_ros/io/concatenate_data.h
+++ b/pcl_ros/include/pcl_ros/io/concatenate_data.h
@@ -40,7 +40,7 @@
 
 // ROS includes
 #include <tf/transform_listener.h>
-#include <nodelet/nodelet.h>
+#include <nodelet_topic_tools/nodelet_lazy.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/pass_through.h>
@@ -57,7 +57,7 @@ namespace pcl_ros
     * PointCloud output message.
     * \author Radu Bogdan Rusu
     */
-  class PointCloudConcatenateDataSynchronizer: public nodelet::Nodelet
+  class PointCloudConcatenateDataSynchronizer: public nodelet_topic_tools::NodeletLazy
   {
     public:
       typedef sensor_msgs::PointCloud2 PointCloud2;
@@ -76,11 +76,10 @@ namespace pcl_ros
       virtual ~PointCloudConcatenateDataSynchronizer () {};
 
       void onInit ();
+      void subscribe ();
+      void unsubscribe ();
 
     private:
-      /** \brief ROS local node handle. */
-      ros::NodeHandle private_nh_;
-      
       /** \brief The output PointCloud publisher. */
       ros::Publisher pub_output_;
 
@@ -96,6 +95,9 @@ namespace pcl_ros
       /** \brief Output TF frame the concatenated points should be transformed to. */
       std::string output_frame_;
 
+      /** \brief Input point cloud topics. */
+      XmlRpc::XmlRpcValue input_topics_;
+
       /** \brief TF listener object. */
       tf::TransformListener tf_;
 
diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp
index f7a09a84..a7a00754 100644
--- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp
+++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp
@@ -46,10 +46,11 @@
 void
 pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
 {
-  private_nh_ = getMTPrivateNodeHandle ();
+  nodelet_topic_tools::NodeletLazy::onInit ();
+
   // ---[ Mandatory parameters
-  private_nh_.getParam ("output_frame", output_frame_);
-  private_nh_.getParam ("approximate_sync", approximate_sync_);
+  pnh_->getParam ("output_frame", output_frame_);
+  pnh_->getParam ("approximate_sync", approximate_sync_);
 
   if (output_frame_.empty ())
   {
@@ -57,133 +58,130 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
     return;
   }
 
-  // ---[ Optional parameters
-  private_nh_.getParam ("max_queue_size", maximum_queue_size_);
-
-  // Output
-  pub_output_ = private_nh_.advertise<PointCloud2> ("output", maximum_queue_size_);
-
-  XmlRpc::XmlRpcValue input_topics;
-  if (!private_nh_.getParam ("input_topics", input_topics))
+  if (!pnh_->getParam ("input_topics", input_topics_))
   {
     NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
-    return; 
+    return;
   }
-  // Check the type
-  switch (input_topics.getType ())
+  if (input_topics_.getType () != XmlRpc::XmlRpcValue::TypeArray)
   {
-    case XmlRpc::XmlRpcValue::TypeArray:
+    NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
+    return;
+  }
+  if (input_topics_.size () == 1)
+  {
+    NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
+    return;
+  }
+  if (input_topics_.size () > 8)
+  {
+    NODELET_ERROR ("[onInit] More than 8 topics passed!");
+    return;
+  }
+
+  // ---[ Optional parameters
+  pnh_->getParam ("max_queue_size", maximum_queue_size_);
+
+  // Output
+  pub_output_ = advertise<PointCloud2> (*pnh_, "output", maximum_queue_size_);
+
+  onInitPostProcess ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe ()
+{
+  ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:");
+  for (int d = 0; d < input_topics_.size (); ++d)
+    ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d]));
+
+  // Subscribe to the filters
+  filters_.resize (input_topics_.size ());
+
+  // 8 topics
+  if (approximate_sync_)
+    ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
+                                                                                  PointCloud2, PointCloud2, PointCloud2,
+                                                                                  PointCloud2, PointCloud2>
+                                                  > (maximum_queue_size_));
+  else
+    ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
+                                                                            PointCloud2, PointCloud2, PointCloud2,
+                                                                            PointCloud2, PointCloud2>
+                                                  > (maximum_queue_size_));
+
+  // First input_topics_.size () filters are valid
+  for (int d = 0; d < input_topics_.size (); ++d)
+  {
+    filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
+    filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
+  }
+
+  // Bogus null filter
+  filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
+
+  switch (input_topics_.size ())
+  {
+    case 2:
     {
-      if (input_topics.size () == 1)
-      {
-        NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
-        return;
-      }
-
-      if (input_topics.size () > 8)
-      {
-        NODELET_ERROR ("[onInit] More than 8 topics passed!");
-        return;
-       }
-
-      ROS_INFO_STREAM ("[onInit] Subscribing to " << input_topics.size () << " user given topics as inputs:");
-      for (int d = 0; d < input_topics.size (); ++d)
-        ROS_INFO_STREAM (" - " << (std::string)(input_topics[d]));
-
-      // Subscribe to the filters
-      filters_.resize (input_topics.size ());
-
-      // 8 topics
       if (approximate_sync_)
-        ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
-                                                                                      PointCloud2, PointCloud2, PointCloud2,
-                                                                                      PointCloud2, PointCloud2> 
-                                                      > (maximum_queue_size_));
+        ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
       else
-        ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
-                                                                                PointCloud2, PointCloud2, PointCloud2,
-                                                                                PointCloud2, PointCloud2> 
-                                                      > (maximum_queue_size_));
-
-      // First input_topics.size () filters are valid
-      for (int d = 0; d < input_topics.size (); ++d)
-      {
-        filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
-        filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), maximum_queue_size_);
-      }
-
-      // Bogus null filter
-      filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
-
-      switch (input_topics.size ())
-      {
-        case 2:
-        {
-          if (approximate_sync_)
-            ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
-          else
-            ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
-          break;
-        }
-        case 3:
-        {
-          if (approximate_sync_)
-            ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
-          else
-            ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
-          break;
-        }
-        case 4:
-        {
-          if (approximate_sync_)
-            ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
-          else
-            ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
-          break;
-        }
-        case 5:
-        {
-          if (approximate_sync_)
-            ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
-          else
-            ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
-          break;
-        }
-        case 6:
-        {
-          if (approximate_sync_)
-            ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
-          else
-            ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
-          break;
-        }
-        case 7:
-        {
-          if (approximate_sync_)
-            ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
-          else
-            ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
-          break;
-        }
-        case 8:
-        {
-          if (approximate_sync_)
-            ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
-          else
-            ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
-          break;
-        }
-        default:
-        {
-          NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
-          return;
-        }
-      }
+        ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
+      break;
+    }
+    case 3:
+    {
+      if (approximate_sync_)
+        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
+      else
+        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
+      break;
+    }
+    case 4:
+    {
+      if (approximate_sync_)
+        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
+      else
+        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
+      break;
+    }
+    case 5:
+    {
+      if (approximate_sync_)
+        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
+      else
+        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
+      break;
+    }
+    case 6:
+    {
+      if (approximate_sync_)
+        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
+      else
+        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
+      break;
+    }
+    case 7:
+    {
+      if (approximate_sync_)
+        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
+      else
+        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
+      break;
+    }
+    case 8:
+    {
+      if (approximate_sync_)
+        ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
+      else
+        ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
       break;
     }
     default:
     {
-      NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
+      NODELET_FATAL ("Invalid 'input_topics' parameter given!");
       return;
     }
   }
@@ -194,6 +192,16 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
     ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe ()
+{
+  for (int d = 0; d < filters_.size (); ++d)
+  {
+    filters_[d]->unsubscribe ();
+  }
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void 
 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)