Robot Simulator of the Robotics Group for Self-Organization of Control: integration/main.cpp

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