X-lanni
67 lines
1.2 KiB
C++
67 lines
1.2 KiB
C++
#include <cstdlib>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
#include <assert.h>
|
|
#include <utils/point.h>
|
|
|
|
#define MAXLINELENGHT (10240)
|
|
#define MAXREADINGS (10240)
|
|
|
|
using namespace std;
|
|
using namespace GMapping;
|
|
|
|
int main (int argc, char** argv){
|
|
if (argc<3){
|
|
cout << "usage scanstudio2carmen scanfilename carmenfilename" << endl;
|
|
exit(1);
|
|
}
|
|
ifstream is(argv[1]);
|
|
if (!is){
|
|
cout << "cannopt open file" << argv[1] << endl;
|
|
exit(1);
|
|
}
|
|
|
|
ofstream os(argv[2]);
|
|
|
|
double readings[MAXREADINGS];
|
|
OrientedPoint pose;
|
|
int nbeams;
|
|
while (is){
|
|
char buf[MAXLINELENGHT];
|
|
is.getline(buf,MAXLINELENGHT);
|
|
istringstream st(buf);
|
|
string token;
|
|
st>>token;
|
|
if (token=="RobotPos:"){
|
|
st >> pose.x >> pose.y >> pose.theta;
|
|
pose.x/=1000;
|
|
pose.y/=1000;
|
|
} else
|
|
if (token=="NumPoints:"){
|
|
st >> nbeams;
|
|
assert(nbeams<MAXREADINGS);
|
|
} else
|
|
if (token=="DATA"){
|
|
int c=0;
|
|
while (c<nbeams && is){
|
|
double angle;
|
|
is >> angle;
|
|
is >> readings[c];
|
|
readings[c]/=1000;
|
|
c++;
|
|
}
|
|
if (c==nbeams)
|
|
os << "FLASER " << nbeams << " ";
|
|
c=0;
|
|
while (c<nbeams){
|
|
os << readings[c] << " ";
|
|
c++;
|
|
}
|
|
os << pose.x << " " << pose.y << " " << pose.theta << "0 0 0 0 pippo 0" << endl;
|
|
}
|
|
|
|
}
|
|
os.close();
|
|
}
|