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

# Modules/ObstaclesLocator/GTCamObstaclesLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GTCamObstaclesLocator.cpp
    00003 *
    00004 * This file contains a class for obstacle localization using the worldstate of the GTCam.
    00005 *
    00006 * @author <a href="mailto:timo.diekmann@uni-dortmund.de">Timo Diekmann</a>
    00007 */
    00008
    00009 /*
    00010 GTCamObstaclesLocator:
    00011
    00012 The GTCam solution for the module ObstaclesLocator uses the positions and orientations of the three teammates and four opponents of the GTCamWorldState to model them as obstacles in the ObstaclesModel. Further the 16 segments of the border including the goals are modeled. The ObstacleModell is filled relative to the own position determined by robotPose. To use the information of the GTCam for the own pose, you have to switch the SelfLocator to GTCam!
    00013
    00014 These attributes of obstaclesModel are calculated:
    00015   - distance[]: the distance to the nearest border, goal, or player for each sector
    00016     range: [0..8000)
    00017   - obstacleType[]: the type of the nearest obstacle for each sector
    00018     range: {ObstaclesPercept::border, ...::goal, ...::teammate, ...::opponent, ...::unknown}
    00019     remark: normaly no sector is marked with the type ObstaclesPercept::unknown. This only happens if the robot is standing very close to one border or outside the field
    00020   - frameNumber: set to cameraMatrix.frameNumber
    00021   - distanceToFreePartOfGoal[]: the two distances to free part of goals
    00022     range: [0..8000)
    00023     remark: 0 is the index for own goal, 1 the index for opponent goal
    00024     Attention: the calculation of this and the following three goal-attributes may not be correct, if the robot is posed far back in one goal!
    00025   - widthOfFreePartOfGoal[]: the two widths of free part of goals
    00026     range: [2pi/90..2pi)
    00027     remark: see distanceToFreePartOfGoal[]
    00028     Attention: see distanceToFreePartOfGoal[]!
    00029   - angleToFreePartOfGoal[]: the two angles to the center of the free part of goals
    00030     range: (-pi..pi)
    00031     remark: see distanceToFreePartOfGoal[]
    00032     Attention: see distanceToFreePartOfGoal[]!
    00033   - angleToFreePartOfGoalWasDetermined[]: the two boolean values, whether there is a free part of the goals found
    00034     range: {true, false}
    00035     remark: see distanceToFreePartOfGoal[]
    00036     Attention: see distanceToFreePartOfGoal[]!
    00037   - distanceToNextFreeTeammate: the distance to the next teammate with no obstacle between
    00038     range: [0..8000)
    00039     remark: this calculation is based on GTCam data, not on the PlayersPoseCollection
    00040   - angleToNextFreeTeammateWasDetermined: the angle to the next teammate with no obstacle between
    00041     range: (-pi..pi)
    00042     remark: see distanceToNextFreeTeammate
    00043   - corridorInFront: length of a free rectangular corridor in front of the robot
    00044     range: [0..8000)
    00045     remark: aproximated sector width: 280 mm within a length of ca. 2000 mm
    00046     Attention: only aproximated by observing four sectors!
    00047
    00048 These attributes of obstaclesModel are not set:
    00049   - bodyPSD: it is set in the constructor of ObstaclesModel to 0.0
    00050
    00051 Proceeding:
    00052   For every border the sectors are determined. For every sector the distance to the border is determined.
    00053   For each of the three teammates and four opponents the sectors are (aproximately) determined. For every sector the distance to the circle around the player is (aproximately) determined.
    00054   When the sector modell is filled, the "XABSL-Symbols" are calculated based on the sector modell and GTCam data.
    00055 */
    00056
    00057 #include "GTCamObstaclesLocator.h"
    00058 #include "Tools/Player.h"
    00059 #include "Platform/SystemCall.h"
    00060 #include "Tools/FieldDimensions.h"
    00061
    00062 GTCamObstaclesLocator::GTCamObstaclesLocator(const ObstaclesLocatorInterfaces& interfaces)
    00063 : ObstaclesLocator(interfaces)
    00064 {
    00065   minStep = 0.1/ObstaclesModel::numOfSectors;
    00066   sectorWidth = pi2/ObstaclesModel::numOfSectors;
    00067   frontSectorNr = obstaclesModel.getSectorFromAngle(0.0);
    00068   deltaSector45Nr = obstaclesModel.getSectorFromAngle(pi/4)-frontSectorNr;
    00069 }
    00070
    00071
    00072 void GTCamObstaclesLocator::execute()
    00073 {
    00074   //long myTime = SystemCall::getCurrentSystemTime();
    00075   obstaclesModel.frameNumber = cameraMatrix.frameNumber;
    00076
    00077   rx = robotPose.translation.x;
    00078   ry = robotPose.translation.y;
    00079   ra = robotPose.rotation;
    00080   int sector;
    00081   for(sector = 0; sector < ObstaclesModel::numOfSectors; sector++)
    00082   {
    00083     obstaclesModel.obstacleType[sector] = ObstaclesPercept::unknown;
    00084     obstaclesModel.distance[sector] = 8000;
    00085   }
    00086
    00087   // model borders and goals
    00088   modelLine(false, xPosOpponentGoal, yPosRightGoal, yPosLeftGoal, ObstaclesPercept::goal);
    00089   modelLine(false, xPosOwnGoal, yPosRightGoal, yPosLeftGoal, ObstaclesPercept::goal);
    00090   modelLine(true, yPosLeftGoal, xPosOpponentGoalpost, xPosOpponentGoal, ObstaclesPercept::goal);
    00091   modelLine(true, yPosLeftGoal, xPosOwnGoal, xPosOwnGoalpost, ObstaclesPercept::goal);
    00092   modelLine(true, yPosRightGoal, xPosOpponentGoalpost, xPosOpponentGoal, ObstaclesPercept::goal);
    00093   modelLine(true, yPosRightGoal, xPosOwnGoal, xPosOwnGoalpost, ObstaclesPercept::goal);
    00094   modelLine(true, yPosLeftGoal, xPosOpponentGroundline, xPosOpponentGoalpost, ObstaclesPercept::border);
    00095   modelLine(true, yPosLeftGoal, xPosOwnGoalpost, xPosOwnGroundline, ObstaclesPercept::border);
    00096   modelLine(true, yPosRightGoal, xPosOpponentGroundline, xPosOpponentGoalpost, ObstaclesPercept::border);
    00097   modelLine(true, yPosRightGoal, xPosOwnGoalpost, xPosOwnGroundline, ObstaclesPercept::border);
    00098   modelLine(false, xPosOpponentGroundline, yPosRightSideline, yPosRightGoal, ObstaclesPercept::border);
    00099   modelLine(false, xPosOwnGroundline, yPosRightSideline, yPosRightGoal, ObstaclesPercept::border);
    00100   modelLine(false, xPosOpponentGroundline, yPosLeftGoal, yPosLeftSideline, ObstaclesPercept::border);
    00101   modelLine(false, xPosOwnGroundline, yPosLeftGoal, yPosLeftSideline, ObstaclesPercept::border);
    00102   modelLine(true, yPosLeftSideline, xPosOwnGroundline, xPosOpponentGroundline, ObstaclesPercept::border);
    00103   modelLine(true, yPosRightSideline, xPosOwnGroundline, xPosOpponentGroundline, ObstaclesPercept::border);
    00104
    00105   const int me = getPlayer().getPlayerNumber();
    00106   const int color = getPlayer().getTeamColor();
    00107   const int oppColor = (color == Player::red ? Player::blue : Player:: red);
    00108   Vector2<double> pos;
    00109
    00110   // model other players
    00111   int i;
    00112   for (i = 0; i < 4; i++)
    00113   {
    00114     if (i != me)
    00115     {
    00116       pos = gtCamWorldState.getPlayerPos(i, color);
    00117       modelRobot(pos.x, pos.y, gtCamWorldState.getPlayerOrientation(i, color), ObstaclesPercept::teammate);
    00118     }
    00119     pos = gtCamWorldState.getPlayerPos(i, oppColor);
    00120     modelRobot(pos.x, pos.y, gtCamWorldState.getPlayerOrientation(i, oppColor), ObstaclesPercept::opponent);
    00121   }
    00122
    00123   // find nextFreeTeammate
    00124   obstaclesModel.angleToNextFreeTeammate = 0.0;
    00125   obstaclesModel.distanceToNextFreeTeammate = 8000.0 * 8000.0;
    00126   obstaclesModel.angleToNextFreeTeammateWasDetermined = false;
    00127   for (i = 0; i < 4; i++)
    00128   {
    00129     if (i != me)
    00130     {
    00131       pos = gtCamWorldState.getPlayerPos(i, color);
    00132       double dx = pos.x - rx;
    00133       double dy = pos.y - ry;
    00134       double angle = atan2(dy, dx) - ra;
    00135       int sector = obstaclesModel.getSectorFromAngle(angle);
    00136       if (obstaclesModel.obstacleType[sector] == ObstaclesPercept::teammate)
    00137       {
    00138         double d = dx * dx + dy * dy;
    00139         if (d < obstaclesModel.distanceToNextFreeTeammate)
    00140         {
    00141           // normalize angle:
    00142           if (angle < -pi) angle += pi2;
    00143           if (angle >  pi) angle -= pi2;
    00144           obstaclesModel.angleToNextFreeTeammate = angle;
    00145           obstaclesModel.distanceToNextFreeTeammate = d;
    00146           obstaclesModel.angleToNextFreeTeammateWasDetermined = true;
    00147         }
    00148       }
    00149     }
    00150   }
    00151   obstaclesModel.distanceToNextFreeTeammate = sqrt(obstaclesModel.distanceToNextFreeTeammate);
    00152
    00153   // calculate angleToFreePartOfGoal...
    00154   int gs[2] = {0, 0}; // goalSector
    00155   int gsc = 0; // goalSectorCount
    00156   int gscmax[2] = {0, 0};
    00157   int gi = 0; // goal index: 0 = own goal, 1 = opponent goal // Is this correct??
    00158   obstaclesModel.distanceToFreePartOfGoal[0] = 8000.0;
    00159   obstaclesModel.distanceToFreePartOfGoal[1] = 8000.0;
    00160   for(sector = 0; sector < ObstaclesModel::numOfSectors; sector++)
    00161   {
    00162     if (obstaclesModel.obstacleType[sector] == ObstaclesPercept::goal)
    00163     {
    00164       // if we saw no goal in the last sector, we have to determine which goal we see in the current sector
    00165       if (gsc == 0)
    00166       {
    00167         int degree = (int)((180.0/pi)*((sector+0.5)*sectorWidth - pi + robotPose.rotation));
    00168         degree = (degree + 360 + 180) % 360 - 180;
    00169         if (rx > 0)
    00170         {
    00171           if (degree > 120  || degree < -120)
    00172             gi = 0;
    00173           else
    00174             gi = 1;
    00175         }
    00176         else
    00177         {
    00178           if (degree > 60 || degree < -60)
    00179             gi = 0;
    00180           else
    00181             gi = 1;
    00182         }
    00183       }
    00184       ++gsc;
    00185       if (gsc > gscmax[gi])
    00186       {
    00187         gscmax[gi] = gsc;
    00188         gs[gi] = sector;
    00189       }
    00190       if (obstaclesModel.distance[sector] < obstaclesModel.distanceToFreePartOfGoal[gi])
    00191         obstaclesModel.distanceToFreePartOfGoal[gi] = obstaclesModel.distance[sector];
    00192     }
    00193     else
    00194     {
    00195       gsc = 0;
    00196     }
    00197   }
    00198   if (gsc > 0)
    00199   {
    00200     sector = 0;
    00201     while (obstaclesModel.obstacleType[sector++] == ObstaclesPercept::goal)
    00202       ++gsc;
    00203     if (gsc > gscmax[gi])
    00204     {
    00205       gscmax[gi] = gsc;
    00206       gs[gi] = sector-2;
    00207     }
    00208   }
    00209   for (gi = 0; gi < 2; gi++)
    00210   {
    00211     if (gscmax[gi] > 0)
    00212     {
    00213       obstaclesModel.widthOfFreePartOfGoal[gi] = gscmax[gi]*sectorWidth;
    00214       obstaclesModel.angleToFreePartOfGoal[gi] = (gs[gi]+1)*sectorWidth - pi -
    00215         (obstaclesModel.widthOfFreePartOfGoal[gi])/2.0;
    00216       // normalize angleToFreePartOfGoal
    00217       if (obstaclesModel.angleToFreePartOfGoal[gi] < -pi)
    00218         obstaclesModel.angleToFreePartOfGoal[gi] += pi2;
    00219       obstaclesModel.angleToFreePartOfGoalWasDetermined[gi] = true;
    00220       obstaclesModel.lastTimeFreePartOfGoalWasDetermined[gi] = SystemCall::getCurrentSystemTime();
    00221     }
    00222     else
    00223     {
    00224       obstaclesModel.angleToFreePartOfGoalWasDetermined[gi] = false;
    00225     }
    00226   }
    00227
    00228   // approximation for obstaclesModel.corridorInFront:
    00229   obstaclesModel.corridorInFront = obstaclesModel.distance[frontSectorNr-1];
    00230   if (obstaclesModel.distance[frontSectorNr+1] < obstaclesModel.corridorInFront)
    00231     obstaclesModel.corridorInFront = obstaclesModel.distance[frontSectorNr+1];
    00232   if (obstaclesModel.distance[frontSectorNr+deltaSector45Nr] < 200)
    00233   {
    00234     int dist = (int)(obstaclesModel.distance[frontSectorNr+deltaSector45Nr] * 0.707);
    00235     if (dist < obstaclesModel.corridorInFront)
    00236       obstaclesModel.corridorInFront = dist;
    00237   }
    00238   if (obstaclesModel.distance[frontSectorNr-deltaSector45Nr] < 200)
    00239   {
    00240     int dist = (int)(obstaclesModel.distance[frontSectorNr-deltaSector45Nr] * 0.707);
    00241     if (dist < obstaclesModel.corridorInFront)
    00242       obstaclesModel.corridorInFront = dist;
    00243   }
    00244   //OUTPUT(idText,text,"dist: " << obstaclesModel.corridorInFront);
    00245
    00246   //OUTPUT(idText,text,"time: " << SystemCall::getTimeSince(myTime));
    00247 }
    00248
    00249 bool GTCamObstaclesLocator::handleMessage(InMessage& message)
    00250 {
    00251   return false;
    00252 }
    00253
    00254 /**
    00255  * Models a line with constant x or constant y-value as an obstacle.
    00256  * If you want to model a line with constant x-value (a horizontal line) you have to call modelLine with:
    00257  *  - vert = false
    00258  *  - x = the constant x-value of the line
    00259  *  - sy with the minimum y-value of the line
    00260  *  - ey with the maximum y-value of the line
    00261  * If you want to model a line with constant y-value (a verlical line) you have to call modelLine with:
    00262  *  - vert = true
    00263  *  - x = the constant y-value of the line
    00264  *  - sy with the minimum x-value of the line
    00265  *  - ey with the maximum x-value of the line
    00266  * In any case you determine the type of the obstacle with
    00267  *  - type (border or goal, for example)
    00268  * Attention: make sure that sy is less than ey, or uncomment the swap-passage in the method!
    00269  */
    00270 void GTCamObstaclesLocator::modelLine(bool vert, double x, double sy, double ey, ObstaclesPercept::ObstacleType type)
    00271 {
    00272   double r = (vert ? ry : rx);
    00273   // break, if robot is standing to close on line
    00274   if (x-1 < r && x+1 > r) return;
    00275   /* swap-passage:
    00276   if (sy > ey)
    00277   {
    00278     double h = sy;
    00279     sy = ey;
    00280     ey = h;
    00281     OUTPUT(idText,text,"swap!");
    00282   }
    00283   // now sy is less or equal than ey
    00284   */
    00285   const double dx = x - r;
    00286   double startAngle = 0;
    00287   double endAngle = 0;
    00288   if (vert)
    00289   {
    00290     startAngle = atan2(dx, sy - rx);
    00291     endAngle = atan2(dx, ey - rx);
    00292   } else {
    00293     startAngle = atan2(sy - ry, dx);
    00294     endAngle = atan2(ey - ry, dx);
    00295   }
    00296   int sector = obstaclesModel.getSectorFromAngle(startAngle - ra);
    00297   int endSector = obstaclesModel.getSectorFromAngle(endAngle - ra);
    00298
    00299   double subtract = 0;
    00300   if ((dx < 0) ^ vert)
    00301   {
    00302     int hi = sector;
    00303     sector = endSector;
    00304     endSector = hi;
    00305     double hd = startAngle;
    00306     startAngle = endAngle;
    00307     endAngle = hd;
    00308     subtract = pi;
    00309   }
    00310   startAngle += (int)vert * pi_2 - subtract;
    00311   endAngle += (int)vert * pi_2 - subtract;
    00312   // normalize angles
    00313   if (startAngle < -pi) startAngle += pi2;
    00314   if (startAngle >  pi) startAngle -= pi2;
    00315   if (endAngle < -pi) endAngle += pi2;
    00316   if (endAngle >  pi) endAngle -= pi2;
    00317   if (startAngle < -pi) startAngle += pi2;
    00318   if (startAngle >  pi) startAngle -= pi2;
    00319   if (endAngle < -pi) endAngle += pi2;
    00320   if (endAngle >  pi) endAngle -= pi2;
    00321
    00322   double angle = obstaclesModel.getAngleOfSector(sector) + ra - subtract + (int)vert * pi_2;
    00323   // normalize angle
    00324   if (angle < -pi) angle += pi2;
    00325   if (angle >  pi) angle -= pi2;
    00326   do
    00327   {
    00328     double d = angle;
    00329     if (d >= startAngle && d <= endAngle)
    00330     {
    00331       // calculate distance
    00332       d = dx*tan(d);
    00333       d *= d;
    00334       d += dx*dx;
    00335       d = sqrt(d);
    00336       if (obstaclesModel.distance[sector] > d)
    00337       {
    00338         obstaclesModel.distance[sector] = (int)d;
    00339         obstaclesModel.obstacleType[sector] = type;
    00340       }
    00341     }
    00342     if (sector == endSector) break;
    00343     if (++sector == ObstaclesModel::numOfSectors) sector = 0;
    00344     angle += sectorWidth; // angle is already normalized
    00345   } while (true);
    00346 }
    00347
    00348 /**
    00349  * Models a robot as a circular obstacle
    00350  *  - px = the x-coordinate of the robots neck
    00351  *  - py = the y-coordinate of the robots neck
    00352  *  - rotation = the rotation of the robot
    00353  *  - type = the type of the obstacle (teammate or opponent, for example)
    00354  * Remark: This Method uses the attribute obstaclesDist, wich determines the distance between the neck of the robot and the center of the circle around the robot.
    00355  */
    00356 void GTCamObstaclesLocator::modelRobot(double px, double py, double rotation, ObstaclesPercept::ObstacleType type)
    00357 {
    00358   px -= obstaclesDist*cos(rotation);
    00359   py -= obstaclesDist*sin(rotation);
    00360   double relx = px-rx;
    00361   double rely = py-ry;
    00362   double dist = sqrt(relx*relx+rely*rely);
    00363   double angle = atan2(rely, relx)-ra;
    00364   double openangle = atan(obstaclesRadius/dist)*(1+exp(-dist/obstaclesRadius));
    00365   int sl = obstaclesModel.getSectorFromAngle(angle+openangle);
    00366   int sr = obstaclesModel.getSectorFromAngle(angle-openangle);
    00367   int sector = sl;
    00368   do
    00369   {
    00370     double x = angle-obstaclesModel.getAngleOfSector(sector);
    00371     if (x < -pi)
    00372       x += 2*pi;
    00373     if (x > pi)
    00374       x -= 2*pi;
    00375     x /= openangle;
    00376     x *= x;
    00377     if (x <= 1)
    00378     {
    00379       int d = (int)(dist - obstaclesRadius*sqrt(1.0-x));
    00380       if (d < 100) d = 100;
    00381       if (obstaclesModel.distance[sector] > d)
    00382       {
    00383         obstaclesModel.distance[sector] = d;
    00384         obstaclesModel.obstacleType[sector] = type;
    00385       }
    00386     }
    00387     if (sector == sr)
    00388       break;
    00389     if (--sector < 0)
    00390       sector = ObstaclesModel::numOfSectors-1;
    00391   } while (true);
    00392 }