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

# Modules/ObstaclesLocator/GT2005ObstaclesLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005ObstaclesLocator.cpp
    00003 *
    00004 * This file contains a class for obstacle localization.
    00005 * @author Jan Hoffmann
    00006 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
    00007 */
    00008
    00009 #include "GT2005ObstaclesLocator.h"
    00010 #include "Tools/Debugging/DebugDrawings.h"
    00011 #include "Tools/Math/Geometry.h"
    00012 #include "Tools/FieldDimensions.h"
    00013 #include "Tools/Math/Matrix2x2.h"
    00014 #include "Tools/Debugging/GenericDebugData.h"
    00015 #include "Platform/SystemCall.h"
    00016
    00017 GT2005ObstaclesLocator::GT2005ObstaclesLocator(const ObstaclesLocatorInterfaces& interfaces)
    00018 : ObstaclesLocator(interfaces), headTiltClipPSD(-.3)
    00019 {
    00020   usePSD = true;
    00021   useLinesPercept = false;
    00022   useObstaclesPercept = true;
    00023   useAging = true;
    00024
    00025   sectorWidth = pi2/(double)ObstaclesModel::numOfSectors;
    00026
    00027   for(int i = 0; i < ObstaclesModel::numOfSectors; i++)
    00028   {
    00029     obstacles[i].x = 0;
    00030     obstacles[i].y = 0;
    00031     obstacleTypes[i] = ObstaclesPercept::unknown;
    00032   }
    00033
    00034   for( int i = 0; i < 2; ++i)
    00035   {
    00036     angleToSeenGoal[i] = 0;
    00037     distanceToSeenGoal[i] = 0;
    00038     timeOfSeenGoal[i] = ULONG_MAX;
    00039   }
    00040
    00041 }
    00042
    00043
    00044 void GT2005ObstaclesLocator::execute()
    00045 {
    00046   odometry = odometryData - lastOdometry;
    00047   lastOdometry = odometryData;
    00048
    00049   MODIFY("obstacles locator:odometry", odometry);
    00050
    00051   moveObstaclesByOdometry();
    00052
    00053 //  if(usePSD) addPSDPercept();
    00054   if(useObstaclesPercept) addObstaclesPercept();
    00055   if(useLinesPercept) addLinesPercept();
    00056
    00057   setObstaclesModel(true);
    00058   obstaclesModel.setFrameNumber(obstaclesPercept.frameNumber);
    00059
    00060
    00061   determineFreePartsOfGoals();
    00062   determineNextFreeTeammate();
    00063   determineSeenGoals();
    00064   DEBUG_DRAWING_FINISHED(sketch);
    00065 }
    00066
    00067 void GT2005ObstaclesLocator::moveObstaclesByOdometry()
    00068 {
    00069   // move representatives by odometry
    00070 /*  if (odometry.translation.x == 0 &&
    00071       odometry.translation.y == 0 &&
    00072       odometry.rotation == 0) return;
    00073 */
    00074
    00075   Vector2<double> c1(cos(odometry.rotation),-sin(odometry.rotation));
    00076   Vector2<double> c2(sin(odometry.rotation),cos(odometry.rotation));
    00077   Matrix2x2<double> R(c1, c2);
    00078
    00079   Vector2<double> movedObstacles[ObstaclesModel::numOfSectors];
    00080   unsigned long movedTimestamps[ObstaclesModel::numOfSectors];
    00081   ObstaclesPercept::ObstacleType movedObstacleTypes[ObstaclesModel::numOfSectors];
    00082   int numOfObstacles = 0;
    00083
    00084   int i;
    00085
    00086   for(i = 0; i < ObstaclesModel::numOfSectors; i++)
    00087   {
    00088     if (
    00089       (obstacles[i].x != 0 || obstacles[i].y != 0) &&
    00090       (!useAging ||
    00091        SystemCall::getTimeSince(timestamps[i]) < timeAfterWhichObstacleIsForgotten)
    00092        )
    00093     {
    00094       movedObstacles[numOfObstacles] = R*obstacles[i];
    00095       movedObstacles[numOfObstacles] -= odometry.translation;
    00096       movedObstacleTypes[numOfObstacles] = obstacleTypes[i];
    00097       movedTimestamps[numOfObstacles] = timestamps[i];
    00098       numOfObstacles++;
    00099     }
    00100     obstacles[i].x = obstacles[i].y = 0;
    00101     obstacleTypes[i] = ObstaclesPercept::unknown;
    00102   }
    00103
    00104   for(i = 0; i < numOfObstacles; i++)
    00105   {
    00106     int targetSector = ObstaclesModel::getSectorFromAngle(movedObstacles[i].angle());
    00107     double sqrDistance = movedObstacles[i]*movedObstacles[i];
    00108
    00109     if
    00110       (
    00111         sqrDistance >= 10.0 &&
    00112 //        sqrDistance < ObstaclesModel::maxDistance*ObstaclesModel::maxDistance &&
    00113         (
    00114          (obstacles[targetSector].x == 0 && obstacles[targetSector].y == 0) ||
    00115          sqrDistance <= obstacles[targetSector]*obstacles[targetSector]
    00116         )
    00117       )
    00118     {
    00119       obstacles[targetSector] = movedObstacles[i];
    00120       obstacleTypes[targetSector] = movedObstacleTypes[i];
    00121       timestamps[targetSector] = movedTimestamps[i];
    00122     }
    00123   }
    00124 }
    00125
    00126 void GT2005ObstaclesLocator::setObstaclesModel(bool addWorldModel)
    00127 {
    00128   int i;
    00129   double distance;
    00130
    00131   ObstaclesModel buffer;
    00132
    00133   // set sector distances
    00134   for(i = 0; i < ObstaclesModel::numOfSectors; i++)
    00135   {
    00136     distance = obstacles[i].abs();
    00137
    00138     // if the player is the goalie, don't avoid own penalty area!
    00139     if (getPlayer().getPlayerNumber() != Player::one)
    00140     //if (addWorldModel)
    00141     {
    00142       if (FieldDimensions::distanceToOwnPenaltyArea(robotPose) > 0)
    00143       {
    00144         double distToPenaltyArea = field.getDistanceToOwnPenaltyArea(robotPose + Pose2D(ObstaclesModel::getAngleOfSector(i)));
    00145         if (distToPenaltyArea > 50 && distToPenaltyArea < ObstaclesModel::maxDistance)
    00146           if ((distToPenaltyArea < distance) || (distance == 0))
    00147             distance = distToPenaltyArea;
    00148       }
    00149     }
    00150
    00151     if(distance > 0)
    00152     {
    00153       buffer.distance[i] = (int)distance;
    00154       buffer.obstacleType[i] = obstacleTypes[i];
    00155     }
    00156     else
    00157     {
    00158       buffer.distance[i] = ObstaclesModel::maxDistance;
    00159       buffer.obstacleType[i] = ObstaclesPercept::unknown;
    00160     }
    00161   }
    00162
    00163
    00164
    00165   // remove near peaks to make it more robust against
    00166   // noise!
    00167   for(i = 0; i < ObstaclesModel::numOfSectors; i++)
    00168   {
    00169     // if the two neighbouring sectors have a distance
    00170     // that is 200 mm greater than the current one,
    00171     // perform smoothing:
    00172     if ((buffer.distance[i] - buffer.distance[(i+1)%ObstaclesModel::numOfSectors] < -200) &&
    00173         (buffer.distance[i] - buffer.distance[(i+ObstaclesModel::numOfSectors-1)%ObstaclesModel::numOfSectors]) < -200)
    00174     {
    00175       obstaclesModel.distance[i] = (buffer.distance[(i+1)%ObstaclesModel::numOfSectors] +
    00176         buffer.distance[(i-1)%ObstaclesModel::numOfSectors])/2;
    00177     }
    00178     else
    00179     {
    00180       obstaclesModel.distance[i] = buffer.distance[i];
    00181     }
    00182     //obstaclesModel.distance[i] = buffer.distance[i];
    00183     obstaclesModel.obstacleType[i] = buffer.obstacleType[i];
    00184   }
    00185
    00186   // calculate corridor
    00187
    00188   /* the following is pretty useless !! - thinks JH
    00189
    00190
    00191
    00192   */
    00193   double closest = ObstaclesModel::maxDistance;
    00194   for(i = ObstaclesModel::getSectorFromAngle(-pi_2); i < ObstaclesModel::getSectorFromAngle(pi_2); i++)
    00195   {
    00196     if ((obstacles[i].x < closest) && (obstacles[i].x > 10)
    00197       && (obstacles[i].y < 90) && (obstacles[i].y > -90))
    00198       closest = obstacles[i].x;
    00199   }
    00200   obstaclesModel.corridorInFront = (int )closest;
    00201
    00202   obstaclesModel.bodyPSD = psdPercept[psdPercept.numOfPercepts - 1].body;
    00203 }
    00204
    00205 void GT2005ObstaclesLocator::addPSDPercept()
    00206 {
    00207   for (int i = 0; i < psdPercept.numOfPercepts; i++)
    00208   {
    00209     if (psdPercept[i].neckTilt > headTiltClipPSD && psdPercept[i].isValid)
    00210     {
    00211       // latest measurement
    00212       Vector2<double> psdPoint(psdPercept[i].x, psdPercept[i].y);
    00213       int sector = ObstaclesModel::getSectorFromAngle(psdPoint.angle());
    00214
    00215        // point is on the ground
    00216       if(psdPercept[i].z < 90)
    00217       {
    00218         double sqrDistance = obstacles[sector] * obstacles[sector];
    00219         // only update if the point seen is farther away
    00220         if (sqrDistance > 0 &&
    00221             sqrDistance < psdPoint * psdPoint)
    00222         {
    00223           obstacles[sector].x = psdPoint.x;
    00224           obstacles[sector].y = psdPoint.y;
    00225           timestamps[sector] = SystemCall::getCurrentSystemTime();
    00226         }
    00227       }
    00228       // point is an obstacle
    00229       else if (!psdPercept[i].tooFarAway)
    00230       {
    00231         double angleToPoint = normalize(psdPoint.angle());
    00232         double angleToBall = Geometry::angleTo(robotPose.getPose(), ballModel.seen.positionField);
    00233
    00234         if(toDegrees(normalize(angleToPoint - angleToBall)) < 10 &&
    00235           toDegrees(normalize(angleToPoint - angleToBall)) > -10 &&
    00236           SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 1000)
    00237         {
    00238           DOT(sketch, 20, 10, Drawings::green, Drawings::green);
    00239         }
    00240         else
    00241         {
    00242           obstacles[sector].x = psdPoint.x;
    00243           obstacles[sector].y = psdPoint.y;
    00244           timestamps[sector] = SystemCall::getCurrentSystemTime();
    00245         }
    00246       }
    00247       // if nothing in this direction, perform reset
    00248       else
    00249       {
    00250         obstacles[sector].x = 0;
    00251         obstacles[sector].y = 0;
    00252       }
    00253     }
    00254   }
    00255 }
    00256
    00257 void GT2005ObstaclesLocator::addLinesPercept()
    00258 {
    00259   int i;
    00260   for(i = 0; i < linesPercept.points[LinesPercept::redRobot].numberOfPoints; i++)
    00261   {
    00262     Vector2<double> obstaclePoint(linesPercept.points[LinesPercept::redRobot].pointsOnField[i].x, linesPercept.points[LinesPercept::redRobot].pointsOnField[i].y);
    00263     addObstaclePoint(obstaclePoint, limit);
    00264   }
    00265   for(i = 0; i < linesPercept.points[LinesPercept::blueRobot].numberOfPoints; i++)
    00266   {
    00267     Vector2<double> obstaclePoint(linesPercept.points[LinesPercept::blueRobot].pointsOnField[i].x, linesPercept.points[LinesPercept::blueRobot].pointsOnField[i].y);
    00268     addObstaclePoint(obstaclePoint, limit);
    00269   }
    00270   //for(i = 0; i < linesPercept.points[LinesPercept::border].numberOfPoints; i++)
    00271   //{
    00272   //  Vector2<double> obstaclePoint(linesPercept.points[LinesPercept::border].pointsOnField[i].x, linesPercept.points[LinesPercept::border].pointsOnField[i].y);
    00273   //  addObstaclePoint(obstaclePoint, extend);
    00274   //}
    00275 }
    00276
    00277 void GT2005ObstaclesLocator::addObstaclesPercept()
    00278 {
    00279   int i;
    00280   obstaclesModel.currentlySeenStartSector = obstaclesModel.numOfSectors;
    00281   obstaclesModel.currentlySeenEndSector = 0;
    00282   for(i = 0; i < obstaclesPercept.numberOfSegments ; i++)
    00283   {
    00284     const ObstaclesPercept::Segment& s = obstaclesPercept.segments[i];
    00285 //    addObstaclePoint(obstaclePoint, limit);
    00286
    00287 //    if(
    00288 //      Geometry::distance(
    00289 //      obstaclesPercept.farPoints[i],
    00290 //      obstaclesPercept.farPoints[i-1]) < 300
    00291 //      &&
    00292 //      Geometry::distance(
    00293 //      obstaclesPercept.farPoints[i],
    00294 //      obstaclesPercept.farPoints[i+1]) < 300
    00295 //      )
    00296     addObstaclePoints(
    00297       Vector2<int>(int(s.nearPointOnField.x), int(s.nearPointOnField.y)),
    00298       Vector2<int>(int(s.farPointOnField.x), int(s.farPointOnField.y)),
    00299       s.farPointIsOnImageBorder,
    00300       s.obstacleType);
    00301   }
    00302 }
    00303
    00304 void GT2005ObstaclesLocator::addObstaclePoint
    00305 (
    00306  const Vector2<double>& obstaclePoint,
    00307  UpdateMode mode
    00308  )
    00309 {
    00310   double angleToPoint = normalize(obstaclePoint.angle());
    00311
    00312   //Vision based obstacles percept does not contain balls.
    00313 /*
    00314   double angleToBall = Geometry::angleTo(robotPose.getPose(), ballModel.seen);
    00315   if(toDegrees(normalize(angleToPoint - angleToBall)) < 10 &&
    00316     toDegrees(normalize(angleToPoint - angleToBall)) > -10 &&
    00317     SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 1000)
    00318   {
    00319     DOT(sketch, 10, 10, Drawings::red, Drawings::red);
    00320     return;
    00321   }
    00322 */
    00323
    00324   int sector = ObstaclesModel::getSectorFromAngle(angleToPoint);
    00325
    00326   if ( sector >= ObstaclesModel::numOfSectors || ObstaclesModel::numOfSectors < 0 )
    00327   {
    00328     sector = 0;
    00329     return;
    00330
    00331     OUTPUT ( idText, text, "sector >= ObstaclesModel::numOfSectors" );
    00332   }
    00333   double sqrDistanceInModel = obstacles[sector] * obstacles[sector];
    00334   double sqrDistanceToPoint = obstaclePoint * obstaclePoint;
    00335
    00336   if(sqrt(sqrDistanceToPoint) < 150)
    00337   {
    00338     DOT(sketch, 30, 10, Drawings::blue, Drawings::blue);
    00339     return;
    00340   }
    00341
    00342   switch(mode)
    00343   {
    00344   case overwrite:
    00345       obstacles[sector].x = obstaclePoint.x;
    00346       obstacles[sector].y = obstaclePoint.y;
    00347       timestamps[sector] = SystemCall::getCurrentSystemTime();
    00348     break;
    00349   case extend:
    00350     // only update if the new point seen is farther away than the old one
    00351     if(sqrDistanceInModel < sqrDistanceToPoint)
    00352     {
    00353       obstacles[sector].x = obstaclePoint.x;
    00354       obstacles[sector].y = obstaclePoint.y;
    00355       timestamps[sector] = SystemCall::getCurrentSystemTime();
    00356     }
    00357     break;
    00358  case limit:
    00359     // only update if the new point is closer than the old one
    00360     if(
    00361       (obstacles[sector].x == 0 && obstacles[sector].y == 0) ||
    00362        sqrDistanceToPoint < sqrDistanceInModel)
    00363     {
    00364       obstacles[sector].x = obstaclePoint.x;
    00365       obstacles[sector].y = obstaclePoint.y;
    00366       timestamps[sector] = SystemCall::getCurrentSystemTime();
    00367     }
    00368     break;
    00369   }
    00370 }
    00371
    00372 void GT2005ObstaclesLocator::addObstaclePoints
    00373 (
    00374  const Vector2<int>& nearPoint,
    00375  const Vector2<int>& farPoint,
    00376  bool farPointIsOnImageBorder,
    00377  ObstaclesPercept::ObstacleType obstacleType
    00378  )
    00379 {
    00380   double angleToPoints = normalize(farPoint.angle());
    00381   int sector = ObstaclesModel::getSectorFromAngle(angleToPoints);
    00382
    00383   obstaclesModel.currentlySeenStartSector = min(obstaclesModel.currentlySeenStartSector,sector);
    00384   obstaclesModel.currentlySeenEndSector = max(obstaclesModel.currentlySeenEndSector,sector);
    00385
    00386   if ( sector >= ObstaclesModel::numOfSectors || ObstaclesModel::numOfSectors < 0 )
    00387   {
    00388     sector = 0;
    00389     return;
    00390     OUTPUT ( idText, text, "sector >= ObstaclesModel::numOfSectors" );
    00391   }
    00392   double sqrDistanceInModel = obstacles[sector] * obstacles[sector];
    00393   double sqrDistanceToNearPoint = nearPoint * nearPoint;
    00394   double sqrDistanceToFarPoint = farPoint * farPoint;
    00395
    00396   if(sqrDistanceToNearPoint > sqrDistanceInModel && sqrDistanceInModel != 0) return;
    00397   else if(!farPointIsOnImageBorder)
    00398   {
    00399     obstacles[sector].x = farPoint.x;
    00400     obstacles[sector].y = farPoint.y;
    00401     if(obstacleType != ObstaclesPercept::unknown || (fabs(sqrDistanceToFarPoint - sqrDistanceInModel) > 500))
    00402       obstacleTypes[sector] = obstacleType;
    00403     timestamps[sector] = SystemCall::getCurrentSystemTime();
    00404   }
    00405   else if(sqrDistanceInModel < sqrDistanceToFarPoint && sqrDistanceInModel != 0)
    00406   {
    00407     obstacles[sector].x = farPoint.x;
    00408     obstacles[sector].y = farPoint.y;
    00409     timestamps[sector] = SystemCall::getCurrentSystemTime();
    00410   }
    00411 }
    00412
    00413
    00414 void GT2005ObstaclesLocator::determineFreePartsOfGoals()
    00415 {
    00416   for(int i = 0; i < 2; i++)
    00417   {
    00418     const ObstaclesPercept::FreePartOfGoal& f = obstaclesPercept.freePartOfGoal[i];
    00419     if (f.wasDetermined)
    00420     {
    00421       double seenLeft = normalize(f.directionAsAngle + f.widthAsAngle / 2.0);
    00422       double seenRight = normalize(f.directionAsAngle - f.widthAsAngle / 2.0);
    00423       double oldLeft = normalize(angleToFreePartOfGoal[i] + widthOfFreePartOfGoal[i] / 2.0);
    00424       double oldRight = normalize(angleToFreePartOfGoal[i] - widthOfFreePartOfGoal[i] / 2.0);
    00425       double newLeft = oldLeft;
    00426       double newRight = oldRight;
    00427       bool updated = false;
    00428
    00429       if
    00430       (
    00431         f.leftEndSeen &&
    00432         (f.widthAsAngle > widthOfFreePartOfGoal[i] ||
    00433          (normalize(oldRight - seenLeft) < 0 && normalize(seenLeft - oldLeft) < 0)
    00434         )
    00435       )
    00436       {
    00437         newLeft = seenLeft;
    00438         if (newLeft < oldRight) newRight = seenRight;
    00439         updated = true;
    00440       }
    00441       if
    00442       (
    00443         f.rightEndSeen &&
    00444         (f.widthAsAngle > widthOfFreePartOfGoal[i]||
    00445          (normalize(oldRight - seenLeft) < 0 && normalize(seenLeft - oldLeft) < 0)
    00446         )
    00447       )
    00448       {
    00449         newRight = seenRight;
    00450         if (newRight > oldLeft) newLeft = seenLeft;
    00451         updated = true;
    00452       }
    00453       if (updated)
    00454       {
    00455         obstaclesModel.lastTimeFreePartOfGoalWasDetermined[i] = SystemCall::getCurrentSystemTime();
    00456         widthOfFreePartOfGoal[i] = normalize(newLeft - newRight);
    00457         angleToFreePartOfGoal[i] = normalize(newRight + widthOfFreePartOfGoal[i] / 2.0);
    00458         distanceToFreePartOfGoal[i] = f.distanceOnField;
    00459
    00460         obstaclesModel.angleToFreePartOfGoalWasDetermined[i] = true;
    00461       }
    00462     }
    00463     else if(SystemCall::getTimeSince(obstaclesModel.lastTimeFreePartOfGoalWasDetermined[i]) < 500)
    00464     {
    00465       angleToFreePartOfGoal[i] = normalize(angleToFreePartOfGoal[i] - odometry.getAngle());
    00466       obstaclesModel.angleToFreePartOfGoalWasDetermined[i] = true;
    00467     }
    00468     else
    00469     {
    00470       angleToFreePartOfGoal[i] = 0.0;
    00471       widthOfFreePartOfGoal[i] = 0.0;
    00472       distanceToFreePartOfGoal[i] = 1000.0;
    00473
    00474       obstaclesModel.angleToFreePartOfGoalWasDetermined[i] = false;
    00475     }
    00476     obstaclesModel.angleToFreePartOfGoal[i] = angleToFreePartOfGoal[i];
    00477     obstaclesModel.widthOfFreePartOfGoal[i] = widthOfFreePartOfGoal[i];
    00478     obstaclesModel.distanceToFreePartOfGoal[i] = distanceToFreePartOfGoal[i];
    00479   }
    00480 }
    00481
    00482 void GT2005ObstaclesLocator::determineNextFreeTeammate()
    00483 {
    00484   int distanceToNextTeammate = 10000;
    00485   double angleToNextTeammate = 0;
    00486
    00487   bool foundTeammate = false;
    00488
    00489   for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
    00490   {
    00491     PlayerPose currentPose = playerPoseCollection.getOwnPlayerPose(i);
    00492     int currentDistance = (int)((currentPose.getPose().translation - robotPose.translation).abs());
    00493     double currentAngle = normalize(Geometry::angleTo(robotPose, currentPose.getPose().translation));
    00494
    00495     int targetSector = ObstaclesModel::getSectorFromAngle(currentAngle);
    00496     int distanceOfTargetSector = (int)obstacles[targetSector].abs();
    00497
    00498     if(currentDistance < distanceToNextTeammate &&
    00499       (
    00500       distanceOfTargetSector == 0 ||
    00501       distanceOfTargetSector + 200 > distanceToNextTeammate
    00502       )
    00503       )
    00504     {
    00505       foundTeammate = true;
    00506       angleToNextTeammate = currentAngle;
    00507       distanceToNextTeammate = currentDistance;
    00508     }
    00509   }
    00510   if(foundTeammate)
    00511   {
    00512     obstaclesModel.angleToNextFreeTeammateWasDetermined = true;
    00513     obstaclesModel.angleToNextFreeTeammate = angleToNextTeammate;
    00514     obstaclesModel.distanceToNextFreeTeammate = distanceToNextTeammate;
    00515   }
    00516   else
    00517   {
    00518     obstaclesModel.angleToNextFreeTeammateWasDetermined = false;
    00519   }
    00520 }
    00521
    00522
    00523 void GT2005ObstaclesLocator::determineSeenGoals()
    00524 {
    00525   NDECLARE_DEBUGDRAWING( "obstacles locator:determined seen goal angle and distance", "drawingOnField", "draws the seen goal angle and distance as determined by obstacleslocator");
    00526   NDECLARE_DEBUGDRAWING( "obstacles locator:seen goal angle", "drawingOnField", "draws the seen goal angle and distance as determined by obstacleslocator");
    00527   // update for seen goals
    00528   bool notSeenGoals[2] = { true, true};
    00529   for( int i = 0; i < landmarksPercept.numberOfGoals; ++i)
    00530   {
    00531     const Goal& goal = landmarksPercept.goals[i];
    00532     colorClass ownGoalColor = (getPlayer().getTeamColor()==Player::blue) ? skyblue : yellow;
    00533     ObstaclesPercept::WhoseGoal index = (landmarksPercept.goals[i].color==ownGoalColor) ? ObstaclesPercept::ownGoal : ObstaclesPercept::opponentGoal;
    00534
    00535     distanceToSeenGoal[index] = obstaclesModel.distanceToSeenGoal[index] = goal.distance;
    00536     double expectedWidthOfGoalAngle = fabs(atan2( 400, distanceToSeenGoal[index])); // half of this angle is sufficient
    00537     double widthOfDetectedGoalAngle = fabs( goal.x.max - goal.x.min)/2; // half of the width
    00538
    00539     if( goal.isOnBorder( goal.x.min) && !goal.isOnBorder(goal.x.max))
    00540     {
    00541       obstaclesModel.angleToSeenGoal[index] = goal.angle + widthOfDetectedGoalAngle - expectedWidthOfGoalAngle;
    00542       angleToSeenGoal[index] = obstaclesModel.angleToSeenGoal[index];
    00543     }
    00544     else if( !goal.isOnBorder( goal.x.min) && goal.isOnBorder(goal.x.max))
    00545     {
    00546       obstaclesModel.angleToSeenGoal[index] = goal.angle - widthOfDetectedGoalAngle + expectedWidthOfGoalAngle;
    00547       angleToSeenGoal[index] = obstaclesModel.angleToSeenGoal[index];
    00548     }
    00549     else
    00550     {
    00551       obstaclesModel.angleToSeenGoal[index] = goal.angle;
    00552       angleToSeenGoal[index] = obstaclesModel.angleToSeenGoal[index];
    00553     }
    00554
    00555     NCOMPLEX_DRAWING(
    00556       "obstacles locator:determined seen goal angle and distance",
    00557       {
    00558         Pose2D leftPost =robotPose + Pose2D( angleToSeenGoal[index]) + Pose2D( distanceToSeenGoal[index], tan( expectedWidthOfGoalAngle) * distanceToSeenGoal[index]);
    00559         Pose2D rightPost =robotPose + Pose2D( angleToSeenGoal[index]) + Pose2D( distanceToSeenGoal[index], - tan( expectedWidthOfGoalAngle) * distanceToSeenGoal[index]);
    00560         NLINE(
    00561           "obstacles locator:determined seen goal angle and distance",
    00562           leftPost.translation.x,
    00563           leftPost.translation.y,
    00564           rightPost.translation.x,
    00565           rightPost.translation.y,
    00566           10,
    00567           Drawings::ps_solid,
    00568           Drawings::orange);
    00569
    00570         leftPost =robotPose + Pose2D( goal.angle) + Pose2D( distanceToSeenGoal[index], tan( widthOfDetectedGoalAngle) * distanceToSeenGoal[index]);
    00571         rightPost =robotPose + Pose2D( goal.angle) + Pose2D( distanceToSeenGoal[index], - tan( widthOfDetectedGoalAngle) * distanceToSeenGoal[index]);
    00572         NLINE(
    00573           "obstacles locator:determined seen goal angle and distance",
    00574           leftPost.translation.x,
    00575           leftPost.translation.y,
    00576           rightPost.translation.x,
    00577           rightPost.translation.y,
    00578           10,
    00579           Drawings::ps_solid,
    00580           Drawings::yellow);
    00581       }
    00582     );
    00583
    00584
    00585     timeOfSeenGoal[index] = obstaclesModel.timeOfSeenGoal[index] = SystemCall::getCurrentSystemTime();
    00586     notSeenGoals[index]= false;
    00587     obstaclesModel.trustSeenGoal[index] = true;
    00588   }
    00589
    00590   // update for not seen goals
    00591
    00592   double angleToLeftOpponentGoalPost  = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosLeftGoal));
    00593   double angleToRightOpponentGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosRightGoal));
    00594
    00595   if(angleToLeftOpponentGoalPost < angleToRightOpponentGoalPost)
    00596   {
    00597     angleToLeftOpponentGoalPost += pi2;
    00598   }
    00599   double angleToOpponentGoal = angleToLeftOpponentGoalPost + angleToRightOpponentGoalPost / 2.0;
    00600
    00601   double angleToLeftOwnGoalPost  = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosLeftGoal));
    00602   double angleToRightOwnGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosRightGoal));
    00603
    00604   if(angleToLeftOwnGoalPost < angleToRightOwnGoalPost)
    00605   {
    00606     angleToLeftOwnGoalPost += pi2;
    00607   }
    00608   double angleToOwnGoal = angleToLeftOwnGoalPost + angleToRightOwnGoalPost / 2.0;
    00609
    00610   CameraInfo cameraInfo(getRobotConfiguration().getRobotDesign());
    00611   for( int i = 0; i < 2; ++i)
    00612   {
    00613     if( notSeenGoals[i])
    00614     {
    00615       Pose2D relOffset = Pose2D(
    00616         cos(angleToSeenGoal[i]) * distanceToSeenGoal[i],
    00617         sin(angleToSeenGoal[i]) * distanceToSeenGoal[i]);
    00618
    00619       Pose2D absPoint = robotPose + relOffset;
    00620
    00621       Drawings::Color color = (i == ObstaclesPercept::ownGoal ? ((getPlayer().getTeamColor()==Player::blue) ? Drawings::skyblue : Drawings::yellow) : (((getPlayer().getTeamColor()==Player::blue) ? Drawings::yellow : Drawings::skyblue)));
    00622
    00623       NCIRCLE(
    00624         "obstacles locator:determined seen goal angle and distance",
    00625         absPoint.translation.x,
    00626         absPoint.translation.y,
    00627         30,
    00628         30,
    00629         Drawings::ps_solid,
    00630         Drawings::gray
    00631         );
    00632       relOffset = relOffset - odometry;
    00633
    00634       absPoint = robotPose + relOffset;
    00635
    00636       NCIRCLE(
    00637         "obstacles locator:determined seen goal angle and distance",
    00638         absPoint.translation.x,
    00639         absPoint.translation.y,
    00640         20,
    00641         20,
    00642         Drawings::ps_solid,
    00643         color
    00644         );
    00645
    00646       distanceToSeenGoal[i] = obstaclesModel.distanceToSeenGoal[i] = relOffset.translation.abs();
    00647       double angle = 0;
    00648       if( relOffset.translation.x != 0 || relOffset.translation.y != 0)
    00649         angle  = atan2(  relOffset.translation.y, relOffset.translation.x);
    00650       angleToSeenGoal[i] = obstaclesModel.angleToSeenGoal[i] = angle;
    00651
    00652       // calc trust
    00653       if( fabs(normalize(angleToSeenGoal[i] - (i==ObstaclesPercept::ownGoal?angleToOwnGoal:angleToOpponentGoal))) > pi_2)
    00654       {
    00655         obstaclesModel.trustSeenGoal[i] = false;
    00656       }
    00657       if( obstaclesModel.trustSeenGoal[i])
    00658       {
    00659         for(int j = 0; j < landmarksPercept.numberOfGoals; ++j)
    00660         {
    00661           if(
    00662             (angleToSeenGoal[i] < landmarksPercept.goals[j].angle + (cameraInfo.openingAngleWidth / 2.0)
    00663             && angleToSeenGoal[i] > landmarksPercept.goals[j].angle - (cameraInfo.openingAngleWidth / 2.0))
    00664             )
    00665           {
    00666             obstaclesModel.trustSeenGoal[i] = false;
    00667             break;
    00668           }
    00669         }
    00670       }
    00671     }
    00672   }
    00673
    00674   if( !notSeenGoals[ObstaclesModel::opponentGoal])
    00675   {
    00676
    00677         Drawings::Color color = (getPlayer().getTeamColor()==Player::blue) ? Drawings::yellow : Drawings::skyblue;
    00678         Pose2D leftPost = robotPose + Pose2D( distanceToSeenGoal[ ObstaclesModel::opponentGoal], -400);
    00679         Pose2D rightPose = robotPose + Pose2D( distanceToSeenGoal[ ObstaclesModel::opponentGoal], 400);
    00680         NLINE(
    00681           "obstacles locator:determined seen goal angle and distance",
    00682           leftPost.translation.x,
    00683           leftPost.translation.y,
    00684           leftPost.translation.x,
    00685           leftPost.translation.y,
    00686           10,
    00687           Drawings::ps_solid,
    00688           color
    00689         );
    00690     }
    00691     NCOMPLEX_DRAWING(
    00692       "obstacles locator:seen goal angle",
    00693     {
    00694
    00695       for( int i = 0; i < 2;++i)
    00696       {
    00697         Drawings::Color color = (i == ObstaclesPercept::ownGoal ? ((getPlayer().getTeamColor()==Player::blue) ? Drawings::skyblue : Drawings::yellow) : (((getPlayer().getTeamColor()==Player::blue) ? Drawings::yellow : Drawings::skyblue)));
    00698
    00699         Pose2D relOffset = Pose2D(
    00700           cos(angleToSeenGoal[i]) * distanceToSeenGoal[i],
    00701           sin(angleToSeenGoal[i]) * distanceToSeenGoal[i]);
    00702         Pose2D absPoint = robotPose + relOffset;
    00703         NLINE(
    00704           "obstacles locator:seen goal angle",
    00705           robotPose.translation.x,
    00706           robotPose.translation.y,
    00707           absPoint.translation.x,
    00708           absPoint.translation.y,
    00709           10,
    00710           Drawings::ps_solid,
    00711           color
    00712         );
    00713       }
    00714     }
    00715   );
    00716
    00717   MODIFY( "obstacles locator:angleToSeenGoal[0]", angleToSeenGoal[0]);
    00718   MODIFY( "obstacles locator:angleToSeenGoal[1]", angleToSeenGoal[1]);
    00719   MODIFY( "obstacles locator:angleToSeenGoal[1]", timeOfSeenGoal[0]);
    00720   MODIFY( "obstacles locator:angleToSeenGoal[1]", timeOfSeenGoal[1]);
    00721   MODIFY( "obstacles locator:distanceToSeenGoal[0]", distanceToSeenGoal[0]);
    00722   MODIFY( "obstacles locator:distanceToSeenGoal[1]", distanceToSeenGoal[1]);
    00723 }
    00724
    00725 bool GT2005ObstaclesLocator::handleMessage(InMessage& message)
    00726 {
    00727   bool handled = false;
    00728
    00729   switch(message.getMessageID())
    00730   {
    00731   case idGenericDebugData:
    00732     GenericDebugData d;
    00733     message.bin >> d;
    00734     if(d.id == GenericDebugData::defaultObstaclesLocator)
    00735     {
    00736       OUTPUT(idText,text,"generic debug message handled by module DefaultObstaclesLocator");
    00737
    00738       headTiltClipPSD = d.data[0];
    00739       switch((int)d.data[1])
    00740       {
    00741       case 0: usePSD = true; useLinesPercept = false; useAging = false;
    00742         break;
    00743       case 1: usePSD = false; useLinesPercept = true; useAging = false;
    00744         break;
    00745       case 2: usePSD = false; useLinesPercept = true; useAging = true;
    00746         break;
    00747       case 3: usePSD = true; useLinesPercept = true; useAging = false;
    00748         break;
    00749       case 4: usePSD = true; useLinesPercept = true; useAging = true;
    00750         break;
    00751       };
    00752     }
    00753     handled = true;
    00754     break;
    00755
    00756   }
    00757   return handled;
    00758 }
    00759