Refactors to use pcl-1.7

pcl_ros/src/pcl_ros/filters/voxel_grid.cpp

@@ -58,10 +58,13 @@ pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input,
                             PointCloud2 &output)
 {
   boost::mutex::scoped_lock lock (mutex_);
-
-  impl_.setInputCloud (input);
+  pcl::PCLPointCloud2::Ptr pcl_input;
+  pcl_conversions::toPCL (*(input), *(pcl_input));
+  impl_.setInputCloud (pcl_input);
   impl_.setIndices (indices);
-  impl_.filter (output);
+  pcl::PCLPointCloud2 pcl_output;
+  impl_.filter (pcl_output);
+  pcl_conversions::moveFromPCL(pcl_output, output);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

pcl_ros/src/pcl_ros/io/concatenate_data.cpp

@@ -40,6 +40,8 @@
 #include "pcl_ros/transforms.h"
 #include "pcl_ros/io/concatenate_data.h"
 
+#include <pcl_conversions/pcl_conversions.h>
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()

pcl_ros/src/pcl_ros/io/concatenate_fields.cpp

@@ -41,6 +41,8 @@
 #include <pcl/io/io.h>
 #include "pcl_ros/io/concatenate_fields.h"
 
+#include <pcl_conversions/pcl_conversions.h>
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()

pcl_ros/src/pcl_ros/io/pcd_io.cpp

@@ -86,13 +86,13 @@ pcl_ros::PCDReader::onInit ()
     {
       NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
       file_name = file_name_;
-      PointCloud2 cloud;
+      pcl::PCLPointCloud2 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);
+      pcl_conversions::moveFromPCL(cloud, *(output_));
       output_->header.stamp    = ros::Time::now ();
       output_->header.frame_id = tf_frame_;
     }
@@ -163,7 +163,10 @@ pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
     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_);
+  pcl::PCLPointCloud2 pcl_cloud;
+  // It is safe to remove the const here because we are the only subscriber callback.
+  pcl_conversions::moveToPCL(*(const_cast<PointCloud2*>(cloud.get())), pcl_cloud);
+  impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
 
   NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
 }

pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp

@@ -37,8 +37,15 @@
 
 #include <pluginlib/class_list_macros.h>
 #include <pcl/io/io.h>
+#include <pcl/PointIndices.h>
 #include "pcl_ros/segmentation/extract_clusters.h"
 
+#include <pcl_conversions/pcl_conversions.h>
+
+using pcl_conversions::fromPCL;
+using pcl_conversions::moveFromPCL;
+using pcl_conversions::toPCL;
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::EuclideanClusterExtraction::onInit ()
@@ -158,15 +165,18 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
   }
 
   /// DEBUG
-  if (indices)
+  if (indices) {
+    std_msgs::Header cloud_header = fromPCL(cloud->header);
+    std_msgs::Header indices_header = indices->header;
     NODELET_DEBUG ("[%s::input_indices_callback]\n"
                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
                    getName ().c_str (),
-                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
-                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
-  else
-    NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
+  } else {
+    NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
+  }
   ///
 
   IndicesPtr indices_ptr;
@@ -176,7 +186,7 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
   impl_.setInputCloud (cloud);
   impl_.setIndices (indices_ptr);
 
-  std::vector<PointIndices> clusters;
+  std::vector<pcl::PointIndices> clusters;
   impl_.extract (clusters);
 
   if (publish_indices_)
@@ -186,8 +196,10 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
       if ((int)i >= max_clusters_)
         break;
       // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
-      clusters[i].header.stamp += ros::Duration (i * 0.001);
-      pub_output_.publish (boost::make_shared<const PointIndices> (clusters[i]));
+      pcl_msgs::PointIndices ros_pi;
+      moveFromPCL(clusters[i], ros_pi);
+      ros_pi.header.stamp += ros::Duration (i * 0.001);
+      pub_output_.publish (ros_pi);
     }
 
     NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
@@ -204,11 +216,13 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
       //PointCloud output_blob;     // Convert from the templated output to the PointCloud blob
       //pcl::toROSMsg (output, output_blob);
       // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
-      output.header.stamp += ros::Duration (i * 0.001);
+      std_msgs::Header header = fromPCL(output.header);
+      header.stamp += ros::Duration (i * 0.001);
+      toPCL(header, output.header);
       // Publish a Boost shared ptr const data
       pub_output_.publish (output.makeShared ());
       NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
-                     i, clusters[i].indices.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
+                     i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
     }
   }
 }

pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp

@@ -42,8 +42,8 @@
 
 #include <pcl_conversions/pcl_conversions.h>
 
-using pcl_conversions::fromPCL;
-using pcl_conversions::toPCL;
+using pcl_conversions::moveFromPCL;
+using pcl_conversions::moveToPCL;
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
@@ -132,14 +132,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
   if (!isValid (cloud) || !isValid (hull, "planar_hull"))
   {
     NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
-    pub_output_.publish (boost::make_shared<const pcl_msgs::PointIndices> (inliers));
+    pub_output_.publish (inliers);
     return;
   }
   // If indices are given, check if they are valid
   if (indices && !isValid (indices))
   {
     NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
-    pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
+    pub_output_.publish (inliers);
     return;
   }
 
@@ -169,7 +169,7 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
     if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_))
     {
       // Publish empty before return
-      pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
+      pub_output_.publish (inliers);
       return;
     }
     impl_.setInputPlanarHull (planar_hull.makeShared ());
@@ -187,13 +187,13 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
   // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
   if (!cloud->points.empty ()) {
     pcl::PointIndices pcl_inliers;
-    toPCL(inliers, pcl_inliers);
+    moveToPCL(inliers, pcl_inliers);
     impl_.segment (pcl_inliers);
-    fromPCL(pcl_inliers, inliers);
+    moveFromPCL(pcl_inliers, inliers);
   }
   // Enforce that the TF frame and the timestamp are copied
   inliers.header = fromPCL(cloud->header);
-  pub_output_.publish (boost::make_shared<const PointIndices> (inliers));
+  pub_output_.publish (inliers);
   NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
 }
 

pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp

@@ -39,6 +39,10 @@
 #include "pcl_ros/segmentation/sac_segmentation.h"
 #include <pcl/io/io.h>
 
+#include <pcl_conversions/pcl_conversions.h>
+
+using pcl_conversions::fromPCL;
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::SACSegmentation::onInit ()
@@ -249,25 +253,25 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
   if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
     return;
 
-  PointIndices inliers;
-  ModelCoefficients model;
+  pcl_msgs::PointIndices inliers;
+  pcl_msgs::ModelCoefficients model;
   // Enforce that the TF frame and the timestamp are copied
-  inliers.header = model.header = cloud->header;
+  inliers.header = model.header = fromPCL(cloud->header);
 
   // If cloud is given, check if it's valid
   if (!isValid (cloud)) 
   {
     NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
-    pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
-    pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+    pub_indices_.publish (inliers);
+    pub_model_.publish (model);
     return;
   }
   // If indices are given, check if they are valid
   if (indices && !isValid (indices))
   {
     NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
-    pub_indices_.publish (boost::make_shared<const PointIndices> (inliers));
-    pub_model_.publish (boost::make_shared<const ModelCoefficients> (model));
+    pub_indices_.publish (inliers);
+    pub_model_.publish (model);
     return;
   }
 
@@ -277,11 +281,11 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
                    getName ().c_str (), 
-                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
                    indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
   else
     NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", 
-                   getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
+                   getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
   ///
 
   // Check whether the user has given a different input TF frame
@@ -308,8 +312,15 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou
   impl_.setIndices (indices_ptr);
 
   // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
-  if (!cloud->points.empty ())
-    impl_.segment (inliers, model);
+  if (!cloud->points.empty ()) {
+    pcl::PointIndices pcl_inliers;
+    pcl::ModelCoefficients pcl_model;
+    pcl_conversions::moveToPCL(inliers, pcl_inliers);
+    pcl_conversions::moveToPCL(model, pcl_model);
+    impl_.segment (pcl_inliers, pcl_model);
+    pcl_conversions::moveFromPCL(pcl_inliers, inliers);
+    pcl_conversions::moveFromPCL(pcl_model, model);
+  }
 
   // Probably need to transform the model of the plane here
 
@@ -453,7 +464,7 @@ pcl_ros::SACSegmentationFromNormals::onInit ()
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl::ModelCoefficientsConstPtr &model)
+pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model)
 {
   boost::mutex::scoped_lock lock (mutex_);
 
@@ -551,7 +562,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
   PointIndices inliers;
   ModelCoefficients model;
   // Enforce that the TF frame and the timestamp are copied
-  inliers.header = model.header = cloud->header;
+  inliers.header = model.header = fromPCL(cloud->header);
 
   if (impl_.getModelType () < 0)
   {
@@ -584,16 +595,16 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
                    getName ().c_str (), 
-                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
-                   cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
                    indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
   else
     NODELET_DEBUG ("[%s::input_normals_indices_callback]\n"
                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
                    getName ().c_str (), 
-                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
-                   cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                   cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
   ///
 
 
@@ -618,8 +629,15 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
   impl_.setIndices (indices_ptr);
 
   // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
-  if (!cloud->points.empty ())
-    impl_.segment (inliers, model);
+  if (!cloud->points.empty ()) {
+    pcl::PointIndices pcl_inliers;
+    pcl::ModelCoefficients pcl_model;
+    pcl_conversions::moveToPCL(inliers, pcl_inliers);
+    pcl_conversions::moveToPCL(model, pcl_model);
+    impl_.segment (pcl_inliers, pcl_model);
+    pcl_conversions::moveFromPCL(pcl_inliers, inliers);
+    pcl_conversions::moveFromPCL(pcl_model, model);
+  }
 
   // Check if we have enough inliers, clear inliers + model if not
   if ((int)inliers.indices.size () <= min_inliers_)

pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp

@@ -109,8 +109,8 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl
                  "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                  "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
                  getName ().c_str (),
-                 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
-                 cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), cloud_target->header.stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());
+                 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
+                 cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());
 
   impl_.setInputCloud (cloud);
   impl_.setTargetCloud (cloud_target);
@@ -120,7 +120,7 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl
 
   pub_output_.publish (output.makeShared ());
   NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (),
-                     output.points.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
+                     output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ());
 }
 
 typedef pcl_ros::SegmentDifferences SegmentDifferences;

pcl_ros/src/pcl_ros/surface/convex_hull.cpp

@@ -120,10 +120,10 @@ void
                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
                    getName ().c_str (),
-                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
                    indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
   else
-    NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
+    NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
 
   // Reset the indices and surface pointers
   IndicesPtr indices_ptr;
@@ -140,7 +140,7 @@ void
   if (output.points.size () >= 3)
   {
     geometry_msgs::PolygonStamped poly;
-    poly.header = output.header;
+    poly.header = fromPCL(output.header);
     poly.polygon.points.resize (output.points.size ());
     // Get three consecutive points (without copying)
     pcl::Vector4fMap O = output.points[1].getVector4fMap ();

pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp

@@ -141,10 +141,10 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr
                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
                    getName ().c_str (),
-                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
+                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
                    indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
   else
-    NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
+    NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
   ///
 
   // Reset the indices and surface pointers