scanstudio2carmen.cpp
Go to the documentation of this file.
1 #include <cstdlib>
2 #include <iostream>
3 #include <fstream>
4 #include <sstream>
5 #include <assert.h>
6 #include <gmapping/utils/point.h>
7 
8 #define MAXLINELENGHT (10240)
9 #define MAXREADINGS (10240)
10 
11 using namespace std;
12 using namespace GMapping;
13 
14 int main (int argc, char** argv){
15  if (argc<3){
16  cout << "usage scanstudio2carmen scanfilename carmenfilename" << endl;
17  exit(1);
18  }
19  ifstream is(argv[1]);
20  if (!is){
21  cout << "cannopt open file" << argv[1] << endl;
22  exit(1);
23  }
24 
25  ofstream os(argv[2]);
26 
27  double readings[MAXREADINGS];
28  OrientedPoint pose;
29  int nbeams;
30  while (is){
31  char buf[MAXLINELENGHT];
32  is.getline(buf,MAXLINELENGHT);
33  istringstream st(buf);
34  string token;
35  st>>token;
36  if (token=="RobotPos:"){
37  st >> pose.x >> pose.y >> pose.theta;
38  pose.x/=1000;
39  pose.y/=1000;
40  } else
41  if (token=="NumPoints:"){
42  st >> nbeams;
43  assert(nbeams<MAXREADINGS);
44  } else
45  if (token=="DATA"){
46  int c=0;
47  while (c<nbeams && is){
48  double angle;
49  is >> angle;
50  is >> readings[c];
51  readings[c]/=1000;
52  c++;
53  }
54  if (c==nbeams)
55  os << "FLASER " << nbeams << " ";
56  c=0;
57  while (c<nbeams){
58  os << readings[c] << " ";
59  c++;
60  }
61  os << pose.x << " " << pose.y << " " << pose.theta << "0 0 0 0 pippo 0" << endl;
62  }
63 
64  }
65  os.close();
66 }
c
unsigned int c
Definition: gfs2stream.cpp:41
point.h
main
int main(int argc, char **argv)
Definition: scanstudio2carmen.cpp:14
is
ifstream is(argv[c])
GMapping
Definition: configfile.cpp:34
GMapping::point::y
T y
Definition: point.h:16
GMapping::orientedpoint::theta
A theta
Definition: point.h:60
MAXREADINGS
#define MAXREADINGS
Definition: scanstudio2carmen.cpp:9
GMapping::orientedpoint< double, double >
GMapping::point::x
T x
Definition: point.h:16
MAXLINELENGHT
#define MAXLINELENGHT
Definition: scanstudio2carmen.cpp:8


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51