Modules/TeamBallLocator/GT2005TeamBallLocator.h

Go to the documentation of this file.

/**
 * @file GT2005TeamBallLocator.h
 * 
 * The GT2005TeamBallLocator which uses the particles of the GT2005BallLocator of all
 * team mates to calculate the communicated ball position.
 *
 * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
 * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
 */
//---------------------------------------------------------------------------------------
#ifndef __GT2005TeamBallLocator_h_
#define __GT2005TeamBallLocator_h_
//---------------------------------------------------------------------------------------
#include "TeamBallLocator.h"
#include "Modules/BallLocator/GT2005Particles/GT2005ParticleContainer.h"
#include "GT2005Particles/GT2005ParticleContainerSend.h"
#include "GT2005Particles/GT2005ParticleContainerReceived.h"
#include "GT2005Particles/GT2005ParticleSend.h"
#include "Tools/Math/Vector2.h"
//---------------------------------------------------------------------------------------
/**
* @class GT2005TeamBallLocator
*
* A implementation which can be used with the GT2005BallLocator, particles are send from
* robot to robot.
*
* @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
* @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
*/
class GT2005TeamBallLocator : public TeamBallLocator
{
public:
  //Constructor:
  GT2005TeamBallLocator(const TeamBallLocatorInterfaces& interfaces);

  /** Executes the module */
  virtual void execute();

  static double smallestProb;
  static double biggestProb;
  static int numberOfCells;
  double timeFactorThreshold;

  //For every robot save the communicated particles in this container:
  GT2005ParticleContainerReceived receivingContainer;

  //For each team mate the number of frames without receiving particles of him is saved:
  int framesNothingReceived[3];

  //In this variable the odometry data of the last frame is saved:
  Pose2D lastOdometryData;

  /* The function gets a GT2005ParticleContainer with numOfBLParticles many
   * particles and transforms it into a GT2005ParticleContainer with the given
   * number of particles.
   * The field is divided into cells, how many cells depends on the number and
   * probabilities of particles. First the field is divided into 4 cells, then
   * these cells are iterative subdivided as long as necessary.
   * For this recursive data structure the design patter composity is used.
   */
  void calcRepresentativeParticleContainer(GT2005ParticleContainer& oldContainer,
                                           GT2005ParticleContainerSend& newContainer);

  /* The function performs a timeUpdate on the received particles, so that the time
   * difference due to network delay is compensated.
   */
  void timeUpdateReceived(GT2005Particle* particle, double& difference);

  /* If no new particles were received spread the saved particles and reduce the
   * probability.
   */
  void timeUpdateNotReceived(GT2005Particle* particle, int& framesNotSeen);

  double getTimeFactor(double& difference);
  //-------------------------------------------------------------------------------------
  class AbstractCell
  {
    public:
      //Pointer to the parent:
      AbstractCell* parent;
      //Pointer to the next cell in the queue:
      AbstractCell* next;
      //The first particle in this cell (which has a pointer to the next etc.):
      GT2005Particle* first;

      AbstractCell()
        : parent(0),
          first(0),
          next(0)
      {}

      virtual void getParticles(GT2005Particle** first) { }
      virtual void addParticle(GT2005Particle* particle) { }
      virtual double getSumOfProb() {return 0;}
      virtual void violentBrutalDestruction() { }

      Vector2<double> topLeft,
                      downRight;
      int depth;
  };
  //-------------------------------------------------------------------------------------
  //The grid is needed for calcRepresentativeParticleContainer(...):
  //A cell in the grid:
  class BasicCell : public AbstractCell
  {
    public:
      //The values in this cell added:
      double x;
      double y;
      double p;
      //---------------------------------------------------------------------------------
      //Constructors:
      BasicCell() { }
      BasicCell(Vector2<double> topLeft,
                Vector2<double> downRight,
                AbstractCell* parent,
                int depth)
      {
        x = y = p = 0;
        this->topLeft = topLeft;
        this->downRight = downRight;
        this->parent = parent;
        numberOfCells++;
        this->depth = depth;
        this->first = 0;
      }
      //---------------------------------------------------------------------------------
      //Calculating the averages:
      void calcAverages(int numOfParticles)
      {
        x = y = p = 0;
        if (first == 0)
          return;
        else
        {
          GT2005Particle* walker = first->next;
          for (GT2005Particle* johnny = first; !(johnny == 0); johnny = walker)
          {
            walker = johnny->next;
            x += johnny->probability * johnny->pose.x;
            y += johnny->probability * johnny->pose.y;
            p += johnny->probability;
          }

          x /= p;
          y /= p;
          p /= numOfParticles;
        }
      }
      //---------------------------------------------------------------------------------
      //calculates the representative particle and adds the particle to the list:
      void getParticles(GT2005Particle** first)
      {
        calcAverages(numOfBLParticles);

        if (*first == 0)
        {
          *first = new GT2005Particle(x, y, 0, 0, p, 0);
          (*first)->next = 0;
          return;
        }

        GT2005Particle* tmp = *first;
        *first = 0;
        *first = new GT2005Particle(x, y, 0, 0, p, 0);
        (*first)->next = tmp;
      }
      //---------------------------------------------------------------------------------
      void addParticle(GT2005Particle* particle)
      {
        if (first == 0)
        {
          first = particle;
          particle->next = 0;
          return;
        }
        GT2005Particle* tmp = first;
        first = particle;
        particle->next = tmp;
      }
      //---------------------------------------------------------------------------------
      double getSumOfProb() {return p;}
      //---------------------------------------------------------------------------------
      void violentBrutalDestruction()
      {
        if (first == 0)
          return;
        if (first->next == 0)
        {
          delete first;
          return;
        }
        GT2005Particle* nekscht = 0;
        for (GT2005Particle* walker = first; !(walker == 0); walker = nekscht)
        {
          nekscht = walker->next;
          delete walker;
        }
      }
  };
  //-------------------------------------------------------------------------------------
  class CompositeCell : public AbstractCell
  {
    public:
      AbstractCell* cellTopLeft;
      AbstractCell* cellTopRight;
      AbstractCell* cellDownLeft;
      AbstractCell* cellDownRight;
      //---------------------------------------------------------------------------------
      //Constructors:
      CompositeCell() { }
      CompositeCell(Vector2<double> topLeft,
                    Vector2<double> downRight,
                    int depth)
      {
        double widthHalf = (topLeft.y - downRight.y) / 2;
        double heightHalf = (topLeft.x - downRight.x) / 2;
        cellTopLeft = new BasicCell(topLeft,
                                    Vector2<double>(topLeft.x - heightHalf,
                                                    topLeft.y - widthHalf),
                                    this,
                                    depth + 1);
        cellTopRight = new BasicCell(Vector2<double>(topLeft.x,
                                                     topLeft.y - widthHalf),
                                     Vector2<double>(topLeft.x - heightHalf,
                                                     downRight.y),
                                     this,
                                     depth + 1);
        cellDownLeft = new BasicCell(Vector2<double>(topLeft.x - heightHalf,
                                                     topLeft.y),
                                     Vector2<double>(downRight.x,
                                                     downRight.y + widthHalf),
                                     this,
                                     depth + 1);
        cellDownRight = new BasicCell(Vector2<double>(topLeft.x - heightHalf,
                                                      topLeft.y - widthHalf),
                                      downRight,
                                      this,
                                      depth + 1);
        this->topLeft = topLeft;
        this->downRight = downRight;
        this->depth = depth;
      }
      //---------------------------------------------------------------------------------
      //The basic cell will be transformed to a composite cell:
      void split(BasicCell* bc, CompositeCell** cc)
      {
        *cc = new CompositeCell(bc->topLeft, bc->downRight, depth + 1);

        GT2005Particle* walkNext = 0;
        for (GT2005Particle* p = bc->first; !(p == 0); p = walkNext)
        {
          walkNext = p->next;
          (*cc)->addParticle(p);
        }

        if ((*cc)->cellTopLeft->getSumOfProb() < (smallestProb * bc->p))
        {
          (*cc)->cellTopLeft->violentBrutalDestruction();
          delete (*cc)->cellTopLeft;
          (*cc)->cellTopLeft = 0;
          numberOfCells--;
        }

        if ((*cc)->cellTopRight->getSumOfProb() < (smallestProb * bc->p))
        {
          (*cc)->cellTopRight->violentBrutalDestruction();
          delete (*cc)->cellTopRight;
          (*cc)->cellTopRight = 0;
          numberOfCells--;
        }

        if ((*cc)->cellDownLeft->getSumOfProb() < (smallestProb * bc->p))
        {
          (*cc)->cellDownLeft->violentBrutalDestruction();
          delete (*cc)->cellDownLeft;
          (*cc)->cellDownLeft = 0;
          numberOfCells--;
        }

        if ((*cc)->cellDownRight->getSumOfProb() < (smallestProb * bc->p))
        {
          (*cc)->cellDownRight->violentBrutalDestruction();
          delete (*cc)->cellDownRight;
          (*cc)->cellDownRight = 0;
          numberOfCells--;
        }

        if (bc == cellTopLeft)
          cellTopLeft = (*cc);
        else if (bc == cellTopRight)
          cellTopRight = (*cc);
        else if (bc == cellDownLeft)
          cellDownLeft = (*cc);
        else if (bc == cellDownRight)
          cellDownRight = (*cc);
        
        numberOfCells--;
        delete bc;
      }
      //---------------------------------------------------------------------------------
      //The function adds to the beginning of the list the particles saved in
      //the composite cell:
      void getParticles(GT2005Particle** first)
      {
        if (!(cellTopLeft == 0))
          cellTopLeft->getParticles(first);

        if (!(cellTopRight == 0))
          cellTopRight->getParticles(first);

        if (!(cellDownLeft == 0))
          cellDownLeft->getParticles(first);

        if (!(cellDownRight == 0))
          cellDownRight->getParticles(first);
      }
      //---------------------------------------------------------------------------------
      //This function puts the given particle into the cell where it belongs:
      void addParticle(GT2005Particle* particle)
      {
        int sign = -1;
        if ((cellTopLeft->downRight.y < 0) && (cellTopLeft->downRight.x > 0))
          sign = 1;
        else if ((cellTopLeft->downRight.y > 0) && (cellTopLeft->downRight.x < 0))
          sign = 1;

        int x = 0,
            y = 0;
        if ((cellTopLeft->downRight.y * sign + particle->pose.y) < 0)
          x = 1;
        if ((cellTopLeft->downRight.x * sign + particle->pose.x) < 0)
          y = 1;

        switch (x)
        {
        case 0:
          switch (y)
          {
          case 0: cellTopLeft->addParticle(particle); break;
          case 1: cellTopRight->addParticle(particle); break;
          }
          break;
        case 1:
          switch (y)
          {
          case 0: cellDownLeft->addParticle(particle); break;
          case 1: cellDownRight->addParticle(particle); break;
          }
          break;
        }
      }
      //---------------------------------------------------------------------------------
      double getSumOfProb()
      {
        double prob = 0;

        if (!(cellTopLeft == 0))
          prob += cellTopLeft->getSumOfProb();

        if (!(cellTopRight == 0))
          prob += cellTopRight->getSumOfProb();

        if (!(cellDownLeft == 0))
          prob += cellDownLeft->getSumOfProb();

        if (!(cellDownRight == 0))
          prob += cellDownRight->getSumOfProb();

        return prob;
      }
      //---------------------------------------------------------------------------------
      void violentBrutalDestruction()
      {
        if (!(cellDownLeft == 0))
        {
          cellDownLeft->violentBrutalDestruction();
          delete cellDownLeft;
        }
        if (!(cellDownRight == 0))
        {
          cellDownRight->violentBrutalDestruction();
          delete cellDownRight;
        }
        if (!(cellTopLeft == 0))
        {
          cellTopLeft->violentBrutalDestruction();
          delete cellTopLeft;
        }
        if (!(cellTopRight == 0))
        {
          cellTopRight->violentBrutalDestruction();
          delete cellTopRight;
        }
      }
  };
  //-------------------------------------------------------------------------------------
  class Queue
  {
    public:
      //Pointer to the first element in the list:
      AbstractCell* first;
      //Constructors:
      Queue() {first = 0;}
      Queue(AbstractCell* first) {this->first = first;}
      
      //Function to add a cell to the queue:
      void insertCell(AbstractCell* cell)
      {
        if (first == 0) //queue is empty
        {
          first = cell;
          return;
        }

        AbstractCell* actual = first;
        AbstractCell* tmp = 0;
        while (!(actual == 0)
               && (cell->getSumOfProb() < actual->getSumOfProb()))
        {
          tmp = actual;
          actual = actual->next;
        }
        if (tmp == 0)
        {
          cell->next = first;
          first = cell;
          return;
        }
        tmp->next = cell;
        cell->next = actual;
      }
  };
  //-------------------------------------------------------------------------------------
  bool handleMessage(InMessage& message);
  //-------------------------------------------------------------------------------------
  //Functions to draw the particles and the estimated ball position and
  //velocity:
  void drawParticle(const GT2005Particle& particle,
                    Drawings::Color color) const;
  //-------------------------------------------------------------------------------------
  void draw(const GT2005ParticleContainerReceived& container,
            const double& ballX,
            const double& ballY,
            const double& ballP,
            Drawings::Color particleColor, 
            Drawings::Color estimatedBallColor) const;
};
//---------------------------------------------------------------------------------------
#endif

---