00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00042
00043 #include <ros/ros.h>
00044
00045 #include <sensor_msgs/PointCloud.h>
00046
00047 #include <fstream>
00048 #include <boost/algorithm/string.hpp>
00049 #include <boost/thread/mutex.hpp>
00050
00051 #include <boost/filesystem/operations.hpp>
00052 #include <boost/filesystem/fstream.hpp>
00053 #include "boost/filesystem.hpp"
00054
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
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
00086 PointcloudDelimiter()
00087 {
00088 nh_.param ("~dir", dir_, string(""));
00089 if(dir_ == "")
00090 ROS_ERROR("You must specify path to .pcd files");
00091
00092 get_log_files(dir_, file_list_, ".pcd");
00093
00094 nh_.param ("~min_x", del_.min_x, 0.0);
00095 nh_.param ("~max_x", del_.max_x, 0.0);
00096
00097 nh_.param ("~min_y", del_.min_y, 0.0);
00098 nh_.param ("~max_y", del_.max_y, 0.0);
00099
00100 nh_.param ("~min_z", del_.min_z, 0.0);
00101 nh_.param ("~max_z", del_.max_z, 0.0);
00102 nh_.param ("~normalize", normalize_, 0);
00103 nh_.param ("~norm", norm_, 1.0);
00104
00105 nh_.param ("~delimit_david", delimit_david_, 0);
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
00124 virtual ~PointcloudDelimiter () { }
00125
00127
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
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
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
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
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
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
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
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
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
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
