diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt
index 53275398..86f532a2 100644
--- a/pcl_ros/CMakeLists.txt
+++ b/pcl_ros/CMakeLists.txt
@@ -67,7 +67,7 @@ target_link_libraries (pcd_to_pointcloud pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARI
 add_executable (pointcloud_to_pcd tools/pointcloud_to_pcd.cpp)
 target_link_libraries (pointcloud_to_pcd pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
 
-add_executable (bag_to_pcd tools/bag_to_pcd.cpp)
+add_executable ( bag_to_pcd tools/bag_to_pcd.cpp)
 target_link_libraries (bag_to_pcd pcl_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
 
 add_executable (convert_pcd_to_image tools/convert_pcd_to_image.cpp)
@@ -81,8 +81,15 @@ install(DIRECTORY include/${PROJECT_NAME}/
         DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
-install(TARGETS pcl_ros_tf
+install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
+
+install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image
+  DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
+install(FILES pcl_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+
diff --git a/pcl_ros/src/pcl_ros/io/bag_io.cpp b/pcl_ros/src/pcl_ros/io/bag_io.cpp
new file mode 100644
index 00000000..cc370da9
--- /dev/null
+++ b/pcl_ros/src/pcl_ros/io/bag_io.cpp
@@ -0,0 +1,113 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: bag_io.cpp 34896 2010-12-19 06:21:42Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/io/bag_io.h"
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+bool 
+pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name)
+{
+  try
+  {
+    bag_.open (file_name, rosbag::bagmode::Read);
+    view_.addQuery (bag_, rosbag::TopicQuery (topic_name));
+
+    if (view_.size () == 0)
+      return (false);
+
+    it_ = view_.begin ();
+  }
+  catch (rosbag::BagException &e)
+  {
+    return (false);
+  }
+  return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::BAGReader::onInit ()
+{
+  boost::shared_ptr<ros::NodeHandle> pnh_;
+  pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
+  // ---[ Mandatory parameters
+  if (!pnh_->getParam ("file_name", file_name_))
+  {
+    NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
+    return;
+  }
+   if (!pnh_->getParam ("topic_name", topic_name_))
+  {
+    NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!"); 
+    return;
+  }
+  // ---[ Optional parameters
+  int max_queue_size = 1;
+  pnh_->getParam ("publish_rate", publish_rate_);
+  pnh_->getParam ("max_queue_size", max_queue_size);
+
+  ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2> ("output", max_queue_size);
+
+  NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n"
+                 " - file_name    : %s\n"
+                 " - topic_name   : %s",
+                 file_name_.c_str (), topic_name_.c_str ());
+
+  if (!open (file_name_, topic_name_))
+    return;
+  PointCloud output;
+  output_ = boost::make_shared<PointCloud> (output);
+  output_->header.stamp    = ros::Time::now ();
+
+  // Continous publishing enabled?
+  while (pnh_->ok ())
+  {
+    PointCloudConstPtr cloud = getNextCloud ();
+    NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ());
+    output_->header.stamp = ros::Time::now ();
+
+    pub_output.publish (output_);
+
+    ros::Duration (publish_rate_).sleep ();
+    ros::spinOnce ();
+  }
+}
+
+typedef pcl_ros::BAGReader BAGReader;
+PLUGINLIB_DECLARE_CLASS (pcl, BAGReader, BAGReader, nodelet::Nodelet);
+
diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp
new file mode 100644
index 00000000..3055f4cd
--- /dev/null
+++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp
@@ -0,0 +1,257 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include <pcl/io/io.h>
+#include "pcl_ros/transforms.h"
+#include "pcl_ros/io/concatenate_data.h"
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
+{
+  private_nh_ = getMTPrivateNodeHandle ();
+  // ---[ Mandatory parameters
+  private_nh_.getParam ("output_frame", output_frame_);
+  private_nh_.getParam ("approximate_sync", approximate_sync_);
+
+  if (output_frame_.empty ())
+  {
+    NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
+    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))
+  {
+    NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
+    return; 
+  }
+  // Check the type
+  switch (input_topics.getType ())
+  {
+    case XmlRpc::XmlRpcValue::TypeArray:
+    {
+      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_));
+      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;
+        }
+      }
+      break;
+    }
+    default:
+    {
+      NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
+      return;
+    }
+  }
+
+  if (approximate_sync_)
+    ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
+  else
+    ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void 
+pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
+{
+  //ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
+  PointCloud2::Ptr in1_t (new PointCloud2 ());
+  PointCloud2::Ptr in2_t (new PointCloud2 ());
+
+  // Transform the point clouds into the specified output frame
+  if (output_frame_ != in1.header.frame_id)
+    pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_);
+  else
+    in1_t = boost::make_shared<PointCloud2> (in1);
+
+  if (output_frame_ != in2.header.frame_id)
+    pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_);
+  else
+    in2_t = boost::make_shared<PointCloud2> (in2);
+
+  // Concatenate the results
+  pcl::concatenatePointCloud (*in1_t, *in2_t, out);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void 
+pcl_ros::PointCloudConcatenateDataSynchronizer::input (
+    const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, 
+    const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, 
+    const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, 
+    const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8)
+{
+  PointCloud2::Ptr out1 (new PointCloud2 ());
+  PointCloud2::Ptr out2 (new PointCloud2 ());
+  pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1);
+  if (in3)
+  {
+    pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2);
+    out1 = out2;
+    if (in4)
+    {
+      pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1);
+      if (in5)
+      {
+        pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2);
+        out1 = out2;
+        if (in6)
+        {
+          pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1);
+          if (in7)
+          {
+            pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2);
+            out1 = out2;
+          } 
+        }
+      }
+    }
+  }
+  pub_output_.publish (boost::make_shared<PointCloud2> (*out1)); 
+}
+
+typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer;
+PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateDataSynchronizer, PointCloudConcatenateDataSynchronizer, nodelet::Nodelet);
+
diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp
new file mode 100644
index 00000000..e9a907c5
--- /dev/null
+++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp
@@ -0,0 +1,152 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: concatenate_fields.cpp 35052 2011-01-03 21:04:57Z rusu $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#include <pluginlib/class_list_macros.h>
+#include <pcl/io/io.h>
+#include "pcl_ros/io/concatenate_fields.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
+{
+  private_nh_ = getMTPrivateNodeHandle ();
+  // ---[ Mandatory parameters
+  if (!private_nh_.getParam ("input_messages", input_messages_))
+  {
+    NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!");
+    return;
+  }
+  if (input_messages_ < 2)
+  {
+    NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!");
+    return;
+  }
+  // ---[ Optional parameters
+  private_nh_.getParam ("max_queue_size", maximum_queue_size_);
+  private_nh_.getParam ("maximum_seconds", maximum_seconds_);
+  sub_input_ = private_nh_.subscribe ("input", maximum_queue_size_,  &PointCloudConcatenateFieldsSynchronizer::input_callback, this);
+  pub_output_ = private_nh_.advertise<sensor_msgs::PointCloud2> ("output", maximum_queue_size_);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud)
+{
+  NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
+                 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), private_nh_.resolveName ("input").c_str ());
+
+  // Erase old data in the queue
+  if (maximum_seconds_ > 0 && queue_.size () > 0)
+  {
+    while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0)
+    {
+      NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
+                 (*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ));
+      queue_.erase (queue_.begin ());
+    }
+  }
+
+  // Push back new data
+  queue_[cloud->header.stamp].push_back (cloud);
+  if ((int)queue_[cloud->header.stamp].size () >= input_messages_)
+  {
+    // Concatenate together and publish
+    std::vector<PointCloudConstPtr> &clouds = queue_[cloud->header.stamp];
+    PointCloud cloud_out = *clouds[0];
+
+    // Resize the output dataset
+    int data_size = cloud_out.data.size ();
+    int nr_fields = cloud_out.fields.size ();
+    int nr_points = cloud_out.width * cloud_out.height;
+    for (size_t i = 1; i < clouds.size (); ++i)
+    {
+      assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
+
+      if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height)
+      {
+        NODELET_ERROR ("[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!", 
+            i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
+        break;
+      }
+      // Point step must increase with the length of each new field
+      cloud_out.point_step += clouds[i]->point_step;
+      // Resize data to hold all clouds
+      data_size += clouds[i]->data.size ();
+
+      // Concatenate fields
+      cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ());
+      int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype);
+      for (size_t d = 0; d < clouds[i]->fields.size (); ++d)
+      {
+        cloud_out.fields[nr_fields + d] = clouds[i]->fields[d];
+        cloud_out.fields[nr_fields + d].offset += delta_offset;
+      }
+      nr_fields += clouds[i]->fields.size ();
+    }
+    // Recalculate row_step
+    cloud_out.row_step = cloud_out.point_step * cloud_out.width;
+    cloud_out.data.resize (data_size);
+
+    // Iterate over each point and perform the appropriate memcpys
+    int point_offset = 0;
+    for (int cp = 0; cp < nr_points; ++cp)
+    {
+      for (size_t i = 0; i < clouds.size (); ++i)
+      {
+        // Copy each individual point
+        memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step);
+        point_offset += clouds[i]->point_step;
+      }
+    }
+    pub_output_.publish (boost::make_shared<const PointCloud> (cloud_out));
+    queue_.erase (cloud->header.stamp);
+  }
+
+  // Clean the queue to avoid overflowing
+  if (maximum_queue_size_ > 0)
+  {
+    while ((int)queue_.size () > maximum_queue_size_)
+      queue_.erase (queue_.begin ());
+  }
+
+}
+
+typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;
+PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateFieldsSynchronizer, PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet);
+
diff --git a/pcl_ros/src/pcl_ros/io/io.cpp b/pcl_ros/src/pcl_ros/io/io.cpp
new file mode 100644
index 00000000..41139a6b
--- /dev/null
+++ b/pcl_ros/src/pcl_ros/io/io.cpp
@@ -0,0 +1,57 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: io.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <message_filters/subscriber.h>
+//#include <pcl_ros/subscriber.h>
+#include <nodelet_topic_tools/nodelet_mux.h>
+#include <nodelet_topic_tools/nodelet_demux.h>
+
+typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2, message_filters::Subscriber<sensor_msgs::PointCloud2> > NodeletMUX;
+//typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2, pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX;
+typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
+
+//#include "pcd_io.cpp"
+//#include "bag_io.cpp"
+//#include "concatenate_fields.cpp"
+//#include "concatenate_data.cpp"
+
+PLUGINLIB_DECLARE_CLASS (pcl, NodeletMUX, NodeletMUX, nodelet::Nodelet);
+PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX, NodeletDEMUX, nodelet::Nodelet);
+//PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX_ROS, NodeletDEMUX_ROS, nodelet::Nodelet);
+
diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp
new file mode 100644
index 00000000..5d4e24e4
--- /dev/null
+++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp
@@ -0,0 +1,175 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: pcd_io.cpp 35812 2011-02-08 00:05:03Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include <pcl_ros/io/pcd_io.h>
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PCDReader::onInit ()
+{
+  ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
+  // Provide a latched topic
+  ros::Publisher pub_output = private_nh.advertise<PointCloud2> ("output", max_queue_size_, true);
+
+  private_nh.getParam ("publish_rate", publish_rate_);
+  private_nh.getParam ("tf_frame", tf_frame_);
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - publish_rate : %f\n"
+                 " - tf_frame     : %s",
+                 getName ().c_str (),
+                 publish_rate_, tf_frame_.c_str ());
+
+  PointCloud2Ptr output_new;
+  output_ = boost::make_shared<PointCloud2> ();
+  output_new = boost::make_shared<PointCloud2> ();
+
+  // Wait in a loop until someone connects
+  do
+  {
+    ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
+    ros::spinOnce ();
+    ros::Duration (0.01).sleep ();
+  }
+  while (private_nh.ok () && pub_output.getNumSubscribers () == 0);
+
+  std::string file_name;
+
+  while (private_nh.ok ())
+  {
+    // Get the current filename parameter. If no filename set, loop
+    if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ())
+    {
+      ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
+      ros::Duration (0.01).sleep ();
+      ros::spinOnce ();
+      continue;
+    }
+
+    // If the filename parameter holds a different value than the last one we read
+    if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
+    {
+      NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
+      file_name = file_name_;
+      PointCloud2 cloud;
+      if (impl_.read (file_name_, cloud) < 0)
+      {
+        NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
+        return;
+      }
+      output_ = boost::make_shared<PointCloud2> (cloud);
+      output_->header.stamp    = ros::Time::now ();
+      output_->header.frame_id = tf_frame_;
+    }
+
+    // We do not publish empty data
+    if (output_->data.size () == 0)
+      continue;
+
+    if (publish_rate_ == 0)
+    {
+      if (output_ != output_new)
+      {
+        NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
+        pub_output.publish (output_);
+        output_new = output_;
+      }
+      ros::Duration (0.01).sleep ();
+    }
+    else
+    {
+      NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
+      output_->header.stamp = ros::Time::now ();
+      pub_output.publish (output_);
+
+      ros::Duration (publish_rate_).sleep ();
+    }
+
+    ros::spinOnce ();
+    // Update parameters from server
+    private_nh.getParam ("publish_rate", publish_rate_);
+    private_nh.getParam ("tf_frame", tf_frame_);
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PCDWriter::onInit ()
+{
+  ros::NodeHandle pnh = getMTPrivateNodeHandle ();
+  sub_input_ = pnh.subscribe ("input", 1,  &PCDWriter::input_callback, this);
+  // ---[ Optional parameters
+  pnh.getParam ("filename", file_name_);
+  pnh.getParam ("binary_mode", binary_mode_);
+
+  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
+                 " - filename     : %s\n"
+                 " - binary_mode  : %s", 
+                 getName ().c_str (),
+                 file_name_.c_str (), (binary_mode_) ? "true" : "false");
+
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
+{
+  if (!isValid (cloud))
+    return;
+  
+  getMTPrivateNodeHandle ().getParam ("filename", file_name_);
+
+  NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
+ 
+  std::string fname;
+  if (file_name_.empty ())
+    fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
+  else
+    fname = file_name_;
+  impl_.write (fname, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
+
+  NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
+}
+
+typedef pcl_ros::PCDReader PCDReader;
+typedef pcl_ros::PCDWriter PCDWriter;
+PLUGINLIB_DECLARE_CLASS (pcl, PCDReader, PCDReader, nodelet::Nodelet);
+PLUGINLIB_DECLARE_CLASS (pcl, PCDWriter, PCDWriter, nodelet::Nodelet);
+