pointcloud_delimiter.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009 Dejan Pangercic <pangercic -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: pointcloud_delimiter.cpp 161 2009-09-06 15:18:34Z dejanpan $
00028  *
00029  */
00030 
00042 // ROS core
00043 #include <ros/ros.h>
00044 // ROS messages
00045 #include <sensor_msgs/PointCloud.h>
00046 
00047 #include <fstream>
00048 #include <boost/algorithm/string.hpp>
00049 #include <boost/thread/mutex.hpp>
00050 //for get_log_files()
00051 #include <boost/filesystem/operations.hpp>
00052 #include <boost/filesystem/fstream.hpp>
00053 #include "boost/filesystem.hpp"
00054 //for savePCDFile ()
00055 #include <point_cloud_mapping/cloud_io.h>
00056 
00057 using namespace std;
00058 using namespace ros;
00059 using namespace sensor_msgs;
00060 using namespace cloud_io;
00061 using namespace boost::filesystem; 
00062 
00063 struct delimiter
00064 {
00065   double min_x, max_x, min_y, max_y, min_z, max_z;
00066 };
00067 
00068 class PointcloudDelimiter
00069 {
00070 protected:
00071   NodeHandle nh_;
00072   
00073 public:
00074   // ROS messages
00075   PointCloud cloud_in_, cloud_out_;
00076   delimiter  del_;
00077   string dir_;
00078   vector<string> file_list_;
00079   int normalize_;
00080   double norm_;
00081   int delimit_david_;
00082 
00084   //Constructor
00086   PointcloudDelimiter()
00087   {
00088     nh_.param ("~dir", dir_, string(""));     //dir to fetch .pcd files from
00089     if(dir_ == "")
00090       ROS_ERROR("You must specify path to .pcd files");
00091     //get the list of .log files
00092     get_log_files(dir_, file_list_, ".pcd");
00093   
00094     nh_.param ("~min_x", del_.min_x, 0.0);     // minimum x to be considered
00095     nh_.param ("~max_x", del_.max_x, 0.0);   // maximum x to be considered
00096       
00097     nh_.param ("~min_y", del_.min_y, 0.0);     // minimum y to be considered
00098     nh_.param ("~max_y", del_.max_y, 0.0);   // maximum y to be considered
00099 
00100     nh_.param ("~min_z", del_.min_z, 0.0);     // minimum z to be considered
00101     nh_.param ("~max_z", del_.max_z, 0.0);   // maximum z to be considered
00102     nh_.param ("~normalize", normalize_, 0);   // shall we normalize data?
00103     nh_.param ("~norm", norm_, 1.0);   // normalization coefficient
00104     
00105     nh_.param ("~delimit_david", delimit_david_, 0);   // delimiting david?
00106 
00107     if(!normalize_ && !delimit_david_)
00108       {
00109         cloud_out_.header.frame_id = "base_link";
00110         cloud_out_.channels.resize (7);
00111         cloud_out_.channels[0].name = "intensity";
00112         cloud_out_.channels[1].name = "distance";
00113         cloud_out_.channels[2].name = "sid";
00114         cloud_out_.channels[3].name = "pid";
00115         cloud_out_.channels[4].name = "vx";
00116         cloud_out_.channels[5].name = "vy";
00117         cloud_out_.channels[6].name = "vz";
00118       }
00119   }
00120 
00122   //Destructor
00124   virtual ~PointcloudDelimiter () { }
00125 
00127   // Get a list of pcd files
00129   void get_log_files ( const path & directory, vector <string> &file_list, string suffix=".pcd", bool recurse_into_subdirs = false )
00130   {
00131     if( exists( directory ) )
00132       {
00133         directory_iterator end ;
00134         for( directory_iterator iter(directory) ; iter != end ; ++iter )
00135           if ( is_directory( *iter ) )
00136             {
00137               ROS_WARN("Directory %s", iter->string().c_str());
00138               //if( recurse_into_subdirs ) get_log_files(*iter) ;
00139             }
00140           else 
00141             {
00142               int len = iter->string().length();
00143               int npos =  iter->string().rfind(suffix);
00144               if((len - npos) == suffix.length())
00145                 file_list.push_back(iter->string());
00146             }
00147       }
00148   }
00149     
00150 
00152   // Get a list of log files
00154   bool
00155   spin ()
00156   {
00157     int nr_points = 0;
00158     unsigned int progress = 0;
00159     ROS_INFO("minima/maxima x, y, z : %f, %f, %f, %f, %f, %f", del_.min_x, del_.max_x, del_.min_y, del_.max_y, del_.min_z, del_.max_z);
00160     ROS_INFO("file_list_ size %d", file_list_.size());
00161     while (nh_.ok ())
00162       {
00163         for (unsigned int i = 0; i < file_list_.size(); i++)
00164           {
00165             ROS_INFO("Loading file %s", file_list_[i].c_str());
00166             loadPCDFile(file_list_[i].c_str(), cloud_in_);
00167             ROS_INFO("cloud_in_ with nr_points %d", cloud_in_.points.size());
00168             //reset sizes
00169             nr_points = 0, progress = 0;
00170             cloud_out_.points.resize(0);
00171             if(!normalize_ && !delimit_david_)
00172               {
00173                 for (unsigned int d = 0; d < cloud_in_.channels.size (); d++)
00174                   cloud_out_.channels[d].values.resize (0);
00175                 cloud_out_.header =  cloud_in_.header;
00176               }
00177             for (unsigned int j = 0; j < cloud_in_.points.size(); j++)
00178               {
00179                 if(!normalize_ && !delimit_david_)
00180                   {
00181                     if ((del_.min_x <= cloud_in_.points[j].x) && (cloud_in_.points[j].x <= del_.max_x) &&
00182                         (del_.min_y <= cloud_in_.points[j].y) && (cloud_in_.points[j].y <= del_.max_y) &&
00183                         (del_.min_z <= cloud_in_.points[j].z) && (cloud_in_.points[j].z <= del_.max_z))
00184                       {
00185                         //resize
00186                         cloud_out_.points.resize(nr_points + 1);
00187                         for (unsigned int d = 0; d < cloud_in_.channels.size (); d++)
00188                           cloud_out_.channels[d].values.resize (nr_points + 1);
00189       
00190                         //fill with values
00191                         cloud_out_.points[nr_points].x =  cloud_in_.points[j].x;
00192                         cloud_out_.points[nr_points].y =  cloud_in_.points[j].y;
00193                         cloud_out_.points[nr_points].z =  cloud_in_.points[j].z;
00194                         // Save the rest of the values
00195                         cloud_out_.channels[0].values[nr_points] =  cloud_in_.channels[0].values[j];
00196                         cloud_out_.channels[1].values[nr_points] =  cloud_in_.channels[1].values[j];
00197                         cloud_out_.channels[2].values[nr_points] =  cloud_in_.channels[2].values[j];
00198                         cloud_out_.channels[3].values[nr_points] =  cloud_in_.channels[3].values[j];
00199                         cloud_out_.channels[4].values[nr_points] =  cloud_in_.channels[4].values[j];
00200                         cloud_out_.channels[5].values[nr_points] =  cloud_in_.channels[5].values[j];
00201                         cloud_out_.channels[6].values[nr_points] =  cloud_in_.channels[6].values[j];
00202                         nr_points++;
00203                       }
00204                   }
00205       
00206                 if(delimit_david_)
00207                   {
00208                     //ROS_WARN("DAVID WARN");
00209                     if ((del_.min_x <= cloud_in_.points[j].x) && (cloud_in_.points[j].x <= del_.max_x) &&
00210                         (del_.min_y <= cloud_in_.points[j].y) && (cloud_in_.points[j].y <= del_.max_y) &&
00211                         (del_.min_z <= cloud_in_.points[j].z) && (cloud_in_.points[j].z <= del_.max_z))
00212                       {        
00213                         cloud_out_.points.resize(nr_points + 1);
00214                         //fill with values
00215                         cloud_out_.points[nr_points].x =  cloud_in_.points[j].x;
00216                         cloud_out_.points[nr_points].y =  cloud_in_.points[j].y;
00217                         cloud_out_.points[nr_points].z =  cloud_in_.points[j].z;        
00218                         nr_points++;
00219                       }      
00220                   }
00221       
00222                 //if data need to be normalized (like the ones from david)
00223                 if(normalize_)
00224                   {
00225                     cloud_out_.points.resize(nr_points + 1);
00226                     cloud_out_.points[nr_points].x =  cloud_in_.points[j].x/norm_;
00227                     cloud_out_.points[nr_points].y =  cloud_in_.points[j].y/norm_;
00228                     cloud_out_.points[nr_points].z =  cloud_in_.points[j].z/norm_;
00229                     nr_points++;
00230                   }
00231 
00232                 if (j == int(cloud_in_.points.size() * (float(progress + 1)/10)) )
00233                   {
00234                     ROS_INFO("%d percent", (progress+1) * 10);
00235                     progress++;
00236                   }
00237               }
00238             if(cloud_out_.points.size() != 0)    
00239               {
00240                 int str_len = file_list_[i].length();
00241                 string pcd_del = file_list_[i].erase((str_len - 4), 4);
00242                 if(!normalize_ || delimit_david_)
00243                   pcd_del += ".delimited.pcd";
00244                 else 
00245                   pcd_del += ".normalized.pcd";
00246                 ROS_INFO("Saving file %s with nr. of points %d", pcd_del.c_str(), nr_points);
00247                 savePCDFile (pcd_del.c_str(), cloud_out_, false);
00248                 pcd_del.clear();
00249               }
00250           }
00251         ros::spinOnce ();
00252         break;
00253       }
00254     return (true);
00255   }
00256   
00257 };
00258 
00259 /* ---[ */
00260 int
00261 main (int argc, char** argv)
00262 {
00263   init (argc, argv, "actarray_cloud_out_assembler");
00264 
00265   PointcloudDelimiter p;
00266   p.spin ();
00267 
00268   return (0);
00269 }
00270 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


player_log_actarray
Author(s): Radu Bogdan Rusu
autogenerated on Thu May 23 2013 18:58:38