bag_to_pcd.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: bag_to_pcd.cpp 35812 2011-02-08 00:05:03Z rusu $
00035  *
00036  */
00037 
00047 #include <sstream>
00048 #include <boost/filesystem.hpp>
00049 #include <rosbag/bag.h>
00050 #include <rosbag/view.h>
00051 #include "pcl/io/io.h"
00052 #include "pcl/io/pcd_io.h"
00053 #include "pcl_ros/transforms.h"
00054 #include <tf/transform_listener.h>
00055 #include <tf/transform_broadcaster.h>
00056 
00057 typedef sensor_msgs::PointCloud2 PointCloud;
00058 typedef PointCloud::Ptr PointCloudPtr;
00059 typedef PointCloud::ConstPtr PointCloudConstPtr;
00060 
00061 /* ---[ */
00062 int
00063   main (int argc, char** argv)
00064 {
00065   ros::init (argc, argv, "bag_to_pcd");
00066   if (argc < 4)
00067   {
00068     std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory>" << std::endl;
00069     std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds" << std::endl;
00070     return (-1);
00071   }
00072 
00073   // TF
00074   tf::TransformListener tf_listener;
00075   tf::TransformBroadcaster tf_broadcaster;
00076 
00077   rosbag::Bag bag;
00078   rosbag::View view;
00079   rosbag::View::iterator view_it;
00080 
00081   try
00082   {
00083     bag.open (argv[1], rosbag::bagmode::Read);
00084   } 
00085   catch (rosbag::BagException) 
00086   {
00087     std::cerr << "Error opening file " << argv[1] << std::endl;
00088     return (-1);
00089   }
00090 
00091   view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2"));
00092   view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage"));
00093   view_it = view.begin ();
00094 
00095   std::string output_dir = std::string (argv[3]);
00096   boost::filesystem::path outpath (output_dir);
00097   if (!boost::filesystem::exists (outpath))
00098   {
00099     if (!boost::filesystem::create_directories (outpath))
00100     {
00101       std::cerr << "Error creating directory " << output_dir << std::endl;
00102       return (-1);
00103     }
00104     std::cerr << "Creating directory " << output_dir << std::endl;
00105   }
00106 
00107   // Add the PointCloud2 handler
00108   std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl;
00109 
00110   PointCloud cloud_t;
00111   ros::Duration r (0.001);
00112   // Loop over the whole bag file
00113   while (view_it != view.end ())
00114   {
00115     // Handle TF messages first
00116     tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage> ();
00117     if (tf != NULL)
00118     {
00119       tf_broadcaster.sendTransform (tf->transforms);
00120       ros::spinOnce ();
00121       r.sleep ();
00122     }
00123     else
00124     {
00125       // Get the PointCloud2 message
00126       PointCloudConstPtr cloud = view_it->instantiate<PointCloud> ();
00127       if (cloud == NULL)
00128       {
00129         ++view_it;
00130         continue;
00131       }
00132       // Transform it
00133       pcl_ros::transformPointCloud ("/base_link", *cloud, cloud_t, tf_listener);
00134 
00135       std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl;
00136 
00137       std::stringstream ss;
00138       ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
00139       std::cerr << "Data saved to " << ss.str () << std::endl;
00140       pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (),
00141                             Eigen::Quaternionf::Identity (), true);
00142     }
00143     // Increment the iterator
00144     ++view_it;
00145   }
00146 
00147   return (0);
00148 }
00149 /* ]--- */


pcl_ros
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:22:22