Robot Simulator of the Robotics Group for Self-Organization of Control: replaycontroller.h Source File

# replaycontroller.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 LpzRobots development team                    *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
    00005  *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
    00006  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024 #ifndef __REPLAYCONTROLLER_H
    00025 #define __REPLAYCONTROLLER_H
    00026
    00027 #include "abstractcontroller.h"
    00028
    00029 /**
    00030  * Controller that replays a file
    00031  */
    00032 class ReplayController : public AbstractController {
    00033 public:
    00034   ReplayController(const char* filename, bool repeat=false)
    00035     : AbstractController("ReplayController", "1.0"),
    00036       filename(filename), repeat(repeat) {
    00037
    00038     f=fopen(filename,"r");
    00039     if(!f){
    00040       std::cerr<< "ReplayController: error while opening file " << filename << std::endl;
    00041       exit(1);
    00042     }
    00043     if(!parseDataFileForHeader(f)){
    00044       std::cerr<< "ReplayController: error while seaching for header in file " << filename << std::endl;
    00045       exit(1);
    00046     }
    00047     printf("ReplayController: columns: Senors [%i, %i], Motors [%i, %i]\n",
    00048            sensorStart, sensorEnd, motorStart, motorEnd);
    00049   }
    00050
    00051   virtual void init(int sensornumber, int motornumber, RandGen* randGen = 0){
    00052     //assert(sensornumber == sensorEnd - sensorStart + 1);
    00053     assert(motornumber  == motorEnd - motorStart + 1);
    00054   }
    00055
    00056   virtual int getSensorNumber() const { return sensorEnd - sensorStart + 1;};
    00057
    00058   virtual int getMotorNumber() const { return motorEnd - motorStart + 1; };
    00059
    00060   virtual void step(const sensor* sensors, int sensornumber,
    00061                     motor* motors, int motornumber){
    00062     stepNoLearning(sensors,sensornumber, motors, motornumber);
    00063   }
    00064
    00065   virtual void stepNoLearning(const sensor* , int number_sensors,
    00066                               motor* motors, int number_motors){
    00067
    00068     if(!parseDataLine(m,f)){
    00069       if(repeat){
    00070         std::cout << "ReplayController: rewind" << std::endl;
    00071         rewind(f);
    00072       }else
    00073         std::cout << "ReplayController: no datafile in file" << filename << std::endl;
    00074     }else{
    00075       m=m.rows(motorStart, motorEnd);
    00076     }
    00077     m.convertToBuffer(motors, sensorEnd-sensorStart + 1);
    00078   }
    00079
    00080   /**** STOREABLE ****/
    00081   /** stores the controller values to a given file (binary).  */
    00082   virtual bool store(FILE* f) const {return false;}
    00083   /** loads the controller values from a given file (binary). */
    00084   virtual bool restore(FILE* f) {return false;}
    00085
    00086   // inspectable interface
    00087   virtual std::list<iparamkey> getInternalParamNames()const  { return std::list<iparamkey>(); }
    00088   virtual std::list<iparamval> getInternalParams() const { return std::list<iparamval>(); }
    00089
    00090 protected:
    00091
    00092   bool parseDataFileForHeader(FILE* f){
    00093     char buffer[1024];
    00094     int i;
    00095     sensorStart=-1;
    00096     sensorEnd=-1;
    00097     motorStart=-1;
    00098     motorEnd=-1;
    00099
    00100     while(fgets(buffer, 1024, f)) {
    00101       if(buffer[0]=='#' && buffer[1]=='C'){
    00102         // scan line and return
    00103         i=0;
    00104         char* p;
    00105         p=strtok(buffer," ");
    00106         if(!p) return false; // frist one is #C
    00107         while((p=strtok(NULL," "))!=NULL )  {
    00108           if(p[0]=='x' && p[1]=='['){
    00109             if(sensorStart==-1) sensorStart=i;
    00110             sensorEnd=i;
    00111           }
    00112           if(p[0]=='y' && p[1]=='['){
    00113             if(motorStart==-1) motorStart=i;
    00114             motorEnd=i;
    00115           }
    00116           i++;
    00117         }
    00118         return true;
    00119       }
    00120     }
    00121     return false;
    00122   }
    00123
    00124   static bool isEmpty(const char* c){
    00125     const char* p = c;
    00126     bool foundsomething = false;
    00127     while(*p != 0){
    00128       if(*p > ' ') foundsomething = true;
    00129       p++;
    00130     }
    00131     return !foundsomething;
    00132   }
    00133
    00134
    00135   static bool check4Number(const char* c){
    00136     const char* p = c;
    00137     while(*p != 0){
    00138       if(*p >= '0' && *p <= '9') return true;
    00139       p++;
    00140     }
    00141     return false;
    00142   }
    00143
    00144   static bool parseDataLine(matrix::Matrix& data, FILE* f){
    00145     char buffer[1024];
    00146     int i;
    00147     double dat[1024];
    00148     while(fgets(buffer, 1024, f)){
    00149       if(buffer[0]=='#' || isEmpty(buffer)){
    00150         continue;
    00151       }else{
    00152         i=0;
    00153         char* p;
    00154         p=strtok(buffer," ");
    00155         if(!p) return false;
    00156         dat[i] = atof(p);
    00157         i++;
    00158         while((p=strtok(NULL," "))!=NULL )  {
    00159           if(!check4Number(p)) continue;
    00160           dat[i] = atof(p);
    00161           i++;
    00162         };
    00163         data.set(i,1,dat);
    00164         return true;
    00165       }
    00166     };
    00167     return false;
    00168   }
    00169
    00170
    00171
    00172 protected:
    00173   int sensorStart;
    00174   int sensorEnd;
    00175   int motorStart;
    00176   int motorEnd;
    00177   matrix::Matrix m;
    00178   const char* filename;
    00179   FILE* f;
    00180   bool repeat;
    00181
    00182 };
    00183
    00184 #endif