log_plot.cpp

Go to the documentation of this file.

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <sys/types.h>
#include <gmapping/log/carmenconfiguration.h>
#include <gmapping/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;
                  }
                }
        }
}