utility.cpp
Go to the documentation of this file.
00001 /*
00002  * utility
00003  *
00004  *  Created on: Oct 24, 2011
00005  *      Author: srs-db-dev
00006  */
00007 #include "srs_object_database_msgs/utility.h"
00008 #include <string.h>
00009 #include <ros/ros.h>
00010 #include <vector>
00011 #include <iostream>
00012 #include <fstream>
00013 #include <sstream>
00014 #include <stdint.h>
00015 #include <algorithm>
00016 #include <iterator>
00017 #include <database_interface/postgresql_database.h>
00018 #include <database_interface/db_class.h>
00019 #include <pcl/ros/conversions.h>
00020 #include <pcl/point_cloud.h>
00021 #include <pcl/point_types.h>
00022 #include <pcl/io/pcd_io.h>
00023 #include <pcl/pcl_base.h>
00024 #include <pcl/io/io.h>
00025 #include "srs_object_database_msgs/point_types.h"
00026 #include "srs_object_database_msgs/impl/point_types.cpp"
00027 #include "srs_object_database_msgs/surf.h"
00028 #include <geometry_msgs/Pose.h>
00029 
00030 using namespace std;
00031 
00032 
00033 
00034 void utWriteToFile(char * data, std::string path, long size)
00035 {
00036 
00037   ofstream outfile(path.c_str(), ofstream::binary | ofstream::out);
00038   // write to outfile
00039   outfile.write(data, size);
00040   outfile.close();
00041   //ROS_INFO("Finish writing file \n");
00042 }
00043 
00044 void utDataWrite(vector<uint8_t> data, string path)
00045 {
00046   // write a generic data varible to file as specified in path input variable
00047   //ROS_INFO("Writing data");
00048   if (data.size() <= 0)
00049   {
00050     ROS_INFO("Warning data supplied by service is not valid!");
00051     return;
00052   }
00053   size_t first_obj_size;
00054   vector<uint8_t> c;
00055   c.operator =(data);
00056   first_obj_size = c.size() * sizeof(char);
00057   //ROS_INFO("Object Data %i \n", first_obj_size);
00058   std::stringstream mod_id;
00059   uint8_t dataArray[first_obj_size];
00060   //ROS_INFO("Loading...\n");
00061   for (size_t t = 0; t < first_obj_size; t++)
00062   {
00063     dataArray[t] = (uint8_t)data.at(t);
00064   }
00065   char * buffer;
00066   size_t size;
00067   size = first_obj_size;
00068   buffer = new char[first_obj_size];
00069   //ROS_INFO("Writing data ... \n");
00070   for (uint i = 0; i < first_obj_size; i++)
00071   {
00072     buffer[i] = (char)dataArray[i];
00073   }
00074   utWriteToFile(buffer, path, size);
00075   delete[] buffer;
00076 }
00077 
00078 char * utRetrieveDataFile(std::string path, int * siz)
00079 {
00080 
00081   int size;
00082   //ROS_INFO("Image processing for %s \n", path.c_str());
00083   ifstream file(path.c_str(), ios::in | ios::binary | ios::ate);
00084   //ROS_INFO("Image processing begin \n");
00085   char * memblock;
00086   if (file.is_open())
00087   {
00088     //ROS_INFO(" Processing File \n");
00089     file.seekg(0, ios::end);
00090     size = file.tellg();
00091     file.seekg(0, ios::beg);
00092     //ROS_INFO("File data size %i", size);
00093     memblock = new char[size];
00094     *siz = size;
00095     file.read(memblock, size);
00096     //ROS_INFO("Total block read %i \n", size);
00097     file.close();
00098     return memblock;
00099   } else {
00100     ROS_INFO("Could not retrieve file");
00101   }
00102 
00103   return NULL;
00104 
00105 }
00106 
00107 std::string fromCharPt(char * ch){
00108   if(ch == NULL){
00109     return "";
00110   } else {
00111     string tmp (ch);
00112     return tmp;
00113   }
00114 }
00115 
00116 input utGetInputParameter(std::string path){
00117   ROS_INFO("Data from file %s",path.c_str());
00118   string line = "";
00119 
00120   input input;
00121   char* pch;
00122 
00123   ifstream source(path.c_str());
00124   if (source.is_open())
00125   {
00126     ROS_INFO("Loading input for test client");
00127     char * tokenString;
00128     string str;
00129     while (getline(source, line))
00130     {
00131       pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00132       strcpy(pch, line.c_str());
00133       tokenString = strtok(pch, " ");
00134       std::string param=tokenString;
00135       string tmp;
00136       if (param=="id"){
00137         tokenString = strtok(NULL, " ");
00138         ROS_INFO("Line id ");
00139         input.id=fromCharPt(tokenString);
00140 
00141       }
00142       if (param=="grasp"){
00143         ROS_INFO("Line grasp ");
00144         tokenString = strtok(NULL, " ");
00145         input.grasp=fromCharPt(tokenString);
00146       }
00147       if (param=="mesh"){
00148         ROS_INFO("Line Mesh ");
00149         tokenString = strtok(NULL, " ");
00150         input.mesh=fromCharPt(tokenString);
00151       }
00152       if (param=="image"){
00153         ROS_INFO("Line Image ");
00154         tokenString = strtok(NULL, " ");
00155         input.image=fromCharPt(tokenString);
00156       }
00157       if (param=="image-type"){
00158         ROS_INFO("Line Image Type ");
00159         tokenString = strtok(NULL, " ");
00160         input.image_type=fromCharPt(tokenString);
00161       }
00162       if (param=="pcl"){
00163         ROS_INFO("Line pcl ");
00164         tokenString = strtok(NULL, " ");
00165         input.pcl=fromCharPt(tokenString);
00166       }
00167       if (param=="surf"){
00168         ROS_INFO("Line Surf ");
00169         tokenString = strtok(NULL, " ");
00170         input.surf=fromCharPt(tokenString);
00171 
00172       }
00173       if (param=="typeMesh"){
00174         ROS_INFO("Line TypeMesh  ");
00175         tokenString = strtok(NULL, " ");
00176         input.mType=fromCharPt(tokenString);
00177       }
00178       if (param=="name"){
00179         ROS_INFO("Line Name ");
00180         tokenString = strtok(NULL, " ");
00181         input.name=fromCharPt(tokenString);
00182             //line.substr(param.size()+1,(line.length()-param.size())-1);
00183       }
00184       if (param=="category"){
00185         ROS_INFO("Line category ");
00186         tokenString = strtok(NULL, " ");
00187         input.category=fromCharPt(tokenString);
00188         //input.category=line.substr(param.size()+1,(line.length()-param.size())-1);
00189       }
00190       if (param=="description"){
00191         ROS_INFO("Line Description ");
00192         tokenString = strtok(NULL, " ");
00193         input.description=fromCharPt(tokenString);
00194             //line.substr(param.size()+1,(line.length()-param.size())-1);
00195       }
00196       if (param=="x-size"){
00197         ROS_INFO("Line x ");
00198         tokenString = strtok(NULL, " ");
00199         input.x=line.substr(param.size()+1,(line.length()-param.size())-1);
00200         ROS_INFO("Line x size %f",atof(input.x.c_str()));
00201       }
00202       if (param=="y-size"){
00203         ROS_INFO("Line y size ");
00204         tokenString = strtok(NULL, " ");
00205         input.y=fromCharPt(tokenString);
00206             //line.substr(param.size()+1,(line.length()-param.size())-1);
00207         ROS_INFO("Line y size %f",atof(input.y.c_str()));
00208       }
00209       if (param=="z-size"){
00210         ROS_INFO("Line z size ");
00211         tokenString = strtok(NULL, " ");
00212         input.z=fromCharPt(tokenString);
00213         //line.substr(param.size()+1,(line.length()-param.size())-1);
00214         ROS_INFO("Line z size %f",atof(input.z.c_str()));
00215       }
00216       if (param=="basic-shape"){
00217         ROS_INFO("Line Basic Shape ");
00218         tokenString = strtok(NULL, " ");
00219         input.basic_shape=fromCharPt(tokenString);
00220         //line.substr(param.size()+1,(line.length()-param.size())-1);
00221       }
00222       if (param=="end"){
00223         ROS_INFO("data loaded ");
00224         return input;
00225       }
00226       if (param=="graspable"){
00227         ROS_INFO("graspable loaded ");
00228         tokenString = strtok(NULL, " ");
00229         input.graspable=fromCharPt(tokenString);
00230         //line.substr(param.size()+1,(line.length()-param.size())-1);;
00231       }
00232       if (param=="urdf"){
00233         ROS_INFO("urdf loaded ");
00234         tokenString = strtok(NULL, " ");
00235         input.urdf=fromCharPt(tokenString);
00236         ROS_INFO("urdf %s ",input.urdf.c_str());
00237         //line.substr(param.size()+1,(line.length()-param.size())-1);;
00238       }
00239     }
00240 
00241     return input;
00242   }else{
00243     ROS_INFO("Cannot load Input File");
00244     return input;
00245   }
00246 
00247 }
00248 
00249 database_interface::PostgresqlDatabase utLoadSettings(std::string dbInstance, std::string dbPort, std::string dbUser,
00250                                                       std::string dbPwd, std::string dbName)
00251 {
00252 
00253   database_interface::PostgresqlDatabase database(dbInstance, dbPort, dbUser, dbPwd, dbName);
00254   return database;
00255 
00256 }
00257 
00258 vector<char> utConvertToVector(char* inputData)
00259 {
00260   int size = sizeof(inputData) / sizeof(char);
00261   vector<char> vChar;
00262   for (int i = 0; i < size; i++)
00263   {
00264 
00265     vChar.push_back(inputData[i]);
00266   }
00267   return vChar;
00268 }
00269 
00270 char* utConvertToChar(vector<char> inputData)
00271 {
00272   int size = inputData.size();
00273   char* vChar = new char[size];
00274   for (int i = 0; i < size; i++)
00275   {
00276     vChar[i] = inputData.at(i);
00277   }
00278   return vChar;
00279 }
00280 
00281 void utLoadFeaturePointFromFile(std::string path,srs_object_database_msgs::surf::Ptr & surf)
00282 {
00283   ROS_INFO("Data from file %s",path.c_str());
00284   pcl::PointCloud<PointSURF>::Ptr cloud(new pcl::PointCloud<PointSURF>);
00285   pcl::PointCloud<PointXYZ>::Ptr bBox(new pcl::PointCloud<PointXYZ>);
00286   pcl::PointCloud<PointSURF> pcl;
00287   PointSURF pSurf;
00288   pcl::PointCloud<PointXYZ> boundingBox;
00289 
00290   pcl.height = 0;
00291   pcl.width = 1;
00292   boundingBox.height=2;
00293   boundingBox.width=1;
00294   boundingBox.points.resize(2);
00295   // second point line
00296   uint32_t rRGB = 0;
00297   uint32_t gRGB = 0;
00298   uint32_t bRGB = 0;
00299   string line = "";
00300   long totalLine = 0;
00301   long totalPoints = 0;
00302   long numFeatures = 0;
00303   long points = 0;
00304   bool testLine = false;
00305   char* pch;
00306   // the first four lines represent the general settings of the points
00307   int offset = 4;
00308 
00309   ifstream source(path.c_str());
00310   if (source.is_open())
00311   {
00312 
00313     ROS_INFO("Loading file data to pcl Begin");
00314     char * tokenString;
00315     while (getline(source, line))
00316     {
00317       if (line.length() == 2)
00318       {
00319         string cut;
00320         //ROS_INFO("Testing lenght ");
00321 
00322         if (line.substr(0, 2) == "-1")
00323         {
00324           testLine = true;
00325           //ROS_INFO("found -1");
00326         }
00327         else
00328         {
00329           testLine = false;
00330           //ROS_INFO("found short line but not -1 instead %s",line.substr(0,2).c_str());
00331         }
00332       }
00333       else
00334       {
00335         testLine = false;
00336       }
00337       if (!(testLine))
00338       {
00339         totalLine++;
00340         //ROS_INFO("Points %i",totalPoints);
00341         if (totalLine > offset)
00342         {
00343           // New Point processing after the first settings lines
00344           if (points + 1 <= numFeatures)
00345           {
00346             totalPoints++;
00347             int mod;
00348             mod = (totalPoints) % 3;
00349             if (mod == 1)
00350             {
00351               // this line contains id and confidence
00352               // first token is id, while second is confidence
00353               pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00354               strcpy(pch, line.c_str());
00355               tokenString = strtok(pch, " ");
00356               pSurf.id=atol(tokenString);
00357               //pcl.points[points+1].id = atol(tokenString);
00358               tokenString = strtok(NULL, " ");
00359               pSurf.confidence=atof(tokenString);
00360               //pcl.points[points+1].confidence = atof(tokenString);
00361 
00362             }
00363             if (mod == 2)
00364             {
00365               pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00366               strcpy(pch, line.c_str());
00367 
00368               tokenString = strtok(pch, " ");
00369               pSurf.x=atof(tokenString);
00370               //pcl.points[points+1].x = atof(tokenString);
00371               tokenString = strtok(NULL, " ");
00372               pSurf.y=atof(tokenString);
00373               //pcl.points[points+1].y = atof(tokenString);
00374               tokenString = strtok(NULL, " ");
00375               pSurf.z=atof(tokenString);
00376               pSurf.rgbr=rRGB;
00377               pSurf.rgbg=gRGB;
00378               pSurf.rgbb=bRGB;
00379             }
00380             if (mod == 0)
00381             {
00382               pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00383               strcpy(pch, line.c_str());
00384               tokenString = strtok(pch, " ");
00385               int i = 0;
00386               pSurf.descriptor[0]=atof(tokenString);
00387               tokenString = strtok(NULL, " ");
00388               while ((i < 63) & (tokenString != NULL))
00389               {
00390                 pSurf.descriptor[i+1]=atof(tokenString);
00391                 tokenString = strtok(NULL, " ");
00392                 i++;
00393               }
00394               points++;
00395               pcl.points.push_back(pSurf);
00396             }
00397           }else{
00398 
00399             pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00400             strcpy(pch, line.c_str());
00401             float temp[6];
00402             tokenString = strtok(pch, " ");
00403             int i = 1;
00404             temp[0]=atof(tokenString);
00405             tokenString = strtok(NULL, " ");
00406             // this line contains descriptions
00407             while ((tokenString != NULL))
00408             {
00409               temp[i]=atof(tokenString);
00410               tokenString = strtok(NULL, " ");
00411               i++;
00412             }
00413             if(points + 1 == numFeatures+1){
00414 
00415               // this is the first line after the points -> bounding box
00416               boundingBox.points[0].x=temp[0];
00417               boundingBox.points[0].y=temp[1];
00418               boundingBox.points[0].z=temp[2];
00419               boundingBox.points[1].x=temp[3];
00420               boundingBox.points[1].y=temp[4];
00421               boundingBox.points[1].z=temp[5];
00422 
00423             } else if (points + 1 == numFeatures+2){
00424               // this is the second line after the points -> coord frame
00425               surf->coord_frame.position.x=temp[0];
00426               surf->coord_frame.position.y=temp[1];
00427               surf->coord_frame.position.z=temp[2];
00428               surf->coord_frame.orientation.x=temp[3];
00429               surf->coord_frame.orientation.y=temp[4];
00430               surf->coord_frame.orientation.z=temp[5];
00431             }else {
00432               ROS_INFO("Wrong information in the viewpoint");
00433             }
00434             points++;
00435           }
00436 
00437         }
00438         else
00439         {
00440           // We are processing generic information for all points
00441           if (totalLine == 1)
00442           {
00443             // first four lines contains general information about PCL
00444             // tokenize the string using the separator
00445             pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00446             strcpy(pch, line.c_str());
00447             tokenString = strtok(pch, " ");
00448             // first line contains number of features
00449             numFeatures = atol(tokenString);
00450             //ROS_INFO("Total feature in file... %ld", numFeatures);
00451             pcl.height = numFeatures;
00452             ROS_INFO("Declared features in file %i",numFeatures);
00453             //pcl.points.resize(numFeatures);
00454 
00455           }
00456           if (totalLine == 2)
00457           {
00458             // first line is red
00459             pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00460             strcpy(pch, line.c_str());
00461             tokenString = strtok(pch, " ");
00462             rRGB = atoi(tokenString);
00463             //ROS_INFO("RGB red...%i", rRGB);
00464             // second line is green
00465             tokenString = strtok(NULL, " ");
00466             gRGB = atoi(tokenString);
00467             //ROS_INFO("RGB green...%i", gRGB);
00468             // third line is blue
00469             tokenString = strtok(NULL, " ");
00470             bRGB = atoi(tokenString);
00471             //ROS_INFO("RGB blue...%i", bRGB);
00472 
00473           }
00474           if (totalLine == 3)
00475           {
00476             // number of value in description should be always 64
00477             pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00478             strcpy(pch, line.c_str());
00479             tokenString = strtok(pch, " ");
00480             //int description = atoi(tokenString);
00481             //ROS_INFO("Description...%i", description);
00482           }
00483           if (totalLine == 4)
00484           {
00485             // number of connection dimension
00486             pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00487             strcpy(pch, line.c_str());
00488             tokenString = strtok(pch, " ");
00489             //int connection = atoi(tokenString);
00490             //ROS_INFO("Connection...%i", connection);
00491             //ROS_INFO("################################################################");
00492           }
00493         }
00494       }
00495     }
00496   }
00497   else
00498   {
00499     ROS_INFO("Cannot open file data to load FPt");
00500   }
00501 
00502   ROS_INFO("PCL loaded with: %i size", pcl.size());
00503   ROS_INFO("PCL loaded with: %i features", pcl.points.size());
00504 
00505   *cloud=pcl;
00506   *bBox=boundingBox;
00507   sensor_msgs::PointCloud2 fpt;
00508   sensor_msgs::PointCloud2 bounding_box;
00509   pcl::toROSMsg(*cloud,fpt);
00510   pcl::toROSMsg(*bBox,bounding_box);
00511   surf->surf=fpt;
00512   surf->bounding_box=bounding_box;
00513   ROS_INFO("Retrieval executed");
00514 
00515 }
00516 
00517 void utLoadCloudPointFromPLYFile(std::string path, srs_object_database_msgs::pcl::Ptr & pcl){
00518   ROS_INFO("Data from file %s",path.c_str());
00519   //srs_object_database_msgs::pcl::Ptr pcl(new srs_object_database_msgs::pcl);
00520   pcl::PointCloud<PointPLY>::Ptr plyPt(new pcl::PointCloud<PointPLY>);
00521   pcl::PointCloud<PointPLY> ply;
00522   ply.width=1;
00523   ply.height = 0;
00524 
00525   // second point line
00526   string line = "";
00527   long totalLine = 0;
00528   //long totalPoints = 0;
00529   //long numFeatures = 0;
00530   long points = 0;
00531   long numP=0;
00532   //bool testLine = false;
00533   char* pch;
00534   // the first four lines represent the general settings of the points
00535   int offset = 10;
00536   ifstream source(path.c_str());
00537   if (source.is_open())
00538   {
00539     ROS_INFO("Loading file data to pcl Begin");
00540     char * tokenString;
00541     while (getline(source, line))
00542     {
00543         totalLine++;
00544         //ROS_INFO("Points %i",totalPoints);
00545         if (totalLine > offset)
00546         {
00547           points++;
00548           float temp[6];
00549           pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00550           strcpy(pch, line.c_str());
00551           tokenString = strtok(pch, " ");
00552           int i = 1;
00553           temp[0]=atof(tokenString);
00554           tokenString = strtok(NULL, " ");
00555           while ((tokenString != NULL))
00556           {
00557             temp[i]=atof(tokenString);
00558             tokenString = strtok(NULL, " ");
00559             i++;
00560           }
00561           ply.points[i].x=temp[0];
00562           ply.points[i].y=temp[1];
00563           ply.points[i].z=temp[2];
00564           ply.points[i].rgbr=temp[3];
00565           ply.points[i].rgbb=temp[4];
00566           ply.points[i].rgbg=temp[5];
00567 
00568          }else{
00569 
00570              if(totalLine == 3){
00571                pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00572                strcpy(pch, line.c_str());
00573                tokenString = strtok(pch, " ");
00574                tokenString = strtok(NULL, " ");
00575                tokenString = strtok(NULL, " ");
00576                //the third element is the number of points
00577                ROS_INFO("Resized with points %ld",atol(tokenString));
00578                ply.points.resize(atol(tokenString));
00579                numP=atol(tokenString);
00580                ply.height = numP;
00581              }
00582           }
00583         }
00584       }
00585   else
00586   {
00587     ROS_INFO("Cannot open file data to load Pcl");
00588   }
00589   ROS_INFO("PCL loaded with: %i size", ply.points.size());
00590   *plyPt=ply;
00591   sensor_msgs::PointCloud2 pcl2Msg;
00592   pcl::toROSMsg(*plyPt,pcl2Msg);
00593   pcl->pcl=pcl2Msg;
00594   ROS_INFO("Retrieval executed");
00595 
00596 }
00597 
00598 arm_navigation_msgs::Shape utLoadMeshFromFile(std::string path){
00599   arm_navigation_msgs::Shape mesh;
00600   arm_navigation_msgs::Shape::Ptr meshPt (new arm_navigation_msgs::Shape );
00601   geometry_msgs::Point pt;
00602   std::cerr << path.c_str() << endl;
00603 
00604     // 3 type means that this shape is a mesh
00605   mesh.type = 3;
00606   bool startCoord=false;
00607   bool startIndex=false;
00608   char * pch;
00609   string line;
00610   ifstream source(path.c_str());
00611   if(source.is_open()){
00612     char * tokenString;
00613     while (getline(source, line))
00614     {
00615 
00616 
00617       if ((line.find("]")!=string::npos) && (startCoord) ){
00618 
00619         startCoord=false;
00620       } else if ((line.find("]")==string::npos) && (startCoord)){
00621           pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00622           strcpy(pch, line.c_str());
00623           tokenString = strtok(pch, " ");
00624           pt.x = strtod(tokenString,NULL);
00625           tokenString = strtok(NULL, " ");
00626           pt.y = strtod(tokenString,NULL);
00627           tokenString = strtok(NULL, " ");
00628           pt.z = strtod(tokenString,NULL);
00629           mesh.vertices.push_back(pt);
00630       }
00631 
00632 
00633       if ((line.find("]")!=string::npos) && (startIndex) ){
00634 
00635         startIndex=false;
00636       }else if ((line.find("]")==string::npos) && (startIndex)){
00637 
00638         pch = (char*)malloc(sizeof(char) * (line.length() + 1));
00639         strcpy(pch, line.c_str());
00640         tokenString = strtok(pch, ",");
00641 
00642         mesh.triangles.push_back(atoi(tokenString));
00643 
00644         tokenString = strtok(NULL, ",");
00645 
00646         mesh.triangles.push_back(atoi(tokenString));
00647 
00648         tokenString = strtok(NULL, ",");
00649 
00650         mesh.triangles.push_back(atoi(tokenString));
00651 
00652       }
00653       if (line.find("Coordinate3")!=string::npos){
00654 
00655         getline(source, line);
00656         if (line.find("point")!=string::npos){
00657           startCoord=true;
00658         }
00659       }
00660 
00661       if (line.find("IndexedFaceSet")!=string::npos){
00662         getline(source, line);
00663          if (line.find("coordIndex")!=string::npos){
00664 
00665           startIndex=true;
00666          }
00667        }
00668     }
00669 
00670 
00671   } else {
00672     ROS_INFO("Cannot load file mesh");
00673   }
00674 
00675   return mesh;
00676 }
00677 


srs_object_database_msgs
Author(s): Georg Arbeiter
autogenerated on Sun Jan 5 2014 11:36:26