Robotsystem from Robot Group Leipzig: agent.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# agent.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: agent.cpp,v $
    00023  *   Revision 1.1.2.7  2006/03/31 16:28:32  fhesse
    00024  *   in setTrackoptions: trackrobot.open() only when one of the track optoions is active
    00025  *
    00026  *   Revision 1.1.2.6  2006/02/24 14:46:00  martius
    00027  *   recording inverval in plotoutput
    00028  *
    00029  *   Revision 1.1.2.5  2006/02/20 17:33:15  martius
    00030  *   include interval in logging output
    00031  *
    00032  *   Revision 1.1.2.4  2006/02/06 15:24:09  robot3
    00033  *   avoid conversion from pointer to int
    00034  *
    00035  *   Revision 1.1.2.3  2006/01/31 16:45:37  martius
    00036  *   neuronviz plotoption
    00037  *
    00038  *   Revision 1.1.2.2  2005/11/15 12:30:26  martius
    00039  *   new selforg structure and OdeAgent, OdeRobot ...
    00040  *
    00041  *   Revision 1.1.2.1  2005/11/14 17:37:56  martius
    00042  *   moved to selforg
    00043  *
    00044  *   Revision 1.13  2005/11/07 17:04:20  martius
    00045  *   agent can be constructed using a list of PlotOptions
    00046  *   tracking file gets the controller parameter as well
    00047  *
    00048  *   Revision 1.12  2005/10/28 12:03:36  martius
    00049  *   network description printed
    00050  *
    00051  *   Revision 1.11  2005/10/24 13:32:07  fhesse
    00052  *   comments adjusted and in doxygen style
    00053  *
    00054  *   Revision 1.10  2005/10/06 17:11:36  martius
    00055  *   switched to stl lists
    00056  *
    00057  *   Revision 1.9  2005/09/22 12:24:36  martius
    00058  *   removed global variables
    00059  *   OdeHandle and GlobalData are used instead
    00060  *   sensor prepared
    00061  *
    00062  *   Revision 1.8  2005/08/31 11:15:59  martius
    00063  *   *** empty log message ***
    00064  *
    00065  *   Revision 1.7  2005/08/03 20:34:58  martius
    00066  *   use if Inspectable interface
    00067  *
    00068  *   Revision 1.6  2005/07/26 17:01:47  martius
    00069  *   flushing every 10
    00070  *   guilogger is opened with nice -2
    00071  *
    00072  *   Revision 1.5  2005/07/26 08:36:53  fhesse
    00073  *   delay for logging in file changed from 15 to 3
    00074  *
    00075  *   Revision 1.4  2005/07/21 11:30:59  fhesse
    00076  *   started with blind motors
    00077  *
    00078  *   Revision 1.3  2005/07/18 14:44:27  martius
    00079  *   noise moved into wiring
    00080  *
    00081  *   Revision 1.2  2005/07/18 10:14:04  martius
    00082  *   noise moved to wiring
    00083  *
    00084  *   Revision 1.1  2005/07/14 15:57:53  fhesse
    00085  *   now agent contains controller, robot and wiring, plotting ability included, therefore plotagent can be removed; ono2onewiring replaces one2oneagent
    00086  *
    00087  *                                                                 *
    00088  ***************************************************************************/
    00089
    00090 #include "agent.h"
    00091 #include <signal.h>
    00092 #include "printInternals.h"
    00093
    00094 #include "abstractrobot.h"
    00095 #include "abstractcontroller.h"
    00096 #include "abstractwiring.h"
    00097
    00098 Agent::Agent(const PlotOption& plotOption){
    00099   internInit();
    00100   plotOptions.push_back(plotOption);
    00101 }
    00102
    00103
    00104 Agent::Agent(const list<PlotOption>& plotOptions)
    00105   : plotOptions(plotOptions){
    00106   internInit();
    00107 }
    00108
    00109 void Agent::internInit(){
    00110   controller = 0;
    00111   robot      = 0;
    00112   wiring     = 0;
    00113
    00114   rsensors=0; rmotors=0;
    00115   csensors=0; cmotors=0;
    00116
    00117   t=0;
    00118 }
    00119
    00120 Agent::~Agent(){
    00121   for(list<PlotOption>::iterator i=plotOptions.begin(); i != plotOptions.end(); i++){
    00122     (*i).close();
    00123   }
    00124   trackrobot.close();
    00125   if(rsensors) free(rsensors);
    00126   if(rmotors)  free(rmotors);
    00127   if(csensors) free(csensors);
    00128   if(cmotors)  free(cmotors);
    00129 }
    00130
    00131
    00132 /// initializes the object with the given controller, robot and wiring
    00133 //  and initializes pipe to guilogger
    00134 bool Agent::init(AbstractController* controller, AbstractRobot* robot, AbstractWiring* wiring){
    00135   this->controller = controller;
    00136   this->robot      = robot;
    00137   this->wiring     = wiring;
    00138   if(!controller || !robot || !wiring) return false;
    00139   else{
    00140     rsensornumber = robot->getSensorNumber();
    00141     rmotornumber  = robot->getMotorNumber();
    00142     wiring->init(rsensornumber, rmotornumber);
    00143     csensornumber = wiring->getControllerSensornumber();
    00144     cmotornumber  = wiring->getControllerMotornumber();
    00145     controller->init(csensornumber, cmotornumber);
    00146
    00147     rsensors      = (sensor*) malloc(sizeof(sensor) * rsensornumber);
    00148     rmotors       = (motor*)  malloc(sizeof(motor)  * rmotornumber);
    00149     csensors      = (sensor*) malloc(sizeof(sensor) * csensornumber);
    00150     cmotors       = (motor*)  malloc(sizeof(motor)  * cmotornumber);
    00151
    00152     for(list<PlotOption>::iterator i=plotOptions.begin(); i != plotOptions.end(); i++){
    00153       if((*i).open()){
    00154         // print network description given by the structural information of the controller
    00155         printNetworkDescription((*i).pipe, "Lpzrobots"/*controller->getName()*/, controller);
    00156         // print interval
    00157         if((*i).interval > 1) fprintf((*i).pipe, "# Recording every %dth dataset\n", (*i).interval);
    00158         // print all parameters of the controller
    00159         controller->print((*i).pipe, "# ");
    00160         // print head line with all parameter names
    00161         unsigned int snum = (*i).whichSensors == Robot ? rsensornumber : csensornumber;
    00162         Inspectable* inspectables[2] = {controller, wiring};
    00163         printInternalParameterNames((*i).pipe, snum, cmotornumber, inspectables, 2);
    00164       }
    00165     }
    00166     return true;
    00167   }
    00168 }
    00169
    00170
    00171 bool PlotOption::open(){
    00172   // this prevents the simulation to terminate if the child  closes
    00173   // or if we fail to open it.
    00174   signal(SIGPIPE,SIG_IGN);
    00175   // TODO: get the guilogger call from some config
    00176   switch(mode){
    00177   case GuiLogger_File:
    00178     pipe=popen("guilogger -l -m pipe -d 5","w");
    00179     break;
    00180   case GuiLogger:
    00181     pipe=popen("guilogger -m pipe -d 5","w");
    00182     break;
    00183   case NeuronViz:
    00184     pipe=popen("neuronviz ","w");
    00185     break;
    00186   default:
    00187     return false;
    00188   }
    00189   if(pipe==0){
    00190     fprintf(stderr, "%s:%i: could not open plot tool!\n", __FILE__, __LINE__);
    00191     return false;
    00192   }else return true;
    00193 }
    00194
    00195 void PlotOption::close(){
    00196   if (pipe) pclose(pipe);
    00197   pipe=0;
    00198 }
    00199
    00200 // Plots controller sensor- and motorvalues and internal controller parameters.
    00201 void Agent::plot(const sensor* rx, int rsensornumber, const sensor* cx, int csensornumber,
    00202                  const motor* y, int motornumber){
    00203   assert(controller && rx && cx && y);
    00204
    00205   Inspectable* inspectables[2] = {controller, wiring};
    00206   for(list<PlotOption>::iterator i=plotOptions.begin(); i != plotOptions.end(); i++){
    00207     if( ((*i).pipe) && (t % (*i).interval == 0) ){
    00208       if((*i).whichSensors == Robot){
    00209         printInternalParameters((*i).pipe, rx, rsensornumber, y, motornumber, inspectables , 2);
    00210       }else{
    00211         printInternalParameters((*i).pipe, cx, csensornumber, y, motornumber, inspectables , 2);
    00212       }
    00213       if(t% ((*i).interval * 10)) fflush((*i).pipe);
    00214     }
    00215   }
    00216 };
    00217
    00218
    00219
    00220 //  Performs an step of the agent, including sensor reading, pushing sensor values through wiring,
    00221 //  controller step, pushing controller steps back through wiring and sent resulting motorcommands
    00222 //  to robot.
    00223 //  @param noise Noise strength.
    00224 void Agent::step(double noise){
    00225
    00226   if(!controller || !robot || !wiring || !rsensors || !rmotors || !csensors || !cmotors) {
    00227     fprintf(stderr, "%s:%i: something is null: cont %i rob %i wiring %i rsens %i rmots %i csens %i cmots %i!\n",
    00228             __FILE__, __LINE__, controller==0, robot==0,
    00229             wiring==0, rsensors==0, rmotors==0,
    00230             csensors==0, cmotors==0);
    00231   }
    00232
    00233   int len =  robot->getSensors(rsensors, rsensornumber);
    00234   if(len != rsensornumber){
    00235     fprintf(stderr, "%s:%i: Got not enough sensors, expected %i, got %i!\n", __FILE__, __LINE__,
    00236             rsensornumber, len);
    00237   }
    00238
    00239   wiring->wireSensors(rsensors, rsensornumber, csensors, csensornumber, noise);
    00240   controller->step(csensors, csensornumber, cmotors, cmotornumber);
    00241   wiring->wireMotors(rmotors, rmotornumber, cmotors, cmotornumber);
    00242   robot->setMotors(rmotors, rmotornumber);
    00243   plot(rsensors, rsensornumber, csensors, csensornumber, cmotors, cmotornumber);
    00244
    00245   trackrobot.track(robot);
    00246
    00247   t++;
    00248 }
    00249
    00250
    00251 // sets the trackoptions which enable tracking of a robot
    00252 void Agent::setTrackOptions(const TrackRobot& trackrobot){
    00253   this->trackrobot = trackrobot;
    00254   if (trackrobot.trackPos || trackrobot.trackSpeed || trackrobot.trackOrientation){
    00255     if(!this->trackrobot.open(robot)){
    00256       fprintf(stderr, "could not open trackfile!\n");
    00257     }else{
    00258       // print all parameters of the controller
    00259       controller->print(this->trackrobot.file, "# ");
    00260     }
    00261   }
    00262 }