add slam_gmapping

slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp

@@ -0,0 +1,66 @@
+#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();
+}