Robotsystem of the Robot Group Leipzig: integration/main.cpp

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

- Search for |

# integration/main.cpp

A simple example for the integration of a robot into the framework (with agent, controller, wiring). This faciliates you with the plotting options, tracing and such like. Here the data flow is controlled by the agent, which then asks the robot about the sensors, invokes the controller and sends the motor values the robot again.

    00001 #include <signal.h>
    00002 #include <unistd.h>
    00003 #include <iostream>
    00004 #include <vector>
    00005 #include <list>
    00006 #include <iterator>
    00007 using namespace std;
    00008
    00009 #include <selforg/agent.h>
    00010 #include <selforg/abstractrobot.h>
    00011 #include <selforg/invertmotorspace.h>
    00012 #include <selforg/one2onewiring.h>
    00013
    00014 #include "cmdline.h"
    00015
    00016 bool stop=0;
    00017
    00018 class MyRobot : public AbstractRobot {
    00019 public:
    00020   MyRobot()
    00021     : AbstractRobot("MyRobot", "$Id: main.cpp,v 1.3 2006/08/04 15:16:13 martius Exp $") {
    00022     myparam=0;
    00023     motornumber  = 3;
    00024     sensornumber = 3;
    00025     x = new double[sensornumber];
    00026     y = new double[motornumber];
    00027   }
    00028
    00029   MyRobot::~MyRobot(){
    00030     if(x) delete[] x;
    00031     if(y) delete[] y;
    00032   }
    00033
    00034   // robot interface
    00035
    00036   /** returns actual sensorvalues
    00037       @param sensors sensors scaled to [-1,1]
    00038       @param sensornumber length of the sensor array
    00039       @return number of actually written sensors
    00040   */
    00041   virtual int getSensors(sensor* sensors, int sensornumber){
    00042     assert(sensornumber == this->sensornumber);
    00043     memcpy(sensors, x, sizeof(sensor) * sensornumber);
    00044     return sensornumber;
    00045   }
    00046
    00047   /** sets actual motorcommands
    00048       @param motors motors scaled to [-1,1]
    00049       @param motornumber length of the motor array
    00050   */
    00051   virtual void setMotors(const motor* motors, int motornumber){
    00052     assert(motornumber == this->motornumber);
    00053     memcpy(y, motors, sizeof(motor) * motornumber);
    00054
    00055     // motor values are now stored in y, sensor values are expected to be stored in x
    00056
    00057     // perform robot action here
    00058
    00059     //  this is just a senceless dummy robot
    00060     for(int i=0; i<sensornumber; i++){
    00061       x[i] = sin(myparam) * y[i%motornumber];
    00062     }
    00063     myparam+=0.01;
    00064
    00065   }
    00066
    00067   /** returns number of sensors */
    00068   virtual int getSensorNumber(){ return sensornumber; }
    00069
    00070   /** returns number of motors */
    00071   virtual int getMotorNumber() { return motornumber; }
    00072
    00073   /** returns position of the object
    00074       @return vector of position (x,y,z) */
    00075   virtual Position getPosition() const {return Position(0,0,0);}
    00076
    00077   /** returns linear speed vector of the object
    00078       @return vector  (vx,vy,vz)
    00079    */
    00080   virtual Position getSpeed() const {return Position(0,0,0);}
    00081
    00082   /** returns the orientation of the object
    00083       @return 3x3 rotation matrix
    00084    */
    00085   virtual matrix::Matrix getOrientation() const {
    00086     matrix::Matrix m(3,3);
    00087     m.toId();
    00088     return m;
    00089   };
    00090
    00091   virtual paramval getParam(const paramkey& key) const{
    00092     if(key == "myparam") return myparam;
    00093     else return Configurable::getParam(key);
    00094   }
    00095
    00096   virtual bool setParam(const paramkey& key, paramval val){
    00097     cerr << "huhu";
    00098     if(key == "myparam") myparam = val;
    00099     else return Configurable::setParam(key, val);
    00100     return true;
    00101   }
    00102
    00103   virtual paramlist getParamList() const {
    00104     paramlist list;
    00105     list += pair<paramkey, paramval> (string("myparam"), myparam);
    00106     return list;
    00107   };
    00108
    00109 private:
    00110   int motornumber;
    00111   int sensornumber;
    00112
    00113   double* x;
    00114   double* y;
    00115
    00116   paramval myparam;
    00117 };
    00118
    00119
    00120 void onTermination(){
    00121   stop=1;
    00122 }
    00123
    00124 // Helper
    00125 int contains(char **list, int len,  const char *str){
    00126   for(int i=0; i<len; i++){
    00127     if(strcmp(list[i],str) == 0) return i+1;
    00128   }
    00129   return 0;
    00130 }
    00131
    00132 int main(int argc, char** argv){
    00133   ConfigList configs;
    00134   list<PlotOption> plotoptions;
    00135
    00136   AbstractController* controller = new InvertMotorSpace(10);
    00137   controller->setParam("s4delay",2.0);
    00138   controller->setParam("s4avg",2.0);
    00139
    00140   if(contains(argv,argc,"-g")!=0) plotoptions.push_back(PlotOption(GuiLogger));
    00141   if(contains(argv,argc,"-f")!=0) plotoptions.push_back(PlotOption(File));
    00142   if(contains(argv,argc,"-h")!=0) {
    00143     printf("Usage: %s [-g] [-f]\n",argv[0]);
    00144     printf("\t-g\tstart guilogger\n\t-f\twrite logfile\n\t-h\tdisplay this help\n");
    00145     exit(0);
    00146   }
    00147
    00148   printf("\nPress Ctrl-c to invoke parameter input shell (and again Ctrl-c to quit)\n");
    00149
    00150   AbstractRobot* robot   = new MyRobot();
    00151   Agent* agent           = new Agent(plotoptions);
    00152   AbstractWiring* wiring  = new One2OneWiring(new ColorUniformNoise(0.1));
    00153   agent->init(controller, robot, wiring);
    00154   // if you like, you can keep track of the robot with the following line.
    00155   //  this assumes that you robot returns its position, speed and orientation.
    00156   // agent->setTrackOptions(TrackRobot(true,false,false, false,"interagetiontest"));
    00157
    00158   configs.push_back(robot);
    00159   configs.push_back(controller);
    00160   showParams(configs);
    00161
    00162   cmd_handler_init();
    00163   while(!stop){
    00164     usleep(1000);
    00165     agent->step(0.1);
    00166     if(control_c_pressed()){
    00167       cmd_begin_input();
    00168       changeParams(configs, onTermination);
    00169       cmd_end_input();
    00170     }
    00171
    00172   };
    00173
    00174
    00175   fprintf(stderr,"terminating\n");
    00176   delete agent;
    00177   delete controller;
    00178   delete wiring;
    00179   delete robot;
    00180
    00181   return 0;
    00182 }