add slam_gmapping

slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp

@@ -0,0 +1,58 @@
+#include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <log/carmenconfiguration.h>
+#include <log/sensorlog.h>
+
+
+using namespace std;
+using namespace GMapping;
+
+int main(int argc, char ** argv){
+	if (argc<2){
+		cerr << "usage "<<argv[0]<<" <filename> <outfilename>" << endl;
+		cerr << "or "<<argv[0]<<" <filename> for standard output" << endl;
+		exit (-1);
+	}
+	ifstream is(argv[1]);
+	if (! is){
+		cerr << "no file " << argv[1] << " found" << endl;
+		exit (-1);
+	}
+	ostream *os;
+	if (argc<3) 
+		os=&cout;
+	else{
+		os=new ofstream(argv[2]);
+		if (! os){
+			cerr << "no file " << argv[1] << " found" << endl;
+			exit (-1);
+		}
+	}
+	CarmenConfiguration conf;
+	conf.load(is);
+	
+	SensorMap m=conf.computeSensorMap();
+	
+	//for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++)
+	//	cout << it->first << " " << it->second->getName() << endl;
+	
+	SensorLog log(m);
+	is.close();
+	
+	ifstream ls(argv[1]);
+	log.load(ls);
+	ls.close();
+	cerr << "log size" << log.size() << endl;
+	for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){
+		RangeReading* rr=dynamic_cast<RangeReading*>(*it);
+		if (rr){
+			*os << rr->getSensor()->getName() << " ";
+			*os << rr->size()<< " ";
+			for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){
+				*os << (*it)*0.001 << " ";
+			}
+			*os<< rr->getPose().x*0.001 << " " << rr->getPose().y*0.001 << " " << rr->getPose().theta << endl;
+		}
+	}
+}