Robotsystem of the Robot Group Leipzig: raceground.h Source File

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

- Search for |

- File List
- File Members

# raceground.h

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