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

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