Robotsystem of the Robot Group Leipzig: main.cpp

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

- Search for |

# main.cpp

Simple example for a simulation of one spherical robot (ode_robots/simulations/template_sphererobot/main.cpp).

    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 }