add slam_gmapping

slam_gmapping/openslam_gmapping/log/Makefile

@@ -0,0 +1,9 @@
+OBJS= configuration.o carmenconfiguration.o sensorlog.o sensorstream.o
+APPS= log_test log_plot scanstudio2carmen rdk2carmen
+
+LDFLAGS+=  -lsensor_range -lsensor_odometry -lsensor_base 
+CPPFLAGS+= -I../sensor 
+
+-include ../global.mk
+-include ../build_tools/Makefile.generic-shared-object
+

slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp

@@ -0,0 +1,463 @@
+#include <cstdlib>
+#include "carmenconfiguration.h"
+#include <iostream>
+#include <sstream>
+#include <assert.h>
+#include <sys/types.h>
+#include <sensor_odometry/odometrysensor.h>
+#include <sensor_range/rangesensor.h>
+
+
+#define LINEBUFFER_SIZE 10000
+
+namespace GMapping {
+
+using namespace std;
+
+istream& CarmenConfiguration::load(istream& is){
+	clear();
+	char buf[LINEBUFFER_SIZE];
+	bool laseron=false;
+	bool rlaseron=false;
+	bool rlaser1=false;
+	bool rlaser2=false;
+
+	string beams;
+	string rbeams;
+
+	while (is){
+		is.getline(buf, LINEBUFFER_SIZE);
+		istringstream lis(buf);
+		
+		string qualifier;
+		string name;
+		
+		if (lis) 
+			lis >> qualifier; 
+		else 
+			continue;
+		//this is a workaround for carmen log files
+		//the number lf laser beams should be specofoed in the config
+		//part of the log
+		if (qualifier=="FLASER"){
+			laseron=true;
+			lis >> beams;
+		}
+		if (qualifier=="RLASER"){
+			rlaseron=true;
+			lis >> rbeams;
+		}
+		if (qualifier=="ROBOTLASER1"){
+			string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
+			lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> beams;
+			rlaser1=true;
+		}
+		if (qualifier=="ROBOTLASER2"){
+			string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
+			lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> rbeams;
+			rlaser2=true;
+		}
+		if (qualifier!="PARAM") 
+			continue;
+		if (lis) 
+			lis >> name; 
+		else continue;
+		
+
+		vector<string> v;
+		while (lis){
+			string cparm;
+			lis >> cparm;
+			if (lis)
+				v.push_back(cparm);
+		}
+		insert(make_pair(name, v));
+	}
+	if (laseron || rlaser1){
+		vector<string> v;
+		v.push_back(beams);
+		insert(make_pair("laser_beams", v));
+		cerr << "FRONT LASER BEAMS FROM LOG: " << beams << endl;
+		v.clear();
+		v.push_back("on");
+		insert(make_pair("robot_use_laser", v));
+	}
+	if (rlaseron || rlaser2){
+		vector<string> v;
+		v.push_back(rbeams);
+		insert(make_pair("rear_laser_beams", v));
+		cerr << "REAR LASER BEAMS FROM LOG: " << beams << endl;
+		v.clear();
+		v.push_back("on");
+		insert(make_pair("robot_use_rear_laser", v));
+	}
+	return is;
+}
+
+SensorMap CarmenConfiguration::computeSensorMap() const{
+	//this boring stuff is for retrieving the parameters from the loaded tokens
+	
+	SensorMap smap;
+	//odometry
+	OdometrySensor* odometry=new OdometrySensor("ODOM");
+	OdometrySensor* truepos=new OdometrySensor("TRUEPOS", true);
+	
+	smap.insert(make_pair(odometry->getName(), odometry));
+	smap.insert(make_pair(truepos->getName(), truepos));
+	//sonars
+	const_iterator key=find("robot_use_sonar");
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* sonar=new RangeSensor("SONAR");
+
+		//the center of the sonar is the center of the base
+		sonar->m_pose.x=sonar->m_pose.y=sonar->m_pose.theta=0;
+
+		double maxrange=10.;
+		key=find("robot_max_sonar");
+		if (key!=end()){
+			maxrange=atof(key->second.front().c_str());
+			cerr << "max sonar:" << maxrange << endl;
+		}
+
+		unsigned int sonar_num=0;
+		key=find("robot_num_sonars");
+		if (key!=end()){
+			sonar_num=atoi(key->second.front().c_str());
+			cerr << "robot_num_sonars" << sonar_num << endl;
+		}
+
+		key=find("robot_sonar_offsets");
+		if (key!=end()){
+			const vector<string> & soff=key->second;
+
+			if( (soff.size()/3<sonar_num)){
+				cerr << __PRETTY_FUNCTION__ << ": Error " << soff.size()
+				<< " parameters for defining the sonar offsets"
+				<< " while the specified number of sonars requires "
+				<< sonar_num*3 << " parameters at least" << endl;
+			} else {
+				cerr << __PRETTY_FUNCTION__ << ": Ok " << soff.size() << " parameters for defining the sonar offsets of " << sonar_num << " devices" << endl;
+			}
+
+
+			RangeSensor::Beam beam;
+
+			for (unsigned int i=0; i<sonar_num*3; i+=3){
+				beam.span=M_PI/180.*7.5;
+				beam.pose.x=atof(soff[i].c_str());
+				beam.pose.y=atof(soff[i+1].c_str());
+				beam.pose.theta=atof(soff[i+2].c_str());
+				beam.maxRange=maxrange;
+				sonar->m_beams.push_back(beam);
+				cerr << "beam_x" << beam.pose.x;
+				cerr << " beam_y" << beam.pose.y;
+				cerr << " beam_theta" << beam.pose.theta << endl;;
+			}
+		}
+		sonar->updateBeamsLookup();
+		smap.insert(make_pair(sonar->getName(), sonar));
+	}
+	
+	//laser
+	key=find("robot_use_laser");
+
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* laser=new RangeSensor("FLASER");
+		laser->newFormat=false;
+		//by default the center of the robot is the center of the laser
+		laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
+		key=find("robot_frontlaser_offset");
+		if (key!=end()){
+			laser->m_pose.x=atof(key->second.front().c_str());
+			cerr << "FRONT OFFSET= " << laser->m_pose.x << endl;
+		}
+		
+		
+		
+		RangeSensor::Beam beam;
+			
+		//double angle=-.5*M_PI;
+		unsigned int beam_no=180;
+	
+		key=find("laser_beams");
+		if (key!=end()){
+			beam_no=atoi(key->second.front().c_str());
+			cerr << "FRONT BEAMS="<< beam_no << endl;
+		}
+		
+		double maxrange=50;
+		double resolution=1.;
+				
+		
+		if (beam_no==180 || beam_no==181)
+		  resolution =1.;
+		else if (beam_no==360 || beam_no==361)
+		  resolution =.5;
+		else if (beam_no==540 || beam_no==541)
+		  resolution =.5;
+		else if (beam_no==769) {
+		  resolution =360./1024.;
+		  maxrange = 4.1;
+		}
+		else if (beam_no==682) {
+		  resolution =360./1024.;
+		  maxrange = 4.1;
+		}
+		else if (beam_no==683) {
+		  resolution =360./1024.;
+		  maxrange = 5.5;
+		}
+		else {
+			key=find("laser_front_laser_resolution");
+			if (key!=end()){
+				resolution=atof(key->second.front().c_str());
+				cerr << "FRONT RES " << resolution << endl;
+			}
+		}
+		
+		laser->m_beams.resize(beam_no);
+		double center_beam=(double)beam_no/2.;
+		uint low_index=(uint)floor(center_beam);
+		uint up_index=(uint)ceil(center_beam);
+		double step=resolution*M_PI/180.;
+		double angle=beam_no%2?0:step;
+		unsigned int i=beam_no%2?0:1;
+		for (; i<low_index+1; i++, angle+=step){
+			beam.span=0;
+			beam.pose.x=0;
+			beam.pose.y=0;
+			beam.s = 1;
+			beam.c = 1;
+			beam.pose.theta=-angle;
+			beam.maxRange=maxrange;
+			laser->m_beams[low_index-i]=beam;
+			beam.pose.theta=angle;
+			laser->m_beams[up_index+i-1]=beam;
+		}
+		laser->updateBeamsLookup();
+		smap.insert(make_pair(laser->getName(), laser));
+		cerr << "front beams " << beam_no << endl;
+		cerr << "maxrange " << maxrange << endl;
+	}
+
+
+	key=find("robot_use_laser");
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* laser=new RangeSensor("ROBOTLASER1");
+		laser->newFormat=true;
+		cerr << "ROBOTLASER1 inserted" << endl; 
+		//by default the center of the robot is the center of the laser
+		laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
+		key=find("robot_frontlaser_offset");
+		if (key!=end()){
+			laser->m_pose.x=atof(key->second.front().c_str());
+			cerr << "FRONT OFFSET=" << laser->m_pose.x << endl;
+		}
+		
+		RangeSensor::Beam beam;
+			
+		//double angle=-.5*M_PI;
+		unsigned int beam_no=180;
+	
+		key=find("laser_beams");
+		if (key!=end()){
+			beam_no=atoi(key->second.front().c_str());
+			cerr << "FRONT BEAMS="<< beam_no << endl;
+		}
+		
+		double maxrange=50;
+		double resolution=1.;
+				
+		
+		if (beam_no==180 || beam_no==181)
+		  resolution =1.;
+		else if (beam_no==360 || beam_no==361)
+		  resolution =.5;
+		else if (beam_no==540 || beam_no==541)
+		  resolution =.5;
+		else if (beam_no==769)
+		  resolution =360./1024.;
+		else if (beam_no==683) {
+		  resolution =360./1024.;
+		   maxrange=5.50;
+		}
+		else {
+			key=find("laser_front_laser_resolution");
+			if (key!=end()){
+				resolution=atof(key->second.front().c_str());
+				cerr << "FRONT RES" << resolution << endl;
+			}
+		}
+		
+		laser->m_beams.resize(beam_no);
+		double center_beam=(double)beam_no/2.;
+		uint low_index=(uint)floor(center_beam);
+		uint up_index=(uint)ceil(center_beam);
+		double step=resolution*M_PI/180.;
+		double angle=beam_no%2?0:step;
+		unsigned int i=beam_no%2?0:1;
+		for (; i<low_index+1; i++, angle+=step){
+			beam.span=0;
+			beam.pose.x=0;
+			beam.pose.y=0;
+			beam.s=0;
+			beam.c=1;
+			beam.pose.theta=-angle;
+			beam.maxRange=maxrange;
+			laser->m_beams[low_index-i]=beam;
+			beam.pose.theta=angle;
+			laser->m_beams[up_index+i-1]=beam;
+		}
+		laser->updateBeamsLookup();
+		smap.insert(make_pair(laser->getName(), laser));
+		cerr << "front beams" << beam_no << endl;
+	}
+
+	
+	//vertical laser
+	key=find("robot_use_rear_laser");
+
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* laser=new RangeSensor("RLASER");
+		
+		//by default the center of the robot is the center of the laser
+		laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
+		laser->m_pose.theta=M_PI;
+		key=find("robot_rearlaser_offset");
+		if (key!=end()){
+			laser->m_pose.x=atof(key->second.front().c_str());
+			cerr << "REAR OFFSET = " << laser->m_pose.x << endl;
+		}
+		
+		
+		
+		RangeSensor::Beam beam;
+			
+		//double angle=-.5*M_PI;
+		unsigned int beam_no=180;
+	
+		key=find("rear_laser_beams");
+		if (key!=end()){
+			beam_no=atoi(key->second.front().c_str());
+			cerr << "REAR BEAMS="<< beam_no << endl;
+		}
+		
+		double maxrange=89;
+		double resolution=1.;
+				
+		
+		if (beam_no==180 || beam_no==181)
+			resolution =1.;
+		else if (beam_no==360 || beam_no==361)
+			resolution =.5;
+		else if (beam_no==540 || beam_no==541)
+		  resolution =.5;
+		else if (beam_no==769)
+			resolution =360./1024.;
+		else {
+			key=find("laser_rear_laser_resolution");
+			if (key!=end()){
+				resolution=atof(key->second.front().c_str());
+				cerr << "REAR RES" << resolution << endl;
+			}
+		}
+		
+		laser->m_beams.resize(beam_no);
+		double center_beam=(double)beam_no/2.;
+		uint low_index=(uint)floor(center_beam);
+		uint up_index=(uint)ceil(center_beam);
+		double step=resolution*M_PI/180.;
+		double angle=beam_no%2?0:step;
+		unsigned int i=beam_no%2?0:1;
+		for (; i<low_index+1; i++, angle+=step){
+			beam.span=0;
+			beam.pose.x=0;
+			beam.pose.y=0;
+			beam.s=0;
+			beam.c=1;
+			beam.pose.theta=-angle;
+			beam.maxRange=maxrange;
+			laser->m_beams[low_index-i]=beam;
+			beam.pose.theta=angle;
+			laser->m_beams[up_index+i-1]=beam;
+		}
+		laser->updateBeamsLookup();
+		smap.insert(make_pair(laser->getName(), laser));
+		cerr<< "rear beams" << beam_no << endl;
+	}
+
+	key=find("robot_use_rear_laser");
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* laser=new RangeSensor("ROBOTLASER2");
+		laser->newFormat=true;
+		cerr << "ROBOTLASER2 inserted" << endl; 
+		//by default the center of the robot is the center of the laser
+		laser->m_pose.x=laser->m_pose.y=0;
+		laser->m_pose.theta=M_PI;
+		key=find("robot_rearlaser_offset");
+		if (key!=end()){
+			// laser->m_pose.x==atof(key->second.front().c_str());
+			cerr << "REAR OFFSET not used" << laser->m_pose.x << endl;
+		}
+		
+		RangeSensor::Beam beam;
+			
+		//double angle=-.5*M_PI;
+		unsigned int beam_no=180;
+	
+		key=find("rear_laser_beams");
+		if (key!=end()){
+			beam_no=atoi(key->second.front().c_str());
+			cerr << "REAR BEAMS="<< beam_no << endl;
+		}
+		
+		double maxrange=50;
+		double resolution=1.;
+				
+		
+		if (beam_no==180 || beam_no==181)
+			resolution =1.;
+		else if (beam_no==360 || beam_no==361)
+			resolution =.5;
+		else if (beam_no==540 || beam_no==541)
+		  resolution =.5;
+		else if (beam_no==769)
+			resolution =360./1024.;
+		else {
+			key=find("laser_rear_laser_resolution");
+			if (key!=end()){
+				resolution=atof(key->second.front().c_str());
+				cerr << "REAR RES" << resolution << endl;
+			}
+		}
+		
+		laser->m_beams.resize(beam_no);
+		double center_beam=(double)beam_no/2.;
+		uint low_index=(uint)floor(center_beam);
+		uint up_index=(uint)ceil(center_beam);
+		double step=resolution*M_PI/180.;
+		double angle=beam_no%2?0:step;
+		unsigned int i=beam_no%2?0:1;
+		for (; i<low_index+1; i++, angle+=step){
+			beam.span=0;
+			beam.s=0;
+			beam.c=1;
+			beam.pose.x=0;
+			beam.pose.y=0;
+			beam.pose.theta=-angle;
+			beam.maxRange=maxrange;
+			laser->m_beams[low_index-i]=beam;
+			beam.pose.theta=angle;
+			laser->m_beams[up_index+i-1]=beam;
+		}
+		laser->updateBeamsLookup();
+		smap.insert(make_pair(laser->getName(), laser));
+		cerr << "rear beams" << beam_no << endl;
+	}
+
+
+	return smap;
+}
+
+};
+

slam_gmapping/openslam_gmapping/log/carmenconfiguration.h

@@ -0,0 +1,22 @@
+#ifndef CARMENCONFIGURATION_H
+#define CARMENCONFIGURATION_H
+
+#include <string>
+#include <map>
+#include <vector>
+#include <istream>
+#include <sensor/sensor_base/sensor.h>
+#include "configuration.h"
+
+namespace GMapping {
+
+class CarmenConfiguration: public Configuration, public std::map<std::string, std::vector<std::string> >{
+	public:
+		virtual std::istream& load(std::istream& is);
+		virtual SensorMap computeSensorMap() const;
+};
+
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/log/configuration.cpp

@@ -0,0 +1,8 @@
+#include "configuration.h"
+
+namespace GMapping {
+
+Configuration::~Configuration(){
+}
+
+};

slam_gmapping/openslam_gmapping/log/configuration.h

@@ -0,0 +1,17 @@
+#ifndef CONFIGURATION_H
+#define CONFIGURATION_H
+
+#include <istream>
+#include <sensor/sensor_base/sensor.h>
+
+namespace GMapping {
+
+class Configuration{
+	public:
+		virtual ~Configuration();
+		virtual SensorMap computeSensorMap() const=0;
+};
+
+};
+#endif
+

slam_gmapping/openslam_gmapping/log/log_plot.cpp

@@ -0,0 +1,71 @@
+#include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <sys/types.h>
+#include <log/carmenconfiguration.h>
+#include <log/sensorlog.h>
+
+
+using namespace std;
+using namespace GMapping;
+
+int main(int argc, char ** argv){
+  double maxrange=2.;
+	if (argc<2){
+		cout << "usage log_plot <filename> | gnuplot" << endl;
+		exit (-1);
+	}
+	ifstream is(argv[1]);
+	if (! is){
+		cout << "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();
+	int count=0;
+	int frame=0;
+	cerr << "log size" << log.size() << endl;
+	for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){
+		RangeReading* rr=dynamic_cast<RangeReading*>(*it);
+		if (rr){
+		  count++;
+		  if (count%3)
+		    continue;
+		  std::vector<Point> points(rr->size());
+		  uint j=0;
+		  for (uint i=0; i<rr->size(); i++){
+		    const RangeSensor * rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
+		    double c=rs->beams()[i].c, s=rs->beams()[i].s;
+		    double r=(*rr)[i];
+		    if (r>maxrange)
+		      continue;
+		    points[j++]=Point(r*c,r*s);
+		  }
+		  if (j){
+		    char buf[1024];
+		    sprintf(buf,"frame-%05d.gif",frame);
+		    frame++;
+		    cout << "set terminal gif" << endl;
+		    cout << "set output \"" << buf << "\"" << endl;
+		    cout << "set size ratio -1" << endl;
+		    cout << "plot [-3:3][0:3] '-' w p ps 1" << endl;
+		    for (uint i=0; i<j; i++){
+		      cout << points[i].y << " " << points[i].x << endl;
+		  }
+		  cout << "e" << endl;
+		  }
+		}
+	}
+}

slam_gmapping/openslam_gmapping/log/log_test.cpp

@@ -0,0 +1,47 @@
+#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){
+		cout << "usage log_test <filename>" << endl;
+		exit (-1);
+	}
+	ifstream is(argv[1]);
+	if (! is){
+		cout << "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){
+			//cerr << rr->getSensor()->getName() << " ";
+			//cerr << rr->size()<< " ";
+			//for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){
+			//	cerr << *it << " ";
+			//}
+			cout<< rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime()  << endl;
+		}
+	}
+}

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;
+		}
+	}
+}

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();
+}

slam_gmapping/openslam_gmapping/log/sensorlog.cpp

@@ -0,0 +1,153 @@
+#include "sensorlog.h"
+
+#include <iostream>
+#include <sstream>
+#include <assert.h>
+#include <sensor_odometry/odometrysensor.h>
+#include <sensor_range/rangesensor.h>
+
+#define LINEBUFFER_SIZE 100000
+
+namespace GMapping {
+
+using namespace std;
+
+SensorLog::SensorLog(const SensorMap& sm): m_sensorMap(sm){
+}
+
+SensorLog::~SensorLog(){
+	for (iterator it=begin(); it!=end(); it++)
+		if (*it) delete (*it);
+}
+
+istream& SensorLog::load(istream& is){
+	for (iterator it=begin(); it!=end(); it++)
+		if (*it) delete (*it);
+	clear();
+	
+	char buf[LINEBUFFER_SIZE];
+	while (is){
+		is.getline(buf, LINEBUFFER_SIZE);
+		istringstream lis(buf);
+		
+		string sensorname;
+		
+		if (lis) 
+			lis >>sensorname; 
+		else 
+			continue;
+		
+		
+			
+		SensorMap::const_iterator it=m_sensorMap.find(sensorname);
+		if (it==m_sensorMap.end()){
+			continue;
+		}
+		
+		Sensor* sensor=it->second;
+		
+		SensorReading* reading=0;
+		OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
+		if (odometry)
+			reading=parseOdometry(lis, odometry);
+		
+		RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
+		if (range)
+			reading=parseRange(lis, range);
+		if (reading)
+			push_back(reading);
+	}
+	return is;
+	
+}
+
+OdometryReading* SensorLog::parseOdometry(istream& is, const OdometrySensor* osen) const{
+	OdometryReading* reading=new OdometryReading(osen);
+	OrientedPoint pose;
+	OrientedPoint speed;
+	OrientedPoint accel;
+	is >> pose.x >> pose.y >> pose.theta;
+	is >> speed.x >>speed.theta;
+	speed.y=0;
+	is >> accel.x;
+	accel.y=accel.theta=0;
+	reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
+	return reading;
+}
+
+RangeReading* SensorLog::parseRange(istream& is, const RangeSensor* rs) const{
+	if(rs->newFormat){
+		string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
+		is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy >> remission_mode;
+	}
+	
+	unsigned int size;
+	is >> size;
+	assert(size==rs->beams().size());
+	
+	RangeReading* reading=new RangeReading(rs);
+	//cerr << "#R=" << size << endl;
+	reading->resize(size);
+	for (unsigned int i=0; i<size; i++){
+		is >> (*reading)[i];
+	}
+	if (rs->newFormat){
+		int reflectionBeams;
+		is >> reflectionBeams;
+		double reflection;
+		for (int i=0; i<reflectionBeams; i++)
+			is >> reflection;
+	}
+	//FIXME XXX
+	OrientedPoint laserPose;
+	is >> laserPose.x >> laserPose.y >> laserPose.theta;
+	OrientedPoint pose;
+	is >> pose.x >> pose.y >> pose.theta; 
+	reading->setPose(pose);
+	double a,b,c;
+	if (rs->newFormat){
+		string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
+		is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
+	} else {
+		is >> a >> b >> c;
+	}
+	string s;
+	is >> a >> s;
+	is >> a;
+	reading->setTime(a);
+	return reading;
+}
+
+OrientedPoint SensorLog::boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const {
+	xmin=ymin=1e6;
+	xmax=ymax=-1e6;
+	bool first=true;
+	OrientedPoint start;
+	for (const_iterator it=begin(); it!=end(); it++){
+		double lxmin=0., lxmax=0., lymin=0., lymax=0.;
+		const SensorReading* reading=*it;
+		const OdometryReading* odometry=dynamic_cast<const OdometryReading*> (reading);
+		if (odometry){
+			lxmin=lxmax=odometry->getPose().x;
+			lymin=lymax=odometry->getPose().y;
+		}
+		
+		const RangeReading* rangeReading=dynamic_cast<const RangeReading*> (reading);
+		if (rangeReading){
+			lxmin=lxmax=rangeReading->getPose().x;
+			lymin=lymax=rangeReading->getPose().y;
+			if (first){
+				first=false;
+				start=rangeReading->getPose();	
+			}
+		}
+		xmin=xmin<lxmin?xmin:lxmin;
+		xmax=xmax>lxmax?xmax:lxmax;
+		ymin=ymin<lymin?lymin:lymin;
+		ymax=ymax>lymax?ymax:lymax;
+	}
+	return start;
+}
+
+};
+

slam_gmapping/openslam_gmapping/log/sensorlog.h

@@ -0,0 +1,29 @@
+#ifndef SENSORLOG_H
+#define SENSORLOG_H
+
+#include <list>
+#include <istream>
+#include <sensor/sensor_base/sensorreading.h>
+#include <sensor/sensor_odometry/odometrysensor.h>
+#include <sensor/sensor_range/rangesensor.h>
+#include <sensor/sensor_odometry/odometryreading.h>
+#include <sensor/sensor_range/rangereading.h>
+#include "configuration.h"
+
+namespace GMapping {
+
+class SensorLog : public std::list<SensorReading*>{
+	public:
+		SensorLog(const SensorMap&);
+		~SensorLog();
+		std::istream& load(std::istream& is);
+		OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const;
+	protected:
+		const SensorMap& m_sensorMap;
+		OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const;
+		RangeReading* parseRange(std::istream& is, const RangeSensor* ) const;
+};
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/log/sensorstream.cpp

@@ -0,0 +1,155 @@
+#include <assert.h>
+#include <sstream>
+#include "sensorstream.h"
+//#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-))
+// Can't declare a buffer that big on the stack.  So we'll risk Cyrill's
+// unblessing, and make it smaller.
+#define LINEBUFFER_SIZE 8192
+
+namespace GMapping {
+
+using namespace std;
+
+//SensorStream
+SensorStream::SensorStream(const SensorMap& sensorMap) :m_sensorMap(sensorMap){}
+
+SensorStream::~SensorStream(){}
+
+SensorReading* SensorStream::parseReading(std::istream& is, const SensorMap& smap){
+	SensorReading* reading=0;
+	if (is){
+		char buf[LINEBUFFER_SIZE];
+		is.getline(buf, LINEBUFFER_SIZE);
+		istringstream lis(buf);
+		
+		string sensorname;
+		
+		if (lis){
+			lis >>sensorname; 
+		} else 
+			return 0;
+		
+		SensorMap::const_iterator it=smap.find(sensorname);
+		if (it==smap.end()){
+			return 0;
+		}
+		
+		Sensor* sensor=it->second;
+		
+		OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
+		if (odometry)
+			reading=parseOdometry(lis, odometry);
+		
+		RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
+		if (range)
+			reading=parseRange(lis, range);
+	}
+	return reading;
+}
+
+OdometryReading* SensorStream::parseOdometry(std::istream& is, const OdometrySensor* osen ){
+	OdometryReading* reading=new OdometryReading(osen);
+	OrientedPoint pose;
+	OrientedPoint speed;
+	OrientedPoint accel;
+	is >> pose.x >> pose.y >> pose.theta;
+	is >> speed.x >>speed.theta;
+	speed.y=0;
+	is >> accel.x;
+	accel.y=accel.theta=0;
+	reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
+	double timestamp, reltimestamp;
+	string s;
+	is >> timestamp >>s >> reltimestamp;
+	reading->setTime(timestamp);
+	return reading;
+}
+
+RangeReading* SensorStream::parseRange(std::istream& is, const RangeSensor* rs){
+	//cerr << __PRETTY_FUNCTION__ << endl;
+	if(rs->newFormat){
+		string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
+		is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode;
+		//cerr << " New format laser msg" << endl;
+	}
+	unsigned int size;
+	is >> size;
+	assert(size==rs->beams().size());
+	RangeReading* reading=new RangeReading(rs);
+	reading->resize(size);
+	for (unsigned int i=0; i<size; i++){
+		is >> (*reading)[i];
+	}
+	if (rs->newFormat){
+		int reflectionBeams;
+		is >> reflectionBeams;
+		double reflection;
+		for (int i=0; i<reflectionBeams; i++)
+			is >> reflection;
+	}
+	OrientedPoint laserPose;
+	is >> laserPose.x >> laserPose.y >> laserPose.theta;
+	OrientedPoint pose;
+	is >> pose.x >> pose.y >> pose.theta;
+	reading->setPose(pose);
+
+	if (rs->newFormat){
+		string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
+		is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
+	} 
+// 	else {
+// 	  double a,b,c;
+// 		is >> a >> b >> c;
+// 	}
+	double timestamp, reltimestamp;
+	string s;
+	is >> timestamp >>s >> reltimestamp;
+	reading->setTime(timestamp);
+	return reading;
+
+}
+
+//LogSensorStream
+LogSensorStream::LogSensorStream(const SensorMap& sensorMap, const SensorLog* log):
+	SensorStream(sensorMap){
+	m_log=log;
+	assert(m_log);
+	m_cursor=log->begin();
+}
+
+LogSensorStream::operator bool() const{
+	return m_cursor==m_log->end();
+}
+
+bool LogSensorStream::rewind(){
+	m_cursor=m_log->begin();
+	return true;
+}
+
+SensorStream& LogSensorStream::operator >>(const SensorReading*& rd){
+	rd=*m_cursor;
+	m_cursor++;
+	return *this;
+}
+
+//InputSensorStream
+InputSensorStream::InputSensorStream(const SensorMap& sensorMap, std::istream& is):
+	SensorStream(sensorMap), m_inputStream(is){
+}
+
+InputSensorStream::operator bool() const{
+	return (bool) m_inputStream;
+}
+
+bool InputSensorStream::rewind(){
+	//m_inputStream.rewind();
+	return false;
+}
+
+SensorStream& InputSensorStream::operator >>(const SensorReading*& reading){
+	reading=parseReading(m_inputStream, m_sensorMap);
+	return *this;
+}
+
+};
+

slam_gmapping/openslam_gmapping/log/sensorstream.h

@@ -0,0 +1,47 @@
+#ifndef SENSORSTREAM_H
+#define SENSORSTREAM_H
+
+#include <istream>
+#include "sensorlog.h"
+
+namespace GMapping {
+class SensorStream{
+	public:
+		SensorStream(const SensorMap& sensorMap);
+		virtual ~SensorStream();
+		virtual operator bool() const=0;
+		virtual bool rewind() = 0 ;
+		virtual SensorStream& operator >>(const SensorReading*&) = 0;
+		inline const SensorMap& getSensorMap() const {return m_sensorMap; }
+	protected:
+		const SensorMap& m_sensorMap;
+		static SensorReading* parseReading(std::istream& is, const SensorMap& smap);
+		static OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* );
+		static RangeReading* parseRange(std::istream& is, const RangeSensor* );
+};
+
+class InputSensorStream: public SensorStream{
+	public:
+		InputSensorStream(const SensorMap& sensorMap, std::istream& is);
+		virtual operator bool() const;
+		virtual bool rewind();
+		virtual SensorStream& operator >>(const SensorReading*&);
+		
+		//virtual SensorStream& operator >>(SensorLog*& log);
+	protected:
+		std::istream& m_inputStream;
+};
+
+class LogSensorStream: public SensorStream{
+	public:
+		LogSensorStream(const SensorMap& sensorMap, const SensorLog* log);
+		virtual operator bool() const;
+		virtual bool rewind();
+		virtual SensorStream& operator >>(const SensorReading*&);
+	protected:
+		const SensorLog* m_log;
+		SensorLog::const_iterator m_cursor;
+};
+
+};
+#endif