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

# raceground.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 __RACEGROUND_H
    00025 #define __RACEGROUND_H
    00026
    00027
    00028 #include <stdio.h>
    00029 #include <cmath>
    00030 #include <list>
    00031
    00032 #include "abstractobstacle.h"
    00033 #include <drawstuff/drawstuff.h>
    00034 #include "stl_adds.h"
    00035 #include "matrix.h"
    00036 using namespace matrix;
    00037 #include "mathutils.h"
    00038 #include "abstracttracksection.h"
    00039 #include "straightline.h"
    00040 #include "degreesegment.h"
    00041
    00042
    00043 //Fixme: raceground creates collisions with ground and itself
    00044 class RaceGround : public AbstractObstacle {
    00045
    00046   // the list that contains all segments
    00047   list<AbstractTrackSection*> SegmentList;
    00048
    00049
    00050   bool obstacle_exists;
    00051
    00052  public:
    00053
    00054   RaceGround(const OdeHandle& odehandle):
    00055     AbstractObstacle(odehandle) {
    00056     setParameters(pose);
    00057   };
    00058
    00059
    00060   RaceGround(const OdeHandle& odehandle,const Matrix& pose):
    00061     AbstractObstacle(odehandle) {
    00062     setParameters(pose);
    00063   };
    00064
    00065   RaceGround(const OdeHandle& odehandle,const Position& pos, double angle):
    00066     AbstractObstacle(odehandle) {
    00067     setParameters(getTranslationRotationMatrix(pos,angle));
    00068  };
    00069
    00070
    00071   /**
    00072    * Destructor
    00073    */
    00074   ~RaceGround() {}
    00075
    00076   /**
    00077    * you set the number of segments of the track
    00078    */
    00079   void setNumberOfSegments(int number) {
    00080     numberOfBarcodes=number;
    00081   }
    00082
    00083   /**
    00084    * returns the barcode number of the given point
    00085    * returns (length,width) (-1,-1) if point is not on the track
    00086    */
    00087    pair<double, double> getPositionOnTrack(const Position& p) {
    00088
    00089     double passedLength=0.0f;
    00090     double segmentNumber=-1.0f;
    00091     double width        =-1.0f;
    00092     for(list<AbstractTrackSection*>::iterator it = SegmentList.begin();
    00093         it!= SegmentList.end(); ++it) {
    00094       if((*it)->isInside(p)){
    00095         double sectionLength = (*it)->getSectionIdValue(p); // between 0..length
    00096         width = (*it)->getWidthIdValue(p);
    00097         if (sectionLength<0.0f ) { // weird isegment is found
    00098           printf("Weird! We should be in the segment!\n");
    00099         }
    00100         segmentNumber=numberOfBarcodes*
    00101           (passedLength+sectionLength)/trackLength;
    00102         return pair<double, double> (segmentNumber, width);
    00103       }
    00104       passedLength+=(*it)->getLength();
    00105     }
    00106     return pair<double, double> (segmentNumber, width);
    00107   }
    00108
    00109   /**
    00110    * adds the segments in the list to the SegmentList
    00111    */
    00112   void addSegments(list<AbstractTrackSection*> listToAdd) {
    00113     SegmentList+=listToAdd;
    00114   }
    00115
    00116   /**
    00117    * adds the segment to the SegmentList
    00118    */
    00119   void addSegment(AbstractTrackSection* Segment) {
    00120     SegmentList+=Segment;
    00121   }
    00122
    00123
    00124   /**
    00125    * adds the named segment to the SegmentList
    00126    * names are:
    00127    * straightline: StraightLine
    00128    * 90degree    : DegreeSegment
    00129    */
    00130   void addSegment(string& name) {
    00131     // get first pose from last stored segment
    00132     Matrix newPose(pose); // this is the initial pose
    00133     if (!SegmentList.empty()) {
    00134       Matrix pos = SegmentList.back()->getPoseMatrix();
    00135       Matrix end = SegmentList.back()->getTransformedEndMatrix();
    00136       newPose=pos * end;
    00137     }
    00138
    00139     if (name=="straightline") {
    00140       AbstractTrackSection* segment = new StraightLine(newPose);
    00141       SegmentList += segment;
    00142       trackLength+=segment->getLength();
    00143     } else if (name.find("degree")==0) {
    00144       DegreeSegment* segment = new DegreeSegment(newPose);
    00145       SegmentList+=(AbstractTrackSection*) segment;
    00146       // now get the angle and the radius
    00147       char* d = (char*)malloc(name.length()*sizeof(char));
    00148       double angle, radius;
    00149       if (sscanf(name.c_str(),"%s %lf %lf",d,&angle,&radius)!=3)
    00150         std::cout << "parameter parsing invalid!: " << name << "\n";
    00151       else {
    00152         std::cout << "parameters " << d << "," << angle << " " << radius << "\n";
    00153         // parsing was ok
    00154         segment->setCurveAngle(angle/180.0*M_PI);
    00155         segment->setRadius(radius);
    00156         trackLength+=segment->getLength();
    00157       }
    00158       free(d);
    00159     }
    00160   }
    00161
    00162   /**
    00163    * adds the named segments in the list to the SegmentList
    00164    * names are:
    00165    * straightline: StraightLine
    00166    * 90degree    : 90DegreeSegment
    00167    */
    00168   void addSegments(list<string> names) {
    00169     for(list<string>::iterator it = names.begin(); it!=names.end(); ++it) {
    00170       addSegment(*it);
    00171     }
    00172   }
    00173
    00174 /**
    00175    * draws all the segments stored in SegmentList
    00176    */
    00177   virtual void draw(){
    00178     //    double box[3];
    00179     dsSetTexture (DS_NONE);
    00180     dsSetColor (color.r, color.g, color.b);
    00181     for(list<AbstractTrackSection*>::iterator it = SegmentList.begin();
    00182                                                         it!= SegmentList.end(); ++it) {
    00183       // call the create function of the segment
    00184       (*it)->draw();
    00185     }
    00186  };
    00187
    00188
    00189   virtual void setPosition(double x, double y, double z){
    00190     if (obstacle_exists){
    00191       destroy();
    00192     }
    00193     create();
    00194   };
    00195
    00196   virtual void getPosition(double& x, double& y, double& z){
    00197     x=0.0f;
    00198     y=0.0f;
    00199     z=0.0f;
    00200   };
    00201
    00202
    00203   // normally we don't need this function
    00204   virtual void setGeometry(double length_, double width_, double height_){
    00205     length=length_;
    00206     width=width_;
    00207     height =height_;
    00208   };
    00209
    00210   virtual void setColor(double r, double g, double b){
    00211     color.r=r;
    00212     color.g=g;
    00213     color.b=b;
    00214   };
    00215
    00216
    00217  protected:
    00218   double length;
    00219   double trackLength;
    00220   double width;
    00221   double height;
    00222   Matrix pose;
    00223   double numberOfBarcodes;
    00224
    00225   dSpaceID raceground_space;
    00226
    00227   virtual void setParameters(const Matrix& initpose) {
    00228     pose=initpose;
    00229     obstacle_exists=false;
    00230     setColor(226 / 255.0, 103 / 255.0, 66 / 255.0);
    00231     numberOfBarcodes=256.0f;
    00232     trackLength=0.0;
    00233   }
    00234
    00235   virtual void create(){
    00236     // create raceground space and add it to the top level space
    00237     raceground_space = space;
    00238         //raceground_space = dSimpleSpaceCreate (space);
    00239     //     dSpaceSetCleanup (raceground_space,0);
    00240     // todo:
    00241     // go through all list elements
    00242     // draw them all by there own draw function
    00243     for(list<AbstractTrackSection*>::iterator it = SegmentList.begin();it!= SegmentList.end(); ++it) {
    00244       // call the create function of the segment
    00245       (*it)->create(raceground_space);
    00246     }
    00247   };
    00248
    00249
    00250   virtual void destroy(){
    00251     for(list<AbstractTrackSection*>::iterator it = SegmentList.begin();it!= SegmentList.end(); ++it) {
    00252       // call the create function of the segment
    00253       (*it)->destroy();
    00254     }
    00255     obstacle_exists=false;
    00256   };
    00257
    00258 };
    00259
    00260 #endif