* added support for TAR-PCD files for @PCDGrabber@. Simply use @tar c…

…vf file.tar *.pcd@ and use @PCDGrabber@ on it afterwards

git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@4932 a9d63959-f2ad-4865-b262-bf0e56cfafb6

io/include/pcl/io/file_io.h

@@ -74,22 +74,33 @@ namespace pcl
         * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
         * \param[out] data_type the type of data (binary data=1, ascii=0, etc)
         * \param[out] data_idx the offset of cloud data within the file
+        * \param[in] offset the offset in the file where to expect the true header to begin.
+        * One usage example for setting the offset parameter is for reading
+        * data from a TAR "archive containing multiple files: TAR files always
+        * add a 512 byte header in front of the actual file, so set the offset
+        * to the next byte after the header (e.g., 513).
         */
       virtual int 
       readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 
                   Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, 
-                  int &file_version, int &data_type, int &data_idx) = 0;
+                  int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) = 0;
 
       /** \brief Read a point cloud data from a FILE file and store it into a sensor_msgs/PointCloud2.
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] cloud the resultant PointCloud message read from disk
         * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present)
         * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present)
         * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7)
+        * \param[in] offset the offset in the file where to expect the true header to begin.
+        * One usage example for setting the offset parameter is for reading
+        * data from a TAR "archive containing multiple files: TAR files always
+        * add a 512 byte header in front of the actual file, so set the offset
+        * to the next byte after the header (e.g., 513).
         */
       virtual int 
       read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 
-            Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version) = 0;
+            Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version, 
+            const int offset = 0) = 0;
 
       /** \brief Read a point cloud data from a FILE file (FILE_V6 only!) and store it into a sensor_msgs/PointCloud2.
         * 
@@ -100,33 +111,44 @@ namespace pcl
         *
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] cloud the resultant PointCloud message read from disk
+        *
+        * \param[in] offset the offset in the file where to expect the true header to begin.
+        * One usage example for setting the offset parameter is for reading
+        * data from a TAR "archive containing multiple files: TAR files always
+        * add a 512 byte header in front of the actual file, so set the offset
+        * to the next byte after the header (e.g., 513).
         */
       int 
-      read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
+      read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, const int offset = 0)
       {
         Eigen::Vector4f origin;
         Eigen::Quaternionf orientation;
         int file_version;
-        return read(file_name, cloud, origin, orientation, file_version);
+        return (read (file_name, cloud, origin, orientation, file_version, offset));
       }
 
       /** \brief Read a point cloud data from any FILE file, and convert it to the given template format.
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] cloud the resultant PointCloud message read from disk
+        * \param[in] offset the offset in the file where to expect the true header to begin.
+        * One usage example for setting the offset parameter is for reading
+        * data from a TAR "archive containing multiple files: TAR files always
+        * add a 512 byte header in front of the actual file, so set the offset
+        * to the next byte after the header (e.g., 513).
         */
       template<typename PointT> inline int
-      read (const std::string &file_name, pcl::PointCloud<PointT> &cloud)
+      read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset  =0)
       {
         sensor_msgs::PointCloud2 blob;
         int file_version;
         int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 
-                        file_version);
+                        file_version, offset);
 
         // Exit in case of error
         if (res < 0)
           return res;
         pcl::fromROSMsg (blob, cloud);
-        return 0;
+        return (0);
       }
   };
 

io/include/pcl/io/impl/pcd_io.hpp

@@ -181,15 +181,15 @@ pcl::PCDWriter::writeBinary (const std::string &file_name,
 
 #else
   // Stretch the file size to the size of the data
-  int result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
+  int result = static_cast<int> (pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET));
   if (result < 0)
   {
     pcl_close (fd);
     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!");
     return (-1);
   }
   // Write a bogus entry so that the new file size comes in effect
-  result = ::write (fd, "", 1);
+  result = static_cast<int> (::write (fd, "", 1));
   if (result != 1)
   {
     pcl_close (fd);
@@ -598,9 +598,9 @@ pcl::PCDWriter::writeBinary (const std::string &file_name,
   }
   int data_idx = 0;
   std::ostringstream oss;
-  oss << generateHeader<PointT> (cloud, indices.size ()) << "DATA binary\n";
+  oss << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA binary\n";
   oss.flush ();
-  data_idx = oss.tellp ();
+  data_idx = static_cast<int> (oss.tellp ());
 
 #if _WIN32
   HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
@@ -647,15 +647,15 @@ pcl::PCDWriter::writeBinary (const std::string &file_name,
 
 #else
   // Stretch the file size to the size of the data
-  int result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
+  int result = static_cast<int> (pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET));
   if (result < 0)
   {
     pcl_close (fd);
     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!");
     return (-1);
   }
   // Write a bogus entry so that the new file size comes in effect
-  result = ::write (fd, "", 1);
+  result = static_cast<int> (::write (fd, "", 1));
   if (result != 1)
   {
     pcl_close (fd);
@@ -746,7 +746,7 @@ pcl::PCDWriter::writeASCII (const std::string &file_name,
   pcl::getFields (cloud, fields);
 
   // Write the header information
-  fs << generateHeader<PointT> (cloud, indices.size ()) << "DATA ascii\n";
+  fs << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA ascii\n";
 
   std::ostringstream stream;
   stream.precision (precision);

io/include/pcl/io/pcd_grabber.h

@@ -2,7 +2,7 @@
  * Software License Agreement (BSD License)
  *
  *  Point Cloud Library (PCL) - www.pointclouds.org
- *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
  *
  *  All rights reserved.
  *
@@ -37,8 +37,8 @@
 
 #include "pcl/pcl_config.h"
 
-#ifndef __PCL_IO_PCD_GRABBER__
-#define __PCL_IO_PCD_GRABBER__
+#ifndef PCL_IO_PCD_GRABBER_H_
+#define PCL_IO_PCD_GRABBER_H_
 
 #include <pcl/io/grabber.h>
 #include <pcl/common/time_trigger.h>
@@ -54,14 +54,14 @@ namespace pcl
   class PCL_EXPORTS PCDGrabberBase : public Grabber
   {
     public:
-      /** \brief Constructor taking just one PCD file.
+      /** \brief Constructor taking just one PCD file or one TAR file containing multiple PCD files.
         * \param[in] pcd_file path to the PCD file
         * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
         * \param[in] repeat whether to play PCD file in an endless loop or not.
         */
       PCDGrabberBase (const std::string& pcd_file, float frames_per_second, bool repeat);
 
-      /** \brief Constuctor taking a list of paths to PCD files, that are played in the order the appear in the list.
+      /** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list.
         * \param[in] pcd_files vector of paths to PCD files.
         * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
         * \param[in] repeat whether to play PCD file in an endless loop or not.
@@ -132,39 +132,40 @@ namespace pcl
       PCDGrabberImpl* impl_;
   };
 
-  /**
-   * @ingroup io
-   */
+  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   template <typename T> class PointCloud;
-  //class sensor_msgs::PointCloud2;
   template <typename PointT> class PCDGrabber : public PCDGrabberBase
   {
     public:
       PCDGrabber (const std::string& pcd_path, float frames_per_second = 0, bool repeat = false);
       PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second = 0, bool repeat = false);
     protected:
-      virtual void publish (const sensor_msgs::PointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const;
+      virtual void 
+      publish (const sensor_msgs::PointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const;
       boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_;
   };
 
+  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   template<typename PointT>
   PCDGrabber<PointT>::PCDGrabber (const std::string& pcd_path, float frames_per_second, bool repeat)
-  : PCDGrabberBase ( pcd_path, frames_per_second, repeat)
+  : PCDGrabberBase (pcd_path, frames_per_second, repeat)
   {
     signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
   }
 
+  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   template<typename PointT>
   PCDGrabber<PointT>::PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second, bool repeat)
     : PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ ()
   {
     signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
   }
 
+  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   template<typename PointT>
   void PCDGrabber<PointT>::publish (const sensor_msgs::PointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
   {
-    typename pcl::PointCloud<PointT>::Ptr cloud( new pcl::PointCloud<PointT> () );
+    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
     pcl::fromROSMsg (blob, *cloud);
     cloud->sensor_origin_ = origin;
     cloud->sensor_orientation_ = orientation;

io/include/pcl/io/pcd_io.h

@@ -92,21 +92,28 @@ namespace pcl
         * \attention The PCD data is \b always stored in ROW major format! The
         * read/write PCD methods will detect column major input and automatically convert it.
         *
-        * Returns:
-        *  * < 0 (-1) on error
-        *  * > 0 on success
         * \param[in] file_name the name of the file to load
         * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
         * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present)
         * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present)
         * \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7)
         * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed) 
         * \param[out] data_idx the offset of cloud data within the file
+        * \param[in] offset the offset of where to expect the PCD Header in the
+        * file (optional parameter). One usage example for setting the offset
+        * parameter is for reading data from a TAR "archive containing multiple
+        * PCD files: TAR files always add a 512 byte header in front of the
+        * actual file, so set the offset to the next byte after the header
+        * (e.g., 513).
+        *
+        * \return
+        *  * < 0 (-1) on error
+        *  * > 0 on success
         */
       int 
       readHeader (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 
                   Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version,
-                  int &data_type, int &data_idx);
+                  int &data_type, unsigned int &data_idx, const int offset = 0);
 
       /** \brief Read a point cloud data header from a PCD file. 
         *
@@ -117,29 +124,47 @@ namespace pcl
         * \attention The PCD data is \b always stored in ROW major format! The
         * read/write PCD methods will detect column major input and automatically convert it.
         *
-         * Returns:
-        *  * < 0 (-1) on error
-        *  * > 0 on success
         * \param[in] file_name the name of the file to load
         * \param[out] cloud the resultant point cloud dataset (only the properties will be filled)
         * \param[out] pcd_version the PCD version of the file (either PCD_V6 or PCD_V7)
         * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed) 
         * \param[out] data_idx the offset of cloud data within the file
+        * \param[in] offset the offset of where to expect the PCD Header in the
+        * file (optional parameter). One usage example for setting the offset
+        * parameter is for reading data from a TAR "archive containing multiple
+        * PCD files: TAR files always add a 512 byte header in front of the
+        * actual file, so set the offset to the next byte after the header
+        * (e.g., 513).
+        *
+        * \return
+        *  * < 0 (-1) on error
+        *  * > 0 on success
+        *
         */
       int 
       readHeaderEigen (const std::string &file_name, pcl::PointCloud<Eigen::MatrixXf> &cloud,
-                       int &pcd_version, int &data_type, int &data_idx);
+                       int &pcd_version, int &data_type, unsigned int &data_idx, const int offset = 0);
 
       /** \brief Read a point cloud data from a PCD file and store it into a sensor_msgs/PointCloud2.
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] cloud the resultant PointCloud message read from disk
         * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present)
         * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present)
         * \param[out] pcd_version the PCD version of the file (either PCD_V6 or PCD_V7)
+        * \param[in] offset the offset of where to expect the PCD Header in the
+        * file (optional parameter). One usage example for setting the offset
+        * parameter is for reading data from a TAR "archive containing multiple
+        * PCD files: TAR files always add a 512 byte header in front of the
+        * actual file, so set the offset to the next byte after the header
+        * (e.g., 513).
+        *
+        * \return
+        *  * < 0 (-1) on error
+        *  * > 0 on success
         */
       int 
       read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, 
-            Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version);
+            Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset = 0);
 
       /** \brief Read a point cloud data from a PCD (PCD_V6) and store it into a sensor_msgs/PointCloud2.
         * 
@@ -150,27 +175,46 @@ namespace pcl
         *
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] cloud the resultant PointCloud message read from disk
+        * \param[in] offset the offset of where to expect the PCD Header in the
+        * file (optional parameter). One usage example for setting the offset
+        * parameter is for reading data from a TAR "archive containing multiple
+        * PCD files: TAR files always add a 512 byte header in front of the
+        * actual file, so set the offset to the next byte after the header
+        * (e.g., 513).
+        *
+        * \return
+        *  * < 0 (-1) on error
+        *  * > 0 on success
         */
       int 
-      read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud);
+      read (const std::string &file_name, sensor_msgs::PointCloud2 &cloud, const int offset = 0);
 
       /** \brief Read a point cloud data from any PCD file, and convert it to the given template format.
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] cloud the resultant PointCloud message read from disk
+        * \param[in] offset the offset of where to expect the PCD Header in the
+        * file (optional parameter). One usage example for setting the offset
+        * parameter is for reading data from a TAR "archive containing multiple
+        * PCD files: TAR files always add a 512 byte header in front of the
+        * actual file, so set the offset to the next byte after the header
+        * (e.g., 513).
+        *
+        * \return
+        *  * < 0 (-1) on error
+        *  * > 0 on success
         */
       template<typename PointT> int
-      read (const std::string &file_name, pcl::PointCloud<PointT> &cloud)
+      read (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset = 0)
       {
         sensor_msgs::PointCloud2 blob;
         int pcd_version;
         int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, 
-                        pcd_version);
+                        pcd_version, offset);
 
-        // Exit in case of error
-        if (res < 0)
-          return (res);
-        pcl::fromROSMsg (blob, cloud);
-        return (0);
+        // If no error, convert the data
+        if (res == 0)
+          pcl::fromROSMsg (blob, cloud);
+        return (res);
       }
 
       /** \brief Read a point cloud data from any PCD file, and convert it to a pcl::PointCloud<Eigen::MatrixXf> format.
@@ -179,9 +223,19 @@ namespace pcl
         *
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] cloud the resultant PointCloud message read from disk
+        * \param[in] offset the offset of where to expect the PCD Header in the
+        * file (optional parameter). One usage example for setting the offset
+        * parameter is for reading data from a TAR "archive containing multiple
+        * PCD files: TAR files always add a 512 byte header in front of the
+        * actual file, so set the offset to the next byte after the header
+        * (e.g., 513).
+        *
+        * \return
+        *  * < 0 (-1) on error
+        *  * > 0 on success
         */
       int
-      readEigen (const std::string &file_name, pcl::PointCloud<Eigen::MatrixXf> &cloud);
+      readEigen (const std::string &file_name, pcl::PointCloud<Eigen::MatrixXf> &cloud, const int offset = 0);
   };
 
   /** \brief Point Cloud Data (PCD) file format writer.