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

# Modules/TeamBallLocator/GT2005TeamBallLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005TeamBallLocator.cpp
    00003 *
    00004 * Implementation of class GT2005TeamBallLocator
    00005 *
    00006 * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
    00007 * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
    00008 */
    00009 //---------------------------------------------------------------------------------------
    00010 #include "GT2005TeamBallLocator.h"
    00011 #include "Platform/SystemCall.h"
    00012 #include "Tools/Debugging/GenericDebugData.h"
    00013 #include "Tools/Math/Matrix2x2.h"
    00014 #include "Modules/BallLocator/GT2005BallLocatorParameters.h"
    00015 //---------------------------------------------------------------------------------------
    00016 double GT2005TeamBallLocator::smallestProb = 0.05;
    00017 double GT2005TeamBallLocator::biggestProb = 0.10;
    00018 int GT2005TeamBallLocator::numberOfCells = 0;
    00019 double maxDistTimes = 5;
    00020 double decreaseProbValue = 0.9;
    00021 //---------------------------------------------------------------------------------------
    00022 GT2005TeamBallLocator::GT2005TeamBallLocator(const TeamBallLocatorInterfaces& interfaces)
    00023 : TeamBallLocator(interfaces)
    00024 {
    00025   lastOdometryData = odometryData;
    00026   timeFactorThreshold = 8;
    00027
    00028   communicatedBallState.useGivenProbabilities = true;
    00029
    00030   framesNothingReceived[0] = 0;
    00031   framesNothingReceived[1] = 0;
    00032   framesNothingReceived[2] = 0;
    00033 }
    00034 //---------------------------------------------------------------------------------------
    00035 void GT2005TeamBallLocator::execute()
    00036 {
    00037   MODIFY("TeamBallLocator:parameters", maxDistTimes);
    00038   MODIFY("TeamBallLocator:parameters", decreaseProbValue);
    00039
    00040   lastOdometryData = odometryData;
    00041   int i, j;
    00042
    00043   //Send particles: (only if not penalized)
    00044   GT2005ParticleContainerSend sendingContainer;
    00045   if (gameControlData.getRobot().penalty == GameControlData::notPenalized)
    00046   {
    00047     calcRepresentativeParticleContainer(
    00048       ballLocatorSamples, //Particles of the GT2005BallLocator
    00049       sendingContainer);
    00050     DEBUG_RESPONSE_NOT("TeamBallLocator:doNotsendParticles",
    00051                        teamMessageCollection.send(sendingContainer););
    00052   }
    00053
    00054   //GT2005ParticleContainerReceived receivingContainer(4*numOfBLParticlesSend);
    00055   //Receive particles:
    00056   double currentSystemTime = (double) SystemCall::getCurrentSystemTime();
    00057   bool receivedFromRobot[3] = {false, false, false};
    00058   DEBUG_RESPONSE_NOT
    00059   ( "TeamBallLocator:Put particles to point x=500 y=0",
    00060     for (i = 0; i < teamMessageCollection.numberOfTeamMessages; i++)
    00061       if (teamMessageCollection[i].isActual())
    00062       {
    00063         int times = 0;
    00064         switch(teamMessageCollection[i].playerNumberOfSender)
    00065         {
    00066         case Player::one:   times = 0; break;
    00067         case Player::two:   times = 1; break;
    00068         case Player::three: times = 2; break;
    00069         case Player::four:  times = 3; break;
    00070         }
    00071         int sender = times > getPlayer().getPlayerNumber() ? times - 1 : times;
    00072
    00073         double difference =   currentSystemTime
    00074                             - teamMessageCollection[i].getTimeStampInOwnTime();
    00075
    00076         int numberOfSamples
    00077           = teamMessageCollection[i].gt2005ParticleContainerSend.getNumberOfSamples();
    00078
    00079         GT2005ParticleContainerReceived tmpReceivingContainer(numberOfSamples);
    00080         bool receivedInformation = false;
    00081         for (int k = 0; k < numberOfSamples; k++)
    00082         {
    00083           if (((double)(teamMessageCollection[i].gt2005ParticleContainerSend[k].probability))
    00084               / 10000 < 0.005)
    00085           {
    00086             receivedInformation = true;
    00087           }
    00088         }
    00089
    00090         if (receivedInformation)
    00091           for (j = 0; j < numberOfSamples; j++)
    00092           {
    00093             receivingContainer.current[(int)(j + times*numberOfSamples)].setParameters
    00094               (teamMessageCollection[i].gt2005ParticleContainerSend[j].x,
    00095               teamMessageCollection[i].gt2005ParticleContainerSend[j].y,
    00096               teamMessageCollection[i].gt2005ParticleContainerSend[j].vx,
    00097               teamMessageCollection[i].gt2005ParticleContainerSend[j].vy,
    00098               (double)(teamMessageCollection[i].gt2005ParticleContainerSend[j].probability) / 10000,
    00099               (double)(teamMessageCollection[i].gt2005ParticleContainerSend[j].vprob) / 10000);
    00100             timeUpdateReceived
    00101               (&receivingContainer.current[(int)(j + times*numberOfSamples)], difference);
    00102             receivedFromRobot[sender] = true;
    00103             framesNothingReceived[sender] = 0;
    00104           }
    00105         else
    00106           receivedFromRobot[sender] = false;
    00107       }
    00108
    00109     /* For particles belonging to team mates of which no particles were received
    00110      * make a special timeUpdate (this peace of code is not nice, sorry...):
    00111      */
    00112     for (int sender = 0; sender < 3; sender++)
    00113       if (!receivedFromRobot[sender])
    00114       {
    00115         /* To make spreading more efficient, resample particles to really use
    00116          * the maximum number of particles.
    00117          */
    00118         //Get the number of unused particles and put them to the end:
    00119         int numberOfUnusedParticles = 0;
    00120         int senderRobotNumber = sender > getPlayer().getPlayerNumber() - 1 ? sender + 1 : sender;
    00121         for (j = 0; j < numOfBLParticlesSend - numberOfUnusedParticles; j++)
    00122         {
    00123           if (receivingContainer.current[j + senderRobotNumber*numOfBLParticlesSend].probability == 0)
    00124           {
    00125             //last particle not already treated:
    00126             GT2005Particle& p = receivingContainer.current[  numOfBLParticlesSend - 1
    00127                                                            + senderRobotNumber*numOfBLParticlesSend
    00128                                                            - numberOfUnusedParticles];
    00129             receivingContainer.current[j + senderRobotNumber*numOfBLParticlesSend]
    00130               .setParameters(p.pose.x, p.pose.y, p.vx, p.vy, p.probability, p.vprob);
    00131             p.setParameters(0, 0, 0, 0, 0, 0);
    00132             numberOfUnusedParticles++;
    00133             j--;
    00134           }
    00135         }
    00136
    00137         //If there is no used particle do nothing:
    00138         if (numberOfUnusedParticles == numOfBLParticlesSend)
    00139           continue;
    00140
    00141         /* Resample the particles with highest probability until no unused
    00142         * particle is left.
    00143         */
    00144         for (j = 0; j < numberOfUnusedParticles; j++)
    00145         {
    00146           //Find particle with highest probability:
    00147           GT2005Particle& bigProbParticle
    00148             = receivingContainer.current[senderRobotNumber*numOfBLParticlesSend];
    00149           for (int k = 1; k < numOfBLParticlesSend; k++)
    00150           {
    00151             if (bigProbParticle.probability
    00152               < receivingContainer.current[k + senderRobotNumber*numOfBLParticlesSend].probability)
    00153             {
    00154               bigProbParticle
    00155                 = receivingContainer.current[k + senderRobotNumber*numOfBLParticlesSend];
    00156             }
    00157           }
    00158           //Resample this particle if the probability is not too low:
    00159           if (bigProbParticle.probability > 0.04)
    00160           {
    00161             receivingContainer
    00162               .current[numOfBLParticlesSend - 1 + senderRobotNumber*numOfBLParticlesSend - j]
    00163                 .setParameters(bigProbParticle.pose.x,
    00164                               bigProbParticle.pose.y,
    00165                               bigProbParticle.vx,
    00166                               bigProbParticle.vy,
    00167                               bigProbParticle.probability / 2,
    00168                               bigProbParticle.vprob);
    00169             bigProbParticle.probability /= 2;
    00170           }
    00171           else
    00172             break;
    00173         }
    00174         framesNothingReceived[sender]++;
    00175         for (j = 0; j < numOfBLParticlesSend; j++)
    00176           timeUpdateNotReceived
    00177             (&receivingContainer.current[j + senderRobotNumber*numOfBLParticlesSend],
    00178             framesNothingReceived[sender]);
    00179       }
    00180   );
    00181
    00182   DEBUG_RESPONSE
    00183   ( "TeamBallLocator:Put particles to point x=500 y=0",
    00184     for (int i = 0; i < receivingContainer.getNumberOfSamples(); i++)
    00185       receivingContainer.current[i].setParameters(500, 0, 0, 0, 0.5, 0);
    00186   );
    00187
    00188
    00189   //Copy own particles into the receivingContainer:
    00190   int times = 0;
    00191   if (gameControlData.getRobot().penalty == GameControlData::notPenalized)
    00192   {
    00193     switch (getPlayer().getPlayerNumber())
    00194     {
    00195     case Player::one:   times = 0; break;
    00196     case Player::two:   times = 1; break;
    00197     case Player::three: times = 2; break;
    00198     case Player::four:  times = 3; break;
    00199     }
    00200     for (j = 0; j < sendingContainer.getNumberOfSamples(); j++)
    00201     {
    00202       receivingContainer.current
    00203         [(int)(j + times*sendingContainer.getNumberOfSamples())]
    00204           .setParameters(
    00205             sendingContainer[j].x,
    00206             sendingContainer[j].y,
    00207             sendingContainer[j].vx,
    00208             sendingContainer[j].vy,
    00209             (double)(sendingContainer[j].probability) / 10000,
    00210             (double)(sendingContainer[j].vprob) / 10000);
    00211     }
    00212   }
    00213
    00214   double ballX, ballY, ballP;
    00215   receivingContainer.calculateEstimatedBallPosition(ballX, ballY, ballP);
    00216
    00217   /* If the ball was seen by this robot or the communicated ball position
    00218    * has a high validity (mostly when other team mates have seen the ball)
    00219    * set timeWhenLastObserved at current system time.
    00220    */
    00221   if (ballP > 0.01)
    00222   {
    00223     communicatedBallState.timeWhenLastObserved = SystemCall::getCurrentSystemTime();
    00224     communicatedBallState.setBallDataRelativeToField(robotPose,
    00225                                                      ballX, ballY,
    00226                                                      0, 0);
    00227   }
    00228
    00229   communicatedBallState.positionProb = ballP;
    00230   communicatedBallState.velocityProb = 0;
    00231
    00232   draw(receivingContainer,   //the container with the particles which should be drawed
    00233        ballX,                //the x-position of the estimated ball
    00234        ballY,                //the y-position of the estimated ball
    00235        ballP,                //the probability of the estimated ball position
    00236        Drawings::light_gray, //the color of the particles
    00237        Drawings::pink);      //the color of the estimated ball
    00238 }
    00239 //---------------------------------------------------------------------------------------
    00240 void GT2005TeamBallLocator::calcRepresentativeParticleContainer(
    00241   GT2005ParticleContainer& oldContainer,
    00242   GT2005ParticleContainerSend& newContainer)
    00243 {
    00244   int i;
    00245   //root composite cell, the field is divided into 4 cells:
    00246   CompositeCell compositeCell(Vector2<double>(xPosOpponentGroundline,
    00247                                               yPosLeftSideline),
    00248                               Vector2<double>(xPosOwnGroundline,
    00249                                               yPosRightSideline),
    00250                               0);
    00251   numberOfCells = 4;
    00252   //add all particles to the composite cell:
    00253   if (robotPoseCollection[0].getValidity() == 0)
    00254   {
    00255     for (i = 0; i < oldContainer.getNumberOfSamples(); i++)
    00256     {
    00257       Vector2<double> position = Geometry::relative2FieldCoord(robotPose,
    00258                                                                oldContainer[i].pose.x,
    00259                                                                oldContainer[i].pose.y);
    00260       GT2005Particle *particle = new GT2005Particle(position.x, position.y,
    00261                                                     0, 0,
    00262                                                     oldContainer[i].probability, 0);
    00263       compositeCell.addParticle(particle);
    00264     }
    00265   }
    00266   else
    00267   {
    00268     //get the validities of the robot poses and normalize them:
    00269     Vector3<double> validities;
    00270     if (robotPoseCollection.poses[0].getValidity() > 0)
    00271       validities.x = robotPoseCollection.poses[0].getValidity();
    00272     else
    00273       validities.x = 0;
    00274     if (robotPoseCollection.poses[1].getValidity() > 0)
    00275       validities.y = robotPoseCollection.poses[1].getValidity();
    00276     else
    00277       validities.y = 0;
    00278     if (robotPoseCollection.poses[2].getValidity() > 0)
    00279       validities.z = robotPoseCollection.poses[2].getValidity();
    00280     else
    00281       validities.z = 0;
    00282     validities.normalize();
    00283     double robotPoseValidity = max(0.1, robotPose.getValidity());
    00284
    00285     for (i = 0; i < oldContainer.getNumberOfSamples(); i++)
    00286     {
    00287       //throw particles for all robot poses:
    00288       { //first robot pose
    00289         Vector2<double> position = Geometry::relative2FieldCoord(
    00290           robotPoseCollection.poses[0], oldContainer[i].pose.x, oldContainer[i].pose.y);
    00291         GT2005Particle* particle
    00292           = new GT2005Particle(position.x, position.y, 0, 0, 0, 0);
    00293         particle->probability = max(0.01,   oldContainer[i].probability
    00294                                           * validities.x * robotPoseValidity);
    00295         DEBUG_RESPONSE
    00296         ( "TeamBallLocator:output particle.probability",
    00297           OUTPUT(idText, text, "particle x probability " << particle->probability);
    00298         );
    00299         compositeCell.addParticle(particle);
    00300       }
    00301       { //second robot pose
    00302         Vector2<double> position = Geometry::relative2FieldCoord(
    00303           robotPoseCollection.poses[1], oldContainer[i].pose.x, oldContainer[i].pose.y);
    00304         GT2005Particle* particle
    00305           = new GT2005Particle(position.x, position.y, 0, 0, 0, 0);
    00306         particle->probability = max(0.01,   oldContainer[i].probability
    00307                                           * validities.y * robotPoseValidity);
    00308         DEBUG_RESPONSE
    00309         ( "TeamBallLocator:output particle.probability",
    00310           OUTPUT(idText, text, "particle y probability " << particle->probability);
    00311         );
    00312         compositeCell.addParticle(particle);
    00313       }
    00314       { //third robot pose
    00315         Vector2<double> position = Geometry::relative2FieldCoord(
    00316           robotPoseCollection.poses[2], oldContainer[i].pose.x, oldContainer[i].pose.y);
    00317         GT2005Particle* particle
    00318           = new GT2005Particle(position.x, position.y, 0, 0, 0, 0);
    00319         particle->probability = max(0.01,   oldContainer[i].probability
    00320                                           * validities.z * robotPoseValidity);
    00321         DEBUG_RESPONSE
    00322         ( "TeamBallLocator:output particle.probability",
    00323           OUTPUT(idText, text, "particle z probability " << particle->probability);
    00324         );
    00325         compositeCell.addParticle(particle);
    00326       }
    00327     }
    00328   }
    00329
    00330   //in the queue all cells with probability greater than biggestProb will be saved:
    00331   Queue queue;
    00332
    00333   //the cells of the composite cell with too low probability are destroyed:
    00334   if (compositeCell.cellDownLeft->getSumOfProb()
    00335       < (smallestProb * compositeCell.getSumOfProb()))
    00336   {
    00337     compositeCell.cellDownLeft->violentBrutalDestruction();
    00338     delete compositeCell.cellDownLeft;
    00339     compositeCell.cellDownLeft = 0;
    00340     numberOfCells--;
    00341   }
    00342   else
    00343     queue.insertCell(compositeCell.cellDownLeft);
    00344
    00345   if (compositeCell.cellDownRight->getSumOfProb()
    00346       < (smallestProb * compositeCell.getSumOfProb()))
    00347   {
    00348     compositeCell.cellDownRight->violentBrutalDestruction();
    00349     delete compositeCell.cellDownRight;
    00350     compositeCell.cellDownRight = 0;
    00351     numberOfCells--;
    00352   }
    00353   else
    00354     queue.insertCell(compositeCell.cellDownRight);
    00355
    00356   if (compositeCell.cellTopLeft->getSumOfProb()
    00357       < (smallestProb * compositeCell.getSumOfProb()))
    00358   {
    00359     compositeCell.cellTopLeft->violentBrutalDestruction();
    00360     delete compositeCell.cellTopLeft;
    00361     compositeCell.cellTopLeft = 0;
    00362     numberOfCells--;
    00363   }
    00364   else
    00365     queue.insertCell(compositeCell.cellTopLeft);
    00366
    00367   if (compositeCell.cellTopRight->getSumOfProb()
    00368       < (smallestProb * compositeCell.getSumOfProb()))
    00369   {
    00370     compositeCell.cellTopRight->violentBrutalDestruction();
    00371     delete compositeCell.cellTopRight;
    00372     compositeCell.cellTopRight = 0;
    00373     numberOfCells--;
    00374   }
    00375   else
    00376     queue.insertCell(compositeCell.cellTopRight);
    00377
    00378   //the cells with too much probability are splitted:
    00379   while ((numberOfCells <= 9) && (!(queue.first == 0)))
    00380   {
    00381     AbstractCell* tmp = queue.first;
    00382     queue.first = queue.first->next;
    00383
    00384     //if the depth is too deep, do not sub-divide this cell further:
    00385     if (tmp->depth >= 5)
    00386       continue;
    00387
    00388     CompositeCell* cc = 0;
    00389     CompositeCell* parC = (CompositeCell*)(tmp->parent);
    00390     parC->split((BasicCell*)(tmp), &cc);
    00391     if (!(cc->cellDownLeft == 0) && (cc->cellDownLeft->getSumOfProb()
    00392                                      > (biggestProb * cc->getSumOfProb())))
    00393     {
    00394       queue.insertCell(cc->cellDownLeft);
    00395     }
    00396     if (!(cc->cellDownRight == 0) && (cc->cellDownRight->getSumOfProb()
    00397                                       > (biggestProb*cc->getSumOfProb())))
    00398     {
    00399       queue.insertCell(cc->cellDownRight);
    00400     }
    00401     if (!(cc->cellTopRight == 0) && (cc->cellTopRight->getSumOfProb()
    00402                                      > (biggestProb * cc->getSumOfProb())))
    00403     {
    00404       queue.insertCell(cc->cellTopRight);
    00405     }
    00406     if (!(cc->cellTopLeft == 0) && (cc->cellTopLeft->getSumOfProb()
    00407                                     > (biggestProb * cc->getSumOfProb())))
    00408     {
    00409       queue.insertCell(cc->cellTopLeft);
    00410     }
    00411   }
    00412
    00413   GT2005Particle* first = 0;
    00414   compositeCell.getParticles(&first);
    00415   GT2005Particle* johnny = 0;
    00416   i = 0;
    00417   for (GT2005Particle* walker = first; !(walker == 0); walker = walker->next)
    00418   {
    00419     if (walker->probability > GT2005BallLocatorParameters::throwInProb)
    00420     {
    00421       newContainer[i++].setParameters(
    00422         (short int)(walker->pose.x), (short int)(walker->pose.y),
    00423         0, 0, (unsigned short int)(walker->probability * 10000), 0);
    00424     }
    00425     //delete the particle of the step before:
    00426     if (!(johnny == 0))
    00427       delete johnny;
    00428     johnny = walker;
    00429   }
    00430   //delete the last particle:
    00431   if (!(johnny == 0))
    00432     delete johnny;
    00433
    00434   //Destruct all cells:
    00435   compositeCell.violentBrutalDestruction();
    00436 }
    00437 //---------------------------------------------------------------------------------------
    00438 void GT2005TeamBallLocator::timeUpdateNotReceived(GT2005Particle* particle,
    00439                                                   int& framesNotSeen)
    00440 {
    00441   /* Move the particle into a random direction. How far it is moved depends on the
    00442    * number of frames the particle is old. (gaussian distribution, method of Marsaglia,
    00443    * first for x, then for y)
    00444    */
    00445   double maxDist = framesNotSeen*maxDistTimes;
    00446   double v = 2, u1 = 0, u2 = 0;
    00447   int count = 0;
    00448   while (v >= 1)
    00449   {
    00450     u1 = random();
    00451     u2 = random();
    00452     v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
    00453     if ((v >= 1) && (count++ > 10))
    00454       v = 0.9;
    00455   }
    00456   particle->pose.x += maxDist * (2*u1 - 1) * sqrt(-2*log(v)/v);
    00457
    00458   v = 2;
    00459   count = 0;
    00460   while (v >= 1)
    00461   {
    00462     u1 = random();
    00463     u2 = random();
    00464     v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
    00465     if ((v >= 1) && (count++ > 10))
    00466       v = 0.9;
    00467   }
    00468   particle->pose.y += maxDist * ((2*u1 - 1) * sqrt(-2*log(v)/v));
    00469
    00470   //Decrease the probability (faster than in the normal timeUpdate)
    00471   particle->probability -= decreaseProbValue;
    00472   //if (particle->probability < 0.005)
    00473   //  particle->probability = 0;
    00474 }
    00475 //---------------------------------------------------------------------------------------
    00476 void GT2005TeamBallLocator::timeUpdateReceived(GT2005Particle* particle, double& difference)
    00477 {
    00478   particle->probability *= getTimeFactor(difference);
    00479 }
    00480 //---------------------------------------------------------------------------------------
    00481 double GT2005TeamBallLocator::getTimeFactor(double& difference)
    00482 {
    00483   if (difference < 1 * timeFactorThreshold)
    00484     return 1;
    00485   if (difference < 2 * timeFactorThreshold)
    00486     return 0.9;
    00487   if (difference < 3 * timeFactorThreshold)
    00488     return 0.8;
    00489   if (difference < 4 * timeFactorThreshold)
    00490     return 0.7;
    00491   if (difference < 5 * timeFactorThreshold)
    00492     return 0.6;
    00493   return 0.5;
    00494 }
    00495 //---------------------------------------------------------------------------------------
    00496 bool GT2005TeamBallLocator::handleMessage(InMessage& message)
    00497 {
    00498   switch(message.getMessageID())
    00499   {
    00500   case idGT2005TeamBallLocator:
    00501     {
    00502       GenericDebugData d;
    00503       message.bin >> d;
    00504       smallestProb = d.data[0] / 100;
    00505       biggestProb = d.data[1] / 100;
    00506       timeFactorThreshold = d.data[4];
    00507     }
    00508     break;
    00509   }
    00510   return false;
    00511 }
    00512 //---------------------------------------------------------------------------------------
    00513 void GT2005TeamBallLocator::drawParticle(const GT2005Particle& particle,
    00514                                          Drawings::Color color) const
    00515 {
    00516   if (particle.probability == 0)
    00517     return;
    00518
    00519   double thickness = (particle.probability + 0.1)*50;
    00520
    00521   NCIRCLE("TeamBallLocator:particles", particle.pose.x, particle.pose.y,
    00522           thickness, 2*thickness, Drawings::ps_solid, color);
    00523   CIRCLE(teamBallLocatorFieldParticles, particle.pose.x, particle.pose.y,
    00524          thickness, 2*thickness, Drawings::ps_solid, color);
    00525 }
    00526 //---------------------------------------------------------------------------------------
    00527 void GT2005TeamBallLocator::draw(const GT2005ParticleContainerReceived& container,
    00528                                  const double& ballX,
    00529                                  const double& ballY,
    00530                                  const double& ballP,
    00531                                  Drawings::Color particleColor,
    00532                                  Drawings::Color estimatedBallColor) const
    00533 {
    00534   NDECLARE_DEBUGDRAWING("TeamBallLocator:position:position", "drawingOnField",
    00535                         "The estimated ball position.");
    00536   NDECLARE_DEBUGDRAWING("TeamBallLocator:position:line from robot to position",
    00537                         "drawingOnField", "A line from the robot pose to the team ball.");
    00538   NDECLARE_DEBUGDRAWING("TeamBallLocator:particles", "drawingOnField",
    00539                         "The particles of the GT2005 team ball locator.");
    00540
    00541   //Call the function drawParticle for every particle with the given color:
    00542   for (int i = 0; i < container.getNumberOfSamples(); i++)
    00543     drawParticle(container.current[i], particleColor);
    00544
    00545   //The estimated ball is drawn:
    00546   NCIRCLE("TeamBallLocator:position:position", ballX, ballY, 28, 40,
    00547           Drawings::ps_solid, estimatedBallColor);
    00548   NCOLORED_LINE("TeamBallLocator:position:line from robot to position",
    00549                 robotPose.translation.x, robotPose.translation.y,
    00550                 ballX, ballY, 2, Drawings::ps_solid, 255, 0, 255);
    00551   CIRCLE(teamBallLocatorField, ballX, ballY, 28, 40,
    00552          Drawings::ps_solid, estimatedBallColor);
    00553   LINE(teamBallLocatorField, robotPose.translation.x, robotPose.translation.y,
    00554        ballX, ballY, 2, Drawings::ps_solid, estimatedBallColor);
    00555
    00556   DEBUG_DRAWING_FINISHED(teamBallLocatorField);
    00557   DEBUG_DRAWING_FINISHED(teamBallLocatorFieldParticles);
    00558 }
    00559 //---------------------------------------------------------------------------------------