scanstudio2carmen.cpp
Go to the documentation of this file.
00001 #include <cstdlib>
00002 #include <iostream>
00003 #include <fstream>
00004 #include <sstream>
00005 #include <assert.h>
00006 #include <utils/point.h>
00007 
00008 #define MAXLINELENGHT (10240)
00009 #define MAXREADINGS   (10240)
00010 
00011 using namespace std;
00012 using namespace GMapping;
00013 
00014 int  main (int argc, char** argv){
00015         if (argc<3){
00016                 cout << "usage scanstudio2carmen scanfilename carmenfilename" << endl;
00017                 exit(1);
00018         }
00019         ifstream is(argv[1]);
00020         if (!is){
00021                 cout << "cannopt open file" << argv[1] << endl;
00022                 exit(1);
00023         }
00024         
00025         ofstream os(argv[2]);
00026         
00027         double readings[MAXREADINGS];
00028         OrientedPoint pose;
00029         int nbeams;
00030         while (is){
00031                 char buf[MAXLINELENGHT];
00032                 is.getline(buf,MAXLINELENGHT);
00033                 istringstream st(buf);
00034                 string token;
00035                 st>>token;
00036                 if (token=="RobotPos:"){
00037                         st >> pose.x >> pose.y >> pose.theta;
00038                         pose.x/=1000;
00039                         pose.y/=1000;
00040                 } else
00041                 if (token=="NumPoints:"){
00042                         st >> nbeams;
00043                         assert(nbeams<MAXREADINGS);
00044                 } else
00045                 if (token=="DATA"){
00046                         int c=0;
00047                         while (c<nbeams && is){
00048                                 double angle;
00049                                 is >> angle;
00050                                 is >> readings[c];
00051                                 readings[c]/=1000;
00052                                 c++;
00053                         }
00054                         if (c==nbeams)
00055                                 os << "FLASER " << nbeams << " ";
00056                         c=0;
00057                         while (c<nbeams){
00058                                 os << readings[c] << " ";
00059                                 c++;
00060                         }       
00061                         os << pose.x << " " << pose.y << " " << pose.theta << "0 0 0 0 pippo 0" << endl;
00062                 }
00063                 
00064         }
00065         os.close();
00066 }


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13