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

# Modules/BehaviorControl/CommonXabsl2Symbols/AngleSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file AngleSymbols.cpp
    00003 *
    00004 * Implementation of class AngleSymbols.
    00005 *
    00006 * @author Matthias Jüngel
    00007 */
    00008
    00009 #include "AngleSymbols.h"
    00010 #include "Platform/SystemCall.h"
    00011 #include "Tools/Math/Geometry.h"
    00012 #include "Tools/FieldDimensions.h"
    00013 #include "Tools/Debugging/DebugDrawings.h"
    00014 #include "Tools/StringFunctions.h"
    00015
    00016 AngleSymbols::AngleSymbols(const BehaviorControlInterfaces& interfaces)
    00017 : BehaviorControlInterfaces(interfaces)
    00018 {
    00019   angleShownByLEDs = undefined;
    00020   angles[undefined] = 0;
    00021 }
    00022
    00023
    00024 void AngleSymbols::registerSymbols(Xabsl2Engine& engine)
    00025 {
    00026   // localisation based:
    00027   engine.registerDecimalInputSymbol("angle.angle-to-center-of-field",this,
    00028     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToCenterOfField);
    00029
    00030   engine.registerDecimalInputSymbol("angle.angle-to-bb-supporter",this,
    00031     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToBbSupporter);
    00032
    00033   engine.registerDecimalInputSymbol("angle.angle-to-opponent-goal",this,
    00034     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToOpponentGoal);
    00035
    00036   engine.registerDecimalInputSymbol("angle.angle-to-point-behind-opponent-goal",this,
    00037     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToPointBehindOpponentGoal);
    00038
    00039   engine.registerDecimalInputSymbol("angle.angle-to-center-of-opponent-penalty-area",this,
    00040     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToCenterOfOpponentPenaltyArea);
    00041
    00042   // vision + odometry (non seen cases) based
    00043   engine.registerDecimalInputSymbol("angle.seen-angle-to-opponent-goal",this,
    00044     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getSeenAngleToOpponentGoal);
    00045   engine.registerDecimalInputSymbol("angle.time-since-last-seen-opponent-goal",this,
    00046     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getTimeSinceLastSeenOpponentGoal);
    00047   engine.registerBooleanInputSymbol("angle.trust-seen-angle-to-opponent-goal",this,
    00048     (bool (Xabsl2FunctionProvider::*)())&AngleSymbols::getTrustOfSeenAngleToOpponentGoal);
    00049
    00050
    00051
    00052
    00053   // ...
    00054
    00055   // combined: vision + localisation based
    00056   engine.registerDecimalInputSymbol("angle.perfect-angle-to-opponent-goal",this,
    00057     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getPerfectAngleToOpponentGoal);
    00058
    00059   engine.registerDecimalInputSymbol("angle.best-angle-to-opponent-goal",this,
    00060     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleToOpponentGoal);
    00061
    00062   engine.registerDecimalInputSymbol("angle.time-since-opponent-goal-detected",this,
    00063     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getTimeSinceOpponentGoalDetection);
    00064
    00065   engine.registerDecimalInputSymbol("angle.best-width-of-opponent-goal",this,
    00066     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestWidthOfOpponentGoal);
    00067
    00068   engine.registerDecimalInputSymbol("angle.best-angle-to-opponent-goal-no-obstacles",this,
    00069     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleToOpponentGoalNoObstacles);
    00070
    00071   engine.registerDecimalInputSymbol("angle.best-width-of-opponent-goal-no-obstacles",this,
    00072     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestWidthOfOpponentGoalNoObstacles);
    00073
    00074   engine.registerDecimalInputSymbol("angle.best-angle-away-from-own-goal",this,
    00075     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleAwayFromOwnGoal);
    00076
    00077   engine.registerDecimalInputSymbol("angle.best-angle-away-from-own-goal-no-obstacles",this,
    00078     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleAwayFromOwnGoalNoObstacles);
    00079
    00080   engine.registerDecimalInputSymbol("angle.goal-kick-angle",this,
    00081     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getGoalieGoalKickAngle);
    00082
    00083   engine.registerDecimalInputSymbol("angle.test-angle",this,
    00084     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getTestAngle);
    00085
    00086
    00087   engine.registerEnumeratedOutputSymbol("angle.angle-shown-by-leds",(int*)&angleShownByLEDs);
    00088
    00089   int i;
    00090   char s[256];
    00091   for (i = 0; i < numberOfAngles; i++)
    00092   {
    00093     sprintf(s,"angle.");
    00094     getXmlString(s+strlen(s),getAngleName((Angles)i));
    00095     engine.registerEnumeratedOutputSymbolEnumElement("angle.angle-shown-by-leds",s,i);
    00096   }
    00097 }
    00098
    00099 void AngleSymbols::update()
    00100 {
    00101   calculateLocalisationBasedAngles();
    00102   calculateVisionBasedAngles();
    00103   calculateCombinedAngles();
    00104   setTestAngle();
    00105   drawAngleShownByLeds();
    00106   drawAngles();
    00107 }
    00108
    00109 // localisation based
    00110 double AngleSymbols::getAngleToCenterOfField()              {angleIsUsedByTheCurrentBehavior[angleToCenterOfField] = true; return toDegrees(normalize(angles[angleToCenterOfField])); }
    00111 double AngleSymbols::getAngleToBbSupporter()              {angleIsUsedByTheCurrentBehavior[angleToBbSupporter] = true; return toDegrees(normalize(angles[angleToBbSupporter])); }
    00112 double AngleSymbols::getAngleToOpponentGoal()               {angleIsUsedByTheCurrentBehavior[angleToOpponentGoal] = true; return toDegrees(normalize(angles[angleToOpponentGoal])); }
    00113 double AngleSymbols::getAngleToPointBehindOpponentGoal()    {angleIsUsedByTheCurrentBehavior[angleToPointBehindOpponentGoal] = true; return toDegrees(normalize(angles[angleToPointBehindOpponentGoal])); }
    00114 double AngleSymbols::getAngleToCenterOfOpponentPenaltyArea() {angleIsUsedByTheCurrentBehavior[angleToCenterOfOpponentPenaltyArea] = true; return toDegrees(normalize(angles[angleToCenterOfOpponentPenaltyArea])); }
    00115
    00116 // combined: vision + odometry modelling + localisation
    00117 double AngleSymbols::getPerfectAngleToOpponentGoal()            {angleIsUsedByTheCurrentBehavior[perfectAngleToOpponentGoal] = true; return toDegrees(normalize(angles[perfectAngleToOpponentGoal])); }
    00118 // combined: vision + localisation
    00119 double AngleSymbols::getBestAngleToOpponentGoal()            {angleIsUsedByTheCurrentBehavior[bestAngleToOpponentGoal] = true; return toDegrees(normalize(angles[bestAngleToOpponentGoal])); }
    00120 double AngleSymbols::getBestAngleToOpponentGoalNoObstacles() {angleIsUsedByTheCurrentBehavior[bestAngleToOpponentGoalNoObstacles] = true; return toDegrees(normalize(angles[bestAngleToOpponentGoalNoObstacles])); }
    00121 double AngleSymbols::getBestWidthOfOpponentGoal()            {angleIsUsedByTheCurrentBehavior[bestWidthOfOpponentGoal] = true; return toDegrees(normalize(angles[bestWidthOfOpponentGoal])); }
    00122 double AngleSymbols::getBestWidthOfOpponentGoalNoObstacles() {angleIsUsedByTheCurrentBehavior[bestWidthOfOpponentGoalNoObstacles] = true; return toDegrees(normalize(angles[bestWidthOfOpponentGoalNoObstacles])); }
    00123 double AngleSymbols::getBestAngleAwayFromOwnGoal()           {angleIsUsedByTheCurrentBehavior[bestAngleAwayFromOwnGoal] = true; return toDegrees(normalize(angles[bestAngleAwayFromOwnGoal])); }
    00124 double AngleSymbols::getBestAngleAwayFromOwnGoalNoObstacles(){angleIsUsedByTheCurrentBehavior[bestAngleAwayFromOwnGoalNoObstacles] = true; return toDegrees(normalize(angles[bestAngleAwayFromOwnGoalNoObstacles])); }
    00125 double AngleSymbols::getGoalieGoalKickAngle()                {angleIsUsedByTheCurrentBehavior[goalieGoalKickAngle] = true; return toDegrees(normalize(angles[goalieGoalKickAngle])); }
    00126
    00127 double AngleSymbols::getTestAngle()                           {angleIsUsedByTheCurrentBehavior[testAngle] = true; return toDegrees(normalize(angles[testAngle])); }
    00128 double AngleSymbols::getSeenAngleToOpponentGoal()            {angleIsUsedByTheCurrentBehavior[seenAngleToOpponentGoal] = true; return toDegrees(normalize(angles[seenAngleToOpponentGoal])); }
    00129
    00130 void AngleSymbols::calculateLocalisationBasedAngles()
    00131 {
    00132
    00133   angles[angleToCenterOfField]     = Geometry::angleTo(robotPose, Vector2<double>(0.0,0.0));
    00134   angles[angleToBbSupporter]     = Geometry::angleTo(robotPose, Vector2<double>((xPosOpponentGroundline - 300),0.0));
    00135   angles[angleToCenterOfOpponentPenaltyArea] = Geometry::angleTo(robotPose, Vector2<double>((xPosOpponentPenaltyArea + xPosOpponentGroundline) / 2.0, 0.0));
    00136   angles[angleToLeftOpponentGoalPost]  = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosLeftGoal));
    00137   angles[angleToRightOpponentGoalPost] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosRightGoal));
    00138   if(angles[angleToLeftOpponentGoalPost] < angles[angleToRightOpponentGoalPost])
    00139   {
    00140     angles[angleToLeftOpponentGoalPost] += pi2;
    00141   }
    00142   angles[angleToOpponentGoal] = (angles[angleToLeftOpponentGoalPost] + angles[angleToRightOpponentGoalPost]) / 2.0;
    00143
    00144   angles[angleToLeftOpponentGoalCorner] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGoal,yPosLeftGoal));
    00145   angles[angleToRightOpponentGoalCorner] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGoal,yPosRightGoal));
    00146   if(angles[angleToLeftOpponentGoalCorner] < angles[angleToRightOpponentGoalCorner])
    00147   {
    00148     angles[angleToLeftOpponentGoalCorner] += pi2;
    00149   }
    00150   angles[angleToPointBehindOpponentGoal] = (angles[angleToLeftOpponentGoalCorner] + angles[angleToRightOpponentGoalCorner]) / 2.0;
    00151 }
    00152
    00153 void AngleSymbols::calculateVisionBasedAngles()
    00154 {
    00155   angles[angleToFreePartOfOpponentGoal] = obstaclesModel.angleToFreePartOfGoal[ObstaclesModel::opponentGoal];
    00156   angles[widthOfFreePartOfOpponentGoal] = obstaclesModel.widthOfFreePartOfGoal[ObstaclesModel::opponentGoal];
    00157   angles[angleToFreePartOfOpponentGoalLeft] =
    00158     angles[angleToFreePartOfOpponentGoal] + angles[widthOfFreePartOfOpponentGoal] / 2.0;
    00159   angles[angleToFreePartOfOpponentGoalRight] =
    00160     angles[angleToFreePartOfOpponentGoal] - angles[widthOfFreePartOfOpponentGoal] / 2.0;
    00161   angles[seenAngleToOpponentGoal] = obstaclesModel.angleToSeenGoal[ObstaclesModel::opponentGoal];
    00162 }
    00163 double AngleSymbols::getTimeSinceLastSeenOpponentGoal()
    00164 {
    00165   return (double) SystemCall::getTimeSince(obstaclesModel.timeOfSeenGoal[ObstaclesModel::opponentGoal]);
    00166 }
    00167
    00168 double AngleSymbols::getTimeSinceOpponentGoalDetection()
    00169 {
    00170   return (double) SystemCall::getTimeSince(obstaclesModel.lastTimeFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal]);
    00171 }
    00172
    00173 bool AngleSymbols::getTrustOfSeenAngleToOpponentGoal()
    00174 {
    00175   return obstaclesModel.trustSeenGoal[ObstaclesModel::opponentGoal];
    00176 }
    00177 void AngleSymbols::calculateCombinedAngles()
    00178 {
    00179   // perfect angle to opponent goal
    00180   /*if(obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal])
    00181   {
    00182     angles[perfectAngleToOpponentGoal] = angles[angleToFreePartOfOpponentGoal];
    00183     angleUsedForPerfectAngle = angleToFreePartOfOpponentGoal;
    00184   }
    00185   else */
    00186   if( SystemCall::getCurrentSystemTime() - obstaclesModel.timeOfSeenGoal[ObstaclesModel::opponentGoal] < 10000 && obstaclesModel.trustSeenGoal[ObstaclesModel::opponentGoal])
    00187   {
    00188     angles[perfectAngleToOpponentGoal] = angles[seenAngleToOpponentGoal];
    00189     angleUsedForPerfectAngle = seenAngleToOpponentGoal;
    00190   }
    00191   else
    00192   {
    00193     angles[perfectAngleToOpponentGoal] = angles[angleToOpponentGoal];
    00194     angleUsedForPerfectAngle = angleToOpponentGoal;
    00195   }
    00196
    00197   //best angle to opponent goal ///////////
    00198   if(obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal])
    00199   {
    00200     angles[bestAngleToOpponentGoalNoObstacles] = angles[angleToFreePartOfOpponentGoal];
    00201     angles[bestWidthOfOpponentGoalNoObstacles] = angles[widthOfFreePartOfOpponentGoal];
    00202   }
    00203   else
    00204   {
    00205     angles[bestAngleToOpponentGoalNoObstacles] = angles[angleToOpponentGoal];
    00206     angles[bestWidthOfOpponentGoalNoObstacles] = 0;
    00207   }
    00208
    00209   /*
    00210   * this was nice but will lead to shooting ball outside the field. hopefully one day some kind of passing
    00211   * will take care of this.
    00212 */
    00213   if(FieldDimensions::distanceToOpponentPenaltyArea(robotPose.translation) > 300)
    00214   {
    00215     int distanceToNextTeammate = 10000;
    00216     double angleToNextTeammate = 0;
    00217     bool foundTeammate = false;
    00218
    00219     for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
    00220     {
    00221       PlayerPose currentPose = playerPoseCollection.getOwnPlayerPose(i);
    00222       int currentDistance = (int)((currentPose.getPose().translation - robotPose.translation).abs());
    00223       double currentAngle = normalize(Geometry::angleTo(robotPose, currentPose.getPose().translation));
    00224
    00225       if(currentDistance < distanceToNextTeammate)
    00226       {
    00227         foundTeammate = true;
    00228         angleToNextTeammate = currentAngle;
    00229         distanceToNextTeammate = currentDistance;
    00230       }
    00231     }
    00232
    00233     double leftAngleToOpponentGoal = obstaclesModel.getAngleOfNextFreeSectorLeft(pi/8.0, angles[bestAngleToOpponentGoalNoObstacles], 600);
    00234     double rightAngleToOpponentGoal = obstaclesModel.getAngleOfNextFreeSectorRight(pi/8.0, angles[bestAngleToOpponentGoalNoObstacles], 600);
    00235
    00236     if(foundTeammate &&
    00237       fabs(
    00238       fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - leftAngleToOpponentGoal)) -
    00239       fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - rightAngleToOpponentGoal))
    00240       )
    00241       < fromDegrees(20) )
    00242     {
    00243       if(angleToNextTeammate > 0) angles[bestAngleToOpponentGoal] = leftAngleToOpponentGoal;
    00244       else angles[bestAngleToOpponentGoal] = rightAngleToOpponentGoal;
    00245     }
    00246     else
    00247     {
    00248       if(
    00249         fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - leftAngleToOpponentGoal)) <
    00250         fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - rightAngleToOpponentGoal))
    00251         ) angles[bestAngleToOpponentGoal] = leftAngleToOpponentGoal;
    00252       else angles[bestAngleToOpponentGoal] = rightAngleToOpponentGoal;
    00253     }
    00254
    00255     if(fabs(angles[bestAngleToOpponentGoal] - angles[bestAngleToOpponentGoalNoObstacles]) > fromDegrees(70))
    00256     {
    00257       angles[bestAngleToOpponentGoal] = angles[bestAngleToOpponentGoalNoObstacles];
    00258     }
    00259     angles[bestWidthOfOpponentGoal] = 0;
    00260   }
    00261   else
    00262   {
    00263     angles[bestAngleToOpponentGoal] = angles[bestAngleToOpponentGoalNoObstacles];
    00264     angles[bestWidthOfOpponentGoal] = angles[bestWidthOfOpponentGoalNoObstacles];
    00265   }
    00266   //best angle away from own goal ///////////
    00267   if(obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::ownGoal])
    00268   {
    00269     angles[bestAngleAwayFromOwnGoalNoObstacles] = obstaclesModel.angleToFreePartOfGoal[ObstaclesModel::ownGoal] + pi;
    00270   }
    00271   else
    00272   {
    00273     double angleToLeftOwnGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosLeftGoal));
    00274     double angleToRightOwnGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosRightGoal));
    00275
    00276     if(angleToLeftOwnGoalPost < angleToRightOwnGoalPost)
    00277     {
    00278       angleToLeftOwnGoalPost += pi2;
    00279     }
    00280     angles[bestAngleAwayFromOwnGoalNoObstacles] = (angleToLeftOwnGoalPost + angleToRightOwnGoalPost) / 2.0;
    00281   }
    00282
    00283   if(
    00284     robotPose.translation.y > yPosLeftSideline - 350 ||
    00285     robotPose.translation.y < yPosRightSideline + 350
    00286     )
    00287   {
    00288     angles[bestAngleAwayFromOwnGoalNoObstacles] = normalize(-robotPose.getAngle());
    00289   }
    00290
    00291   angles[bestAngleAwayFromOwnGoal] = angles[bestAngleAwayFromOwnGoalNoObstacles];
    00292 /*
    00293   // goal kick angle ///////////
    00294   double angle;
    00295 //  if(motionInfo.executedMotionRequest.walkType != MotionRequest::turnWithBall)
    00296   if(headControlMode.headControlMode != HeadControlMode::catchBall)
    00297   {
    00298     angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchLeftAndRight);
    00299   }
    00300   else if(motionInfo.executedMotionRequest.walkRequest.walkParams.rotation < 0)
    00301   {
    00302     angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchRight);
    00303   }
    00304   else
    00305   {
    00306     angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchLeft);
    00307   }
    00308
    00309   angles[goalieGoalKickAngle] = normalize(angle + robotPose.getAngle());
    00310   if(toDegrees(angles[goalieGoalKickAngle]) > 65) angles[goalieGoalKickAngle] = fromDegrees(65);
    00311   if(toDegrees(angles[goalieGoalKickAngle]) < -65) angles[goalieGoalKickAngle] = fromDegrees(-65);
    00312   angles[goalieGoalKickAngle] = normalize(angles[goalieGoalKickAngle] - robotPose.getAngle());
    00313
    00314   if(robotPose.rotation > fromDegrees(95) || robotPose.rotation < fromDegrees(-95) )
    00315   {
    00316     angles[goalieGoalKickAngle] = angles[bestAngleAwayFromOwnGoal];
    00317   }
    00318 */
    00319   angles[goalieGoalKickAngle] = -robotPose.getAngle();
    00320 }
    00321
    00322 double AngleSymbols::getAngle(Angles id)
    00323 {
    00324   return angles[id];
    00325 }
    00326
    00327 void AngleSymbols::drawAngleShownByLeds()
    00328 {
    00329   LINE(behavior_kickAngles,
    00330     robotPose.getPose().translation.x,
    00331     robotPose.getPose().translation.y,
    00332     robotPose.getPose().translation.x + cos(angles[angleShownByLEDs] + robotPose.getAngle()) * 1500,
    00333     robotPose.getPose().translation.y + sin(angles[angleShownByLEDs] + robotPose.getAngle()) * 1500,
    00334     30, Drawings::ps_solid, Drawings::light_gray
    00335     );
    00336
    00337   DEBUG_DRAWING_FINISHED(behavior_kickAngles);
    00338 }
    00339
    00340 #define ANGLEDRAWING(angleID, color) \
    00341   NDECLARE_DEBUGDRAWING("behavior:symbols:angles:"#angleID, "drawingOnField", "shows the " #angleID); \
    00342   NLINE("behavior:symbols:angles:"#angleID, \
    00343     robotPose.getPose().translation.x, \
    00344     robotPose.getPose().translation.y, \
    00345     robotPose.getPose().translation.x + cos(angles[angleID] + robotPose.getAngle()) * 1800, \
    00346     robotPose.getPose().translation.y + sin(angles[angleID] + robotPose.getAngle()) * 1800, \
    00347     60, Drawings::ps_solid, color);
    00348
    00349
    00350 void AngleSymbols::drawAngles()
    00351 {
    00352   ANGLEDRAWING(angleToCenterOfField, Drawings::red);
    00353   ANGLEDRAWING(angleToCenterOfOpponentPenaltyArea, Drawings::blue);
    00354   ANGLEDRAWING(angleToOpponentGoal, Drawings::yellow);
    00355   ANGLEDRAWING(bestAngleToOpponentGoal, Drawings::gray);
    00356   ANGLEDRAWING(perfectAngleToOpponentGoal, Drawings::yellowOrange);
    00357
    00358   ANGLEDRAWING(seenAngleToOpponentGoal, Drawings::skyblue);
    00359   ANGLEDRAWING(testAngle, Drawings::red);
    00360
    00361 //  ANGLEDRAWING(angleToFreePartOfOpponentGoalLeft, Drawings::black);
    00362 //  ANGLEDRAWING(angleToFreePartOfOpponentGoalRight, Drawings::black);
    00363
    00364   ANGLEDRAWING(angleToPointBehindOpponentGoal, Drawings::orange);
    00365 //  ANGLEDRAWING(bestWidthOfOpponentGoal, Drawings:);
    00366   ANGLEDRAWING(bestAngleToOpponentGoalNoObstacles, Drawings::skyblue);
    00367 //  ANGLEDRAWING(bestWidthOfOpponentGoalNoObstacles, Drawings:);
    00368   ANGLEDRAWING(bestAngleAwayFromOwnGoal, Drawings::pink);
    00369   ANGLEDRAWING(bestAngleAwayFromOwnGoalNoObstacles, Drawings::dark_gray);
    00370   ANGLEDRAWING(goalieGoalKickAngle, Drawings::black);
    00371
    00372   NDECLARE_DEBUGDRAWING("behavior:symbols:angles:_currentlyUsedAngle", "drawingOnField", "shows all angles that are currently used by the behavior");
    00373   DEBUG_RESPONSE("Behavior:output currently used angles", );
    00374   for(int currentAngle = 0; currentAngle < numberOfAngles; currentAngle++)
    00375   {
    00376     if(
    00377       angleIsUsedByTheCurrentBehavior[currentAngle] &&
    00378       (Angles)currentAngle != bestWidthOfOpponentGoal &&
    00379       (Angles)currentAngle != bestWidthOfOpponentGoalNoObstacles
    00380     )
    00381     {
    00382       NLINE("behavior:symbols:angles:_currentlyUsedAngle",
    00383         robotPose.getPose().translation.x,
    00384         robotPose.getPose().translation.y,
    00385         robotPose.getPose().translation.x + cos(angles[currentAngle] + robotPose.getAngle()) * 2000,
    00386         robotPose.getPose().translation.y + sin(angles[currentAngle] + robotPose.getAngle()) * 2000,
    00387         25, Drawings::ps_solid, Drawings::light_gray);
    00388       DEBUG_RESPONSE("Behavior:output currently used angles",
    00389         OUTPUT(idText, text, getAngleName((Angles)currentAngle) << angles[currentAngle] ); );
    00390     }
    00391     angleIsUsedByTheCurrentBehavior[currentAngle] = false;
    00392   }
    00393 }
    00394
    00395
    00396 void AngleSymbols::setTestAngle()
    00397 {
    00398   angles[testAngle] = angles[bestAngleToOpponentGoal];
    00399   DEBUG_RESPONSE("Behavior:testAngle=bestAngleToOpponentGoal", angles[testAngle] = angles[bestAngleToOpponentGoal];);
    00400   DEBUG_RESPONSE("Behavior:testAngle=bestAngleToOpponentGoalNoObstacles", angles[testAngle] = angles[bestAngleToOpponentGoalNoObstacles];);
    00401   DEBUG_RESPONSE("Behavior:testAngle=bestAngleAwayFromOwnGoal", angles[testAngle] = angles[bestAngleAwayFromOwnGoal];);
    00402   DEBUG_RESPONSE("Behavior:testAngle=seenAngleToOpponentGoal", angles[testAngle] = angles[seenAngleToOpponentGoal];);
    00403
    00404
    00405   int number = 0;
    00406   MODIFY("behavior:test angle enum", number);
    00407   DEBUG_RESPONSE("Behavior:testAngle==angle[testangleenum]", angles[testAngle] = angles[(Angles)number];);
    00408 //  DEBUG_RESPONSE("Behavior:testAngle=", angles[testAngle] = angles[]);
    00409 }