Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/TeamBallLocator/GT2005TeamBallLocator.h

Go to the documentation of this file.

    00001 /**
    00002  * @file GT2005TeamBallLocator.h
    00003  *
    00004  * The GT2005TeamBallLocator which uses the particles of the GT2005BallLocator of all
    00005  * team mates to calculate the communicated ball position.
    00006  *
    00007  * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
    00008  * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
    00009  */
    00010 //---------------------------------------------------------------------------------------
    00011 #ifndef __GT2005TeamBallLocator_h_
    00012 #define __GT2005TeamBallLocator_h_
    00013 //---------------------------------------------------------------------------------------
    00014 #include "TeamBallLocator.h"
    00015 #include "Modules/BallLocator/GT2005Particles/GT2005ParticleContainer.h"
    00016 #include "GT2005Particles/GT2005ParticleContainerSend.h"
    00017 #include "GT2005Particles/GT2005ParticleContainerReceived.h"
    00018 #include "GT2005Particles/GT2005ParticleSend.h"
    00019 #include "Tools/Math/Vector2.h"
    00020 //---------------------------------------------------------------------------------------
    00021 /**
    00022 * @class GT2005TeamBallLocator
    00023 *
    00024 * A implementation which can be used with the GT2005BallLocator, particles are send from
    00025 * robot to robot.
    00026 *
    00027 * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
    00028 * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
    00029 */
    00030 class GT2005TeamBallLocator : public TeamBallLocator
    00031 {
    00032 public:
    00033   //Constructor:
    00034   GT2005TeamBallLocator(const TeamBallLocatorInterfaces& interfaces);
    00035
    00036   /** Executes the module */
    00037   virtual void execute();
    00038
    00039   static double smallestProb;
    00040   static double biggestProb;
    00041   static int numberOfCells;
    00042   double timeFactorThreshold;
    00043
    00044   //For every robot save the communicated particles in this container:
    00045   GT2005ParticleContainerReceived receivingContainer;
    00046
    00047   //For each team mate the number of frames without receiving particles of him is saved:
    00048   int framesNothingReceived[3];
    00049
    00050   //In this variable the odometry data of the last frame is saved:
    00051   Pose2D lastOdometryData;
    00052
    00053   /* The function gets a GT2005ParticleContainer with numOfBLParticles many
    00054    * particles and transforms it into a GT2005ParticleContainer with the given
    00055    * number of particles.
    00056    * The field is divided into cells, how many cells depends on the number and
    00057    * probabilities of particles. First the field is divided into 4 cells, then
    00058    * these cells are iterative subdivided as long as necessary.
    00059    * For this recursive data structure the design patter composity is used.
    00060    */
    00061   void calcRepresentativeParticleContainer(GT2005ParticleContainer& oldContainer,
    00062                                            GT2005ParticleContainerSend& newContainer);
    00063
    00064   /* The function performs a timeUpdate on the received particles, so that the time
    00065    * difference due to network delay is compensated.
    00066    */
    00067   void timeUpdateReceived(GT2005Particle* particle, double& difference);
    00068
    00069   /* If no new particles were received spread the saved particles and reduce the
    00070    * probability.
    00071    */
    00072   void timeUpdateNotReceived(GT2005Particle* particle, int& framesNotSeen);
    00073
    00074   double getTimeFactor(double& difference);
    00075   //-------------------------------------------------------------------------------------
    00076   class AbstractCell
    00077   {
    00078     public:
    00079       //Pointer to the parent:
    00080       AbstractCell* parent;
    00081       //Pointer to the next cell in the queue:
    00082       AbstractCell* next;
    00083       //The first particle in this cell (which has a pointer to the next etc.):
    00084       GT2005Particle* first;
    00085
    00086       AbstractCell()
    00087         : parent(0),
    00088           first(0),
    00089           next(0)
    00090       {}
    00091
    00092       virtual void getParticles(GT2005Particle** first) { }
    00093       virtual void addParticle(GT2005Particle* particle) { }
    00094       virtual double getSumOfProb() {return 0;}
    00095       virtual void violentBrutalDestruction() { }
    00096
    00097       Vector2<double> topLeft,
    00098                       downRight;
    00099       int depth;
    00100   };
    00101   //-------------------------------------------------------------------------------------
    00102   //The grid is needed for calcRepresentativeParticleContainer(...):
    00103   //A cell in the grid:
    00104   class BasicCell : public AbstractCell
    00105   {
    00106     public:
    00107       //The values in this cell added:
    00108       double x;
    00109       double y;
    00110       double p;
    00111       //---------------------------------------------------------------------------------
    00112       //Constructors:
    00113       BasicCell() { }
    00114       BasicCell(Vector2<double> topLeft,
    00115                 Vector2<double> downRight,
    00116                 AbstractCell* parent,
    00117                 int depth)
    00118       {
    00119         x = y = p = 0;
    00120         this->topLeft = topLeft;
    00121         this->downRight = downRight;
    00122         this->parent = parent;
    00123         numberOfCells++;
    00124         this->depth = depth;
    00125         this->first = 0;
    00126       }
    00127       //---------------------------------------------------------------------------------
    00128       //Calculating the averages:
    00129       void calcAverages(int numOfParticles)
    00130       {
    00131         x = y = p = 0;
    00132         if (first == 0)
    00133           return;
    00134         else
    00135         {
    00136           GT2005Particle* walker = first->next;
    00137           for (GT2005Particle* johnny = first; !(johnny == 0); johnny = walker)
    00138           {
    00139             walker = johnny->next;
    00140             x += johnny->probability * johnny->pose.x;
    00141             y += johnny->probability * johnny->pose.y;
    00142             p += johnny->probability;
    00143           }
    00144
    00145           x /= p;
    00146           y /= p;
    00147           p /= numOfParticles;
    00148         }
    00149       }
    00150       //---------------------------------------------------------------------------------
    00151       //calculates the representative particle and adds the particle to the list:
    00152       void getParticles(GT2005Particle** first)
    00153       {
    00154         calcAverages(numOfBLParticles);
    00155
    00156         if (*first == 0)
    00157         {
    00158           *first = new GT2005Particle(x, y, 0, 0, p, 0);
    00159           (*first)->next = 0;
    00160           return;
    00161         }
    00162
    00163         GT2005Particle* tmp = *first;
    00164         *first = 0;
    00165         *first = new GT2005Particle(x, y, 0, 0, p, 0);
    00166         (*first)->next = tmp;
    00167       }
    00168       //---------------------------------------------------------------------------------
    00169       void addParticle(GT2005Particle* particle)
    00170       {
    00171         if (first == 0)
    00172         {
    00173           first = particle;
    00174           particle->next = 0;
    00175           return;
    00176         }
    00177         GT2005Particle* tmp = first;
    00178         first = particle;
    00179         particle->next = tmp;
    00180       }
    00181       //---------------------------------------------------------------------------------
    00182       double getSumOfProb() {return p;}
    00183       //---------------------------------------------------------------------------------
    00184       void violentBrutalDestruction()
    00185       {
    00186         if (first == 0)
    00187           return;
    00188         if (first->next == 0)
    00189         {
    00190           delete first;
    00191           return;
    00192         }
    00193         GT2005Particle* nekscht = 0;
    00194         for (GT2005Particle* walker = first; !(walker == 0); walker = nekscht)
    00195         {
    00196           nekscht = walker->next;
    00197           delete walker;
    00198         }
    00199       }
    00200   };
    00201   //-------------------------------------------------------------------------------------
    00202   class CompositeCell : public AbstractCell
    00203   {
    00204     public:
    00205       AbstractCell* cellTopLeft;
    00206       AbstractCell* cellTopRight;
    00207       AbstractCell* cellDownLeft;
    00208       AbstractCell* cellDownRight;
    00209       //---------------------------------------------------------------------------------
    00210       //Constructors:
    00211       CompositeCell() { }
    00212       CompositeCell(Vector2<double> topLeft,
    00213                     Vector2<double> downRight,
    00214                     int depth)
    00215       {
    00216         double widthHalf = (topLeft.y - downRight.y) / 2;
    00217         double heightHalf = (topLeft.x - downRight.x) / 2;
    00218         cellTopLeft = new BasicCell(topLeft,
    00219                                     Vector2<double>(topLeft.x - heightHalf,
    00220                                                     topLeft.y - widthHalf),
    00221                                     this,
    00222                                     depth + 1);
    00223         cellTopRight = new BasicCell(Vector2<double>(topLeft.x,
    00224                                                      topLeft.y - widthHalf),
    00225                                      Vector2<double>(topLeft.x - heightHalf,
    00226                                                      downRight.y),
    00227                                      this,
    00228                                      depth + 1);
    00229         cellDownLeft = new BasicCell(Vector2<double>(topLeft.x - heightHalf,
    00230                                                      topLeft.y),
    00231                                      Vector2<double>(downRight.x,
    00232                                                      downRight.y + widthHalf),
    00233                                      this,
    00234                                      depth + 1);
    00235         cellDownRight = new BasicCell(Vector2<double>(topLeft.x - heightHalf,
    00236                                                       topLeft.y - widthHalf),
    00237                                       downRight,
    00238                                       this,
    00239                                       depth + 1);
    00240         this->topLeft = topLeft;
    00241         this->downRight = downRight;
    00242         this->depth = depth;
    00243       }
    00244       //---------------------------------------------------------------------------------
    00245       //The basic cell will be transformed to a composite cell:
    00246       void split(BasicCell* bc, CompositeCell** cc)
    00247       {
    00248         *cc = new CompositeCell(bc->topLeft, bc->downRight, depth + 1);
    00249
    00250         GT2005Particle* walkNext = 0;
    00251         for (GT2005Particle* p = bc->first; !(p == 0); p = walkNext)
    00252         {
    00253           walkNext = p->next;
    00254           (*cc)->addParticle(p);
    00255         }
    00256
    00257         if ((*cc)->cellTopLeft->getSumOfProb() < (smallestProb * bc->p))
    00258         {
    00259           (*cc)->cellTopLeft->violentBrutalDestruction();
    00260           delete (*cc)->cellTopLeft;
    00261           (*cc)->cellTopLeft = 0;
    00262           numberOfCells--;
    00263         }
    00264
    00265         if ((*cc)->cellTopRight->getSumOfProb() < (smallestProb * bc->p))
    00266         {
    00267           (*cc)->cellTopRight->violentBrutalDestruction();
    00268           delete (*cc)->cellTopRight;
    00269           (*cc)->cellTopRight = 0;
    00270           numberOfCells--;
    00271         }
    00272
    00273         if ((*cc)->cellDownLeft->getSumOfProb() < (smallestProb * bc->p))
    00274         {
    00275           (*cc)->cellDownLeft->violentBrutalDestruction();
    00276           delete (*cc)->cellDownLeft;
    00277           (*cc)->cellDownLeft = 0;
    00278           numberOfCells--;
    00279         }
    00280
    00281         if ((*cc)->cellDownRight->getSumOfProb() < (smallestProb * bc->p))
    00282         {
    00283           (*cc)->cellDownRight->violentBrutalDestruction();
    00284           delete (*cc)->cellDownRight;
    00285           (*cc)->cellDownRight = 0;
    00286           numberOfCells--;
    00287         }
    00288
    00289         if (bc == cellTopLeft)
    00290           cellTopLeft = (*cc);
    00291         else if (bc == cellTopRight)
    00292           cellTopRight = (*cc);
    00293         else if (bc == cellDownLeft)
    00294           cellDownLeft = (*cc);
    00295         else if (bc == cellDownRight)
    00296           cellDownRight = (*cc);
    00297
    00298         numberOfCells--;
    00299         delete bc;
    00300       }
    00301       //---------------------------------------------------------------------------------
    00302       //The function adds to the beginning of the list the particles saved in
    00303       //the composite cell:
    00304       void getParticles(GT2005Particle** first)
    00305       {
    00306         if (!(cellTopLeft == 0))
    00307           cellTopLeft->getParticles(first);
    00308
    00309         if (!(cellTopRight == 0))
    00310           cellTopRight->getParticles(first);
    00311
    00312         if (!(cellDownLeft == 0))
    00313           cellDownLeft->getParticles(first);
    00314
    00315         if (!(cellDownRight == 0))
    00316           cellDownRight->getParticles(first);
    00317       }
    00318       //---------------------------------------------------------------------------------
    00319       //This function puts the given particle into the cell where it belongs:
    00320       void addParticle(GT2005Particle* particle)
    00321       {
    00322         int sign = -1;
    00323         if ((cellTopLeft->downRight.y < 0) && (cellTopLeft->downRight.x > 0))
    00324           sign = 1;
    00325         else if ((cellTopLeft->downRight.y > 0) && (cellTopLeft->downRight.x < 0))
    00326           sign = 1;
    00327
    00328         int x = 0,
    00329             y = 0;
    00330         if ((cellTopLeft->downRight.y * sign + particle->pose.y) < 0)
    00331           x = 1;
    00332         if ((cellTopLeft->downRight.x * sign + particle->pose.x) < 0)
    00333           y = 1;
    00334
    00335         switch (x)
    00336         {
    00337         case 0:
    00338           switch (y)
    00339           {
    00340           case 0: cellTopLeft->addParticle(particle); break;
    00341           case 1: cellTopRight->addParticle(particle); break;
    00342           }
    00343           break;
    00344         case 1:
    00345           switch (y)
    00346           {
    00347           case 0: cellDownLeft->addParticle(particle); break;
    00348           case 1: cellDownRight->addParticle(particle); break;
    00349           }
    00350           break;
    00351         }
    00352       }
    00353       //---------------------------------------------------------------------------------
    00354       double getSumOfProb()
    00355       {
    00356         double prob = 0;
    00357
    00358         if (!(cellTopLeft == 0))
    00359           prob += cellTopLeft->getSumOfProb();
    00360
    00361         if (!(cellTopRight == 0))
    00362           prob += cellTopRight->getSumOfProb();
    00363
    00364         if (!(cellDownLeft == 0))
    00365           prob += cellDownLeft->getSumOfProb();
    00366
    00367         if (!(cellDownRight == 0))
    00368           prob += cellDownRight->getSumOfProb();
    00369
    00370         return prob;
    00371       }
    00372       //---------------------------------------------------------------------------------
    00373       void violentBrutalDestruction()
    00374       {
    00375         if (!(cellDownLeft == 0))
    00376         {
    00377           cellDownLeft->violentBrutalDestruction();
    00378           delete cellDownLeft;
    00379         }
    00380         if (!(cellDownRight == 0))
    00381         {
    00382           cellDownRight->violentBrutalDestruction();
    00383           delete cellDownRight;
    00384         }
    00385         if (!(cellTopLeft == 0))
    00386         {
    00387           cellTopLeft->violentBrutalDestruction();
    00388           delete cellTopLeft;
    00389         }
    00390         if (!(cellTopRight == 0))
    00391         {
    00392           cellTopRight->violentBrutalDestruction();
    00393           delete cellTopRight;
    00394         }
    00395       }
    00396   };
    00397   //-------------------------------------------------------------------------------------
    00398   class Queue
    00399   {
    00400     public:
    00401       //Pointer to the first element in the list:
    00402       AbstractCell* first;
    00403       //Constructors:
    00404       Queue() {first = 0;}
    00405       Queue(AbstractCell* first) {this->first = first;}
    00406
    00407       //Function to add a cell to the queue:
    00408       void insertCell(AbstractCell* cell)
    00409       {
    00410         if (first == 0) //queue is empty
    00411         {
    00412           first = cell;
    00413           return;
    00414         }
    00415
    00416         AbstractCell* actual = first;
    00417         AbstractCell* tmp = 0;
    00418         while (!(actual == 0)
    00419                && (cell->getSumOfProb() < actual->getSumOfProb()))
    00420         {
    00421           tmp = actual;
    00422           actual = actual->next;
    00423         }
    00424         if (tmp == 0)
    00425         {
    00426           cell->next = first;
    00427           first = cell;
    00428           return;
    00429         }
    00430         tmp->next = cell;
    00431         cell->next = actual;
    00432       }
    00433   };
    00434   //-------------------------------------------------------------------------------------
    00435   bool handleMessage(InMessage& message);
    00436   //-------------------------------------------------------------------------------------
    00437   //Functions to draw the particles and the estimated ball position and
    00438   //velocity:
    00439   void drawParticle(const GT2005Particle& particle,
    00440                     Drawings::Color color) const;
    00441   //-------------------------------------------------------------------------------------
    00442   void draw(const GT2005ParticleContainerReceived& container,
    00443             const double& ballX,
    00444             const double& ballY,
    00445             const double& ballP,
    00446             Drawings::Color particleColor,
    00447             Drawings::Color estimatedBallColor) const;
    00448 };
    00449 //---------------------------------------------------------------------------------------
    00450 #endif